Cleanup

moveit_core/online_signal_smoothing/CMakeLists.txt

@@ -7,7 +7,10 @@ target_include_directories(moveit_smoothing_base PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>
   $<INSTALL_INTERFACE:include/moveit_core>
 )
-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

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,

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,

moveit_ros/moveit_servo/CMakeLists.txt

@@ -18,21 +18,18 @@ 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)
 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)

moveit_ros/moveit_servo/package.xml

@@ -26,24 +26,26 @@
   <depend>control_msgs</depend>
   <depend>generate_parameter_library</depend>
   <depend>geometry_msgs</depend>
-  <depend>moveit_msgs</depend>
   <depend>moveit_core</depend>
+  <depend>moveit_msgs</depend>
   <depend>moveit_ros_planning_interface</depend>
   <depend>pluginlib</depend>
   <depend>realtime_tools</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
   <depend>std_srvs</depend>
+  <depend>tf2_eigen</depend>
   <depend>trajectory_msgs</depend>
 
   <exec_depend>controller_manager</exec_depend>
   <exec_depend>gripper_controllers</exec_depend>
   <exec_depend>joint_state_broadcaster</exec_depend>
   <exec_depend>joint_trajectory_controller</exec_depend>
   <exec_depend>joy</exec_depend>
-  <exec_depend>robot_state_publisher</exec_depend>
-  <exec_depend>moveit_configs_utils</exec_depend>
   <exec_depend>launch_param_builder</exec_depend>
+  <exec_depend>moveit_configs_utils</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>

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<bool, KinematicState> 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++)

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];
     }