From ab62aa4ef25e809bc9241feaece58bbaa7fd6435 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 24 Oct 2023 08:11:46 -0500 Subject: [PATCH] Cleanup --- moveit_core/online_signal_smoothing/CMakeLists.txt | 5 ++++- .../online_signal_smoothing/butterworth_filter.h | 8 ++++++-- .../online_signal_smoothing/smoothing_base_class.h | 8 ++++++-- moveit_ros/moveit_servo/CMakeLists.txt | 7 ++----- moveit_ros/moveit_servo/package.xml | 8 +++++--- moveit_ros/moveit_servo/src/servo.cpp | 4 ++-- moveit_ros/moveit_servo/src/utils/common.cpp | 10 +++++----- 7 files changed, 30 insertions(+), 20 deletions(-) diff --git a/moveit_core/online_signal_smoothing/CMakeLists.txt b/moveit_core/online_signal_smoothing/CMakeLists.txt index 9741899a80f..dc79a7a843b 100644 --- a/moveit_core/online_signal_smoothing/CMakeLists.txt +++ b/moveit_core/online_signal_smoothing/CMakeLists.txt @@ -7,7 +7,10 @@ target_include_directories(moveit_smoothing_base PUBLIC $ $ ) -target_link_libraries(moveit_smoothing_base moveit_macros) +target_link_libraries(moveit_smoothing_base + ${Eigen_LIBRARIES} + moveit_macros +) include(GenerateExportHeader) generate_export_header(moveit_smoothing_base) set_target_properties(moveit_smoothing_base PROPERTIES VERSION diff --git a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h index 65a596c2e8d..bdcf6a7e5ba 100644 --- a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h +++ b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h @@ -104,14 +104,18 @@ class ButterworthFilterPlugin : public SmoothingBaseClass /** * Smooth the command signals for all DOF - * @param position_vector array of joint position commands + * @param positions array of joint position commands + * @param velocities array of joint velocity commands + * @param accelerations array of joint acceleration commands * @return True if initialization was successful */ bool doSmoothing(Eigen::VectorXd& positions, Eigen::VectorXd& velocities, Eigen::VectorXd& accelerations) override; /** * Reset to a given joint state - * @param joint_positions reset the filters to these joint positions + * @param positions reset the filters to these joint positions + * @param velocities (unused) + * @param accelerations (unused) * @return True if reset was successful */ bool reset(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, diff --git a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/smoothing_base_class.h b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/smoothing_base_class.h index f1a1bd9d7ec..e870e493f0f 100644 --- a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/smoothing_base_class.h +++ b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/smoothing_base_class.h @@ -73,14 +73,18 @@ class SmoothingBaseClass /** * Smooth an array of joint position deltas - * @param position_vector array of joint position commands + * @param positiona array of joint position commands + * @param velocities array of joint velocity commands + * @param accelerations array of joint acceleration commands * @return True if initialization was successful */ virtual bool doSmoothing(Eigen::VectorXd& positions, Eigen::VectorXd& velocities, Eigen::VectorXd& accelerations) = 0; /** * Reset to a given joint state - * @param joint_positions reset the filters to these joint positions + * @param positions reset the filters to these joint positions + * @param velocities reset the filters to these joint velocities (if applicable) + * @param accelerations reset the filters to these joint accelerations (if applicable) * @return True if reset was successful */ virtual bool reset(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt index 6adca0a66d4..e95461540e6 100644 --- a/moveit_ros/moveit_servo/CMakeLists.txt +++ b/moveit_ros/moveit_servo/CMakeLists.txt @@ -18,12 +18,11 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS sensor_msgs std_msgs std_srvs + tf2_eigen trajectory_msgs ) find_package(ament_cmake REQUIRED) -find_package(eigen3_cmake_module REQUIRED) -find_package(Eigen3 REQUIRED) find_package(generate_parameter_library REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) @@ -31,8 +30,6 @@ endforeach() include_directories( include -SYSTEM - ${EIGEN3_INCLUDE_DIRS} ) ################### @@ -54,7 +51,7 @@ add_library(moveit_servo_lib_cpp SHARED ) set_target_properties(moveit_servo_lib_cpp PROPERTIES VERSION "${moveit_servo_VERSION}") -target_link_libraries(moveit_servo_lib_cpp moveit_servo_lib_parameters ${Eigen_LIBRARIES}) +target_link_libraries(moveit_servo_lib_cpp moveit_servo_lib_parameters) ament_target_dependencies(moveit_servo_lib_cpp ${THIS_PACKAGE_INCLUDE_DEPENDS}) add_library(moveit_servo_lib_ros SHARED src/servo_node.cpp) diff --git a/moveit_ros/moveit_servo/package.xml b/moveit_ros/moveit_servo/package.xml index 8e31d8b7321..2a96034fbab 100644 --- a/moveit_ros/moveit_servo/package.xml +++ b/moveit_ros/moveit_servo/package.xml @@ -26,14 +26,15 @@ control_msgs generate_parameter_library geometry_msgs - moveit_msgs moveit_core + moveit_msgs moveit_ros_planning_interface pluginlib realtime_tools sensor_msgs std_msgs std_srvs + tf2_eigen trajectory_msgs controller_manager @@ -41,9 +42,10 @@ joint_state_broadcaster joint_trajectory_controller joy - robot_state_publisher - moveit_configs_utils launch_param_builder + moveit_configs_utils + robot_state_publisher + tf2_ros ament_cmake_gtest ament_lint_auto diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index bffa6adbf5f..d32c3d427bb 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -180,7 +180,7 @@ void Servo::setSmoothingPlugin() RCLCPP_ERROR(LOGGER, "Smoothing plugin could not be initialized"); std::exit(EXIT_FAILURE); } - KinematicState current_state = getCurrentRobotState(); + const KinematicState current_state = getCurrentRobotState(); smoother_->reset(current_state.positions, current_state.velocities, current_state.accelerations); } @@ -589,7 +589,7 @@ std::pair Servo::smoothHalt(const KinematicState& halt_sta { bool stopped = false; auto target_state = halt_state; - KinematicState current_state = getCurrentRobotState(); + const KinematicState current_state = getCurrentRobotState(); const size_t num_joints = current_state.joint_names.size(); for (size_t i = 0; i < num_joints; i++) diff --git a/moveit_ros/moveit_servo/src/utils/common.cpp b/moveit_ros/moveit_servo/src/utils/common.cpp index d60e7a45450..3936199653c 100644 --- a/moveit_ros/moveit_servo/src/utils/common.cpp +++ b/moveit_ros/moveit_servo/src/utils/common.cpp @@ -135,21 +135,21 @@ trajectory_msgs::msg::JointTrajectory composeTrajectoryMessage(const servo::Para if (servo_params.publish_joint_positions) { - for (size_t i = 0; i < num_joints; i++) + for (size_t i = 0; i < num_joints; ++i) { point.positions[i] = joint_state.positions[i]; } } if (servo_params.publish_joint_velocities) { - for (size_t i = 0; i < num_joints; i++) + for (size_t i = 0; i < num_joints; ++i) { point.velocities[i] = joint_state.velocities[i]; } } if (servo_params.publish_joint_accelerations) { - for (size_t i = 0; i < num_joints; i++) + for (size_t i = 0; i < num_joints; ++i) { point.accelerations[i] = joint_state.accelerations[i]; } @@ -167,14 +167,14 @@ std_msgs::msg::Float64MultiArray composeMultiArrayMessage(const servo::Params& s multi_array.data.resize(num_joints); if (servo_params.publish_joint_positions) { - for (size_t i = 0; i < num_joints; i++) + for (size_t i = 0; i < num_joints; ++i) { multi_array.data[i] = joint_state.positions[i]; } } else if (servo_params.publish_joint_velocities) { - for (size_t i = 0; i < num_joints; i++) + for (size_t i = 0; i < num_joints; ++i) { multi_array.data[i] = joint_state.velocities[i]; }