[Servo] Refactoring servo

moveit_ros/moveit_servo/include/moveit_servo/servo.hpp

@@ -118,6 +118,19 @@ class Servo
    */
   servo::Params& getParams();
 
+  /**
+   * \brief Get the current state of the robot as given by planning scene monitor.
+   * @return The current state of the robot.
+   */
+  KinematicState getCurrentRobotState();
+
+  /**
+   * \brief Smoothly halt at a commanded state when command goes stale.
+   * @param The last commanded joint states.
+   * @return The next state stepping towards the required halting state.
+   */
+  KinematicState smoothHalt(KinematicState halt_state);
+
 private:
   /**
    * \brief Convert a give twist command to planning frame

moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp

@@ -96,9 +96,9 @@ class ServoNode
   void twistCallback(const geometry_msgs::msg::TwistStamped::SharedPtr msg);
   void poseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg);
 
-  std::optional<KinematicState> processJointJogCommand();
-  std::optional<KinematicState> processTwistCommand();
-  std::optional<KinematicState> processPoseCommand();
+  KinematicState processJointJogCommand();
+  KinematicState processTwistCommand();
+  KinematicState processPoseCommand();
 
   // Variables
 
@@ -107,6 +107,7 @@ class ServoNode
   servo::Params servo_params_;
   planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
 
+  KinematicState last_commanded_state_;  // Used when commands go stale;
   control_msgs::msg::JointJog latest_joint_jog_;
   geometry_msgs::msg::TwistStamped latest_twist_;
   geometry_msgs::msg::PoseStamped latest_pose_;

moveit_ros/moveit_servo/src/servo.cpp

@@ -46,6 +46,7 @@ namespace
 {
 const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.servo");
 constexpr double ROBOT_STATE_WAIT_TIME = 5.0;  // seconds
+constexpr double STOPPED_VELOCITY_EPS = 1e-4;
 }  // namespace
 
 namespace moveit_servo
@@ -460,4 +461,42 @@ const PoseCommand Servo::toPlanningFrame(const PoseCommand& command)
   return PoseCommand{ servo_params_.planning_frame, tf2::transformToEigen(target_pose_msg) };
 }
 
+KinematicState Servo::getCurrentRobotState()
+{
+  moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
+  const moveit::core::JointModelGroup* joint_model_group =
+      robot_state->getJointModelGroup(servo_params_.move_group_name);
+  const auto joint_names = joint_model_group->getActiveJointModelNames();
+
+  KinematicState current_state(joint_names.size());
+  current_state.joint_names = joint_names;
+  robot_state->copyJointGroupPositions(joint_model_group, current_state.positions);
+  robot_state->copyJointGroupVelocities(joint_model_group, current_state.velocities);
+  robot_state->copyJointGroupAccelerations(joint_model_group, current_state.accelerations);
+
+  return current_state;
+}
+
+KinematicState Servo::smoothHalt(KinematicState halt_state)
+{
+  const auto current_state = getCurrentRobotState();
+
+  const size_t num_joints = current_state.joint_names.size();
+  for (size_t i = 0; i < num_joints; i++)
+  {
+    const double vel = (halt_state.positions[i] - current_state.positions[i]) / servo_params_.publish_period;
+    halt_state.velocities[i] = (vel > STOPPED_VELOCITY_EPS) ? vel : 0.0;
+    halt_state.accelerations[i] =
+        (halt_state.velocities[i] - current_state.velocities[i]) / servo_params_.publish_period;
+  }
+
+  if (smoother_)
+  {
+    smoother_->reset(current_state.positions);
+    smoother_->doSmoothing(halt_state.positions);
+  }
+
+  return halt_state;
+}
+
 }  // namespace moveit_servo

moveit_ros/moveit_servo/src/servo_node.cpp

@@ -185,37 +185,34 @@ void ServoNode::poseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr ms
   new_pose_msg_ = true;
 }
 
-std::optional<KinematicState> ServoNode::processJointJogCommand()
+KinematicState ServoNode::processJointJogCommand()
 {
-  std::optional<KinematicState> next_joint_state = std::nullopt;
-
-  // Mark latest jointjog command as processed.
+  KinematicState next_joint_state;
   // Reject any other command types that had arrived simultaneously.
-  new_joint_jog_msg_ = new_twist_msg_ = new_pose_msg_ = false;
+  new_twist_msg_ = new_pose_msg_ = false;
 
   const bool command_stale = (node_->now() - latest_joint_jog_.header.stamp) >=
                              rclcpp::Duration::from_seconds(servo_params_.incoming_command_timeout);
   if (!command_stale)
   {
-    // JointJogCommand is an alias for VectorXd, so we can directly make a map and pass it.
     JointJogCommand command{ latest_joint_jog_.joint_names, latest_joint_jog_.velocities };
     next_joint_state = servo_->getNextJointState(command);
   }
   else
   {
-    RCLCPP_WARN(LOGGER, "JointJog command is stale, will not process.");
+    next_joint_state = servo_->smoothHalt(last_commanded_state_);
   }
 
   return next_joint_state;
 }
 
-std::optional<KinematicState> ServoNode::processTwistCommand()
+KinematicState ServoNode::processTwistCommand()
 {
-  std::optional<KinematicState> next_joint_state = std::nullopt;
+  KinematicState next_joint_state;
 
   // Mark latest twist command as processed.
   // Reject any other command types that had arrived simultaneously.
-  new_joint_jog_msg_ = new_twist_msg_ = new_pose_msg_ = false;
+  new_joint_jog_msg_ = new_pose_msg_ = false;
 
   const bool command_stale = (node_->now() - latest_twist_.header.stamp) >=
                              rclcpp::Duration::from_seconds(servo_params_.incoming_command_timeout);
@@ -229,19 +226,19 @@ std::optional<KinematicState> ServoNode::processTwistCommand()
   }
   else
   {
-    RCLCPP_WARN(LOGGER, "Twist command is stale, will not process.");
+    next_joint_state = servo_->smoothHalt(last_commanded_state_);
   }
 
   return next_joint_state;
 }
 
-std::optional<KinematicState> ServoNode::processPoseCommand()
+KinematicState ServoNode::processPoseCommand()
 {
-  std::optional<KinematicState> next_joint_state = std::nullopt;
+  KinematicState next_joint_state;
 
   // Mark latest pose command as processed.
   // Reject any other command types that had arrived simultaneously.
-  new_joint_jog_msg_ = new_twist_msg_ = new_pose_msg_ = false;
+  new_joint_jog_msg_ = new_twist_msg_ = false;
 
   const bool command_stale = (node_->now() - latest_pose_.header.stamp) >=
                              rclcpp::Duration::from_seconds(servo_params_.incoming_command_timeout);
@@ -252,7 +249,7 @@ std::optional<KinematicState> ServoNode::processPoseCommand()
   }
   else
   {
-    RCLCPP_WARN(LOGGER, "Pose command is stale, will not process.");
+    next_joint_state = servo_->smoothHalt(last_commanded_state_);
   }
 
   return next_joint_state;
@@ -261,16 +258,23 @@ std::optional<KinematicState> ServoNode::processPoseCommand()
 void ServoNode::servoLoop()
 {
   moveit_msgs::msg::ServoStatus status_msg;
-  std::optional<KinematicState> next_joint_state;
+  KinematicState next_joint_state;
   rclcpp::WallRate servo_frequency(1 / servo_params_.publish_period);
 
+  // Initialize the kinematicState
+  {
+    auto state = servo_->getCurrentRobotState();
+    std::fill(state.velocities.begin(), state.velocities.end(), 0.0);
+    std::fill(state.accelerations.begin(), state.accelerations.end(), 0.0);
+    next_joint_state = state;
+  }
+
   while (rclcpp::ok() && !stop_servo_)
   {
     // Skip processing if servoing is disabled.
     if (servo_paused_)
       continue;
 
-    next_joint_state = std::nullopt;
     const CommandType expectedType = servo_->getCommandType();
 
     if (expectedType == CommandType::JOINT_JOG && new_joint_jog_msg_)
@@ -291,20 +295,21 @@ void ServoNode::servoLoop()
       RCLCPP_WARN_STREAM(LOGGER, "Command type has not been set, cannot accept input");
     }
 
-    if (next_joint_state && (servo_->getStatus() != StatusCode::INVALID))
+    if ((servo_->getStatus() != StatusCode::INVALID))
     {
       if (servo_params_.command_out_type == "trajectory_msgs/JointTrajectory")
-        trajectory_publisher_->publish(composeTrajectoryMessage(servo_->getParams(), next_joint_state.value()));
+        trajectory_publisher_->publish(composeTrajectoryMessage(servo_->getParams(), next_joint_state));
       else if (servo_params_.command_out_type == "std_msgs/Float64MultiArray")
-        multi_array_publisher_->publish(composeMultiArrayMessage(servo_->getParams(), next_joint_state.value()));
+        multi_array_publisher_->publish(composeMultiArrayMessage(servo_->getParams(), next_joint_state));
+      last_commanded_state_ = next_joint_state;
     }
 
     status_msg.code = static_cast<int8_t>(servo_->getStatus());
     status_msg.message = servo_->getStatusMessage();
     status_publisher_->publish(status_msg);
-  }
 
-  servo_frequency.sleep();
+    servo_frequency.sleep();
+  }
 }
 
 }  // namespace moveit_servo