Cleanup

Author: AndyZe

moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/ruckig_filter.h

@@ -87,14 +87,15 @@ class RuckigFilterPlugin : public SmoothingBaseClass
    */
   void printRuckigState();
 
-  rclcpp::Node::SharedPtr node_;
-  void getDefaultJointVelAccelBounds();
+  /**
+   * A utility to get velocity/acceleration/jerk bounds from the robot model
+   */
+  void getVelAccelJerkBounds();
 
+  rclcpp::Node::SharedPtr node_;
   online_signal_smoothing::Params params_;
-
   size_t num_joints_;
   moveit::core::RobotModelConstPtr robot_model_;
-
   bool have_initial_ruckig_output_;
   std::optional<ruckig::Ruckig<ruckig::DynamicDOFs>> ruckig_;
   std::optional<ruckig::InputParameter<ruckig::DynamicDOFs>> ruckig_input_;

moveit_core/online_signal_smoothing/src/ruckig_filter.cpp

@@ -63,7 +63,7 @@ bool RuckigFilterPlugin::initialize(rclcpp::Node::SharedPtr node, moveit::core::
   params_ = param_listener.get_params();
 
   // Ruckig needs the joint vel/accel/jerk bounds.
-  getDefaultJointVelAccelBounds();
+  getVelAccelJerkBounds();
   ruckig::InputParameter<ruckig::DynamicDOFs> ruckig_input(num_joints_);
   ruckig_input.max_velocity = joint_velocity_bounds_;
   ruckig_input.max_acceleration = joint_acceleration_bounds_;
@@ -136,7 +136,7 @@ bool RuckigFilterPlugin::reset(const Eigen::VectorXd& positions, const Eigen::Ve
   return true;
 }
 
-void RuckigFilterPlugin::getDefaultJointVelAccelBounds()
+void RuckigFilterPlugin::getVelAccelJerkBounds()
 {
   auto joint_model_group = robot_model_->getJointModelGroup(params_.planning_group_name);
   for (const auto& joint : joint_model_group->getActiveJointModels())
@@ -150,7 +150,7 @@ void RuckigFilterPlugin::getDefaultJointVelAccelBounds()
     }
     else
     {
-      RCLCPP_ERROR(getLogger(), "No joint vel limit defined. Exiting for safety.");
+      RCLCPP_ERROR(getLogger(), "No joint velocity limit defined. Exiting for safety.");
       std::exit(EXIT_FAILURE);
     }
 
@@ -161,8 +161,8 @@ void RuckigFilterPlugin::getDefaultJointVelAccelBounds()
     }
     else
     {
-      RCLCPP_ERROR(getLogger(), "WARNING: No joint accel limit defined. Very large accelerations will be "
-                                "possible.");
+      RCLCPP_WARN(getLogger(), "No joint acceleration limit defined. Very large accelerations will be "
+                               "possible.");
       joint_acceleration_bounds_.push_back(DBL_MAX);
     }
 
@@ -184,9 +184,9 @@ void RuckigFilterPlugin::getDefaultJointVelAccelBounds()
 
 void RuckigFilterPlugin::printRuckigState()
 {
-  RCLCPP_ERROR_STREAM(getLogger(), ruckig_->delta_time << "\nRuckig input:\n"
-                                                       << ruckig_input_->to_string() << "\nRuckig output:\n"
-                                                       << ruckig_output_->to_string());
+  RCLCPP_INFO_STREAM(getLogger(), ruckig_->delta_time << "\nRuckig input:\n"
+                                                      << ruckig_input_->to_string() << "\nRuckig output:\n"
+                                                      << ruckig_output_->to_string());
 }
 }  // namespace online_signal_smoothing
 

moveit_ros/moveit_servo/src/servo_node.cpp

@@ -383,7 +383,6 @@ void ServoNode::servoLoop()
     {
       // if no new command was created, use current robot state
       updateSlidingWindow(current_state, joint_cmd_rolling_window_, servo_params_.max_expected_latency, cur_time);
-      // servo_->resetSmoothing(current_state);
     }
 
     status_msg.code = static_cast<int8_t>(servo_->getStatus());