1) fix spelling 2) add comments 3) make sure rolling_window always ha…

…s current state if no command generated 4) fix smooth hault: stop command was not generated if smoothing disabled

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp

@@ -96,7 +96,7 @@ int main(int argc, char* argv[])
   // create command queue to build trajectory message and add current robot state
   std::deque<KinematicState> joint_cmd_rolling_window;
   KinematicState current_state = servo.getCurrentRobotState();
-  updateSlidingWindow(current_state, joint_cmd_rolling_window, 0.0, demo_node->now());
+  updateSlidingWindow(current_state, joint_cmd_rolling_window, servo_params.max_expected_latency, demo_node->now());
 
   RCLCPP_INFO_STREAM(demo_node->get_logger(), servo.getStatusMessage());
   while (rclcpp::ok())

moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp

@@ -120,7 +120,7 @@ int main(int argc, char* argv[])
   // create command queue to build trajectory message and add current robot state
   std::deque<KinematicState> joint_cmd_rolling_window;
   KinematicState current_state = servo.getCurrentRobotState();
-  updateSlidingWindow(current_state, joint_cmd_rolling_window, 0.0, demo_node->now());
+  updateSlidingWindow(current_state, joint_cmd_rolling_window, servo_params.max_expected_latency, demo_node->now());
 
   bool satisfies_linear_tolerance = false;
   bool satisfies_angular_tolerance = false;

moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp

@@ -99,7 +99,7 @@ int main(int argc, char* argv[])
   // create command queue to build trajectory message and add current robot state
   std::deque<KinematicState> joint_cmd_rolling_window;
   KinematicState current_state = servo.getCurrentRobotState();
-  updateSlidingWindow(current_state, joint_cmd_rolling_window, 0.0, demo_node->now());
+  updateSlidingWindow(current_state, joint_cmd_rolling_window, servo_params.max_expected_latency, demo_node->now());
 
   RCLCPP_INFO_STREAM(demo_node->get_logger(), servo.getStatusMessage());
   while (rclcpp::ok())

moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp

@@ -54,6 +54,9 @@
 
 namespace moveit_servo
 {
+// A minimum of 3 points are used to help with interpolation when creating trajectory messages.
+constexpr int MIN_POINTS_FOR_TRAJ_MSG = 3;
+
 /**
  * \brief Get the base frame of the current active joint group or subgroup's IK solver.
  * @param robot_state A pointer to the current robot state.
@@ -121,7 +124,7 @@ composeTrajectoryMessage(const servo::Params& servo_params, const std::deque<Kin
 
 /**
  * \brief Adds a new joint state command to a queue containing commands over a time window. Also modifies the velocities
- * of the commands to avoid overshooting.
+ * of the commands to help avoid overshooting.
  * @param next_joint_state The next commanded joint state.
  * @param joint_cmd_rolling_window Queue of containing a rolling window of joint commands.
  * @param max_expected_latency The next_joint_state will be added to the joint_cmd_rolling_window with a time stamp of

moveit_ros/moveit_servo/src/servo.cpp

@@ -209,7 +209,7 @@ bool Servo::validateParams(const servo::Params& servo_params) const
   {
     RCLCPP_ERROR_STREAM(logger_, "The parameter 'hard_stop_singularity_threshold' "
                                  "should be greater than the parameter 'lower_singularity_threshold'. But the "
-                                 "'hard_stop_signularity_threshold' is: '"
+                                 "'hard_stop_singularity_threshold' is: '"
                                      << servo_params.hard_stop_singularity_threshold
                                      << "' and the 'lower_singularity_threshold' is: '"
                                      << servo_params.lower_singularity_threshold << "'" << check_yaml_string);
@@ -284,12 +284,12 @@ bool Servo::validateParams(const servo::Params& servo_params) const
     params_valid = false;
   }
 
-  if (servo_params.max_expected_latency / 3 < servo_params.publish_period)
+  if (servo_params.max_expected_latency / MIN_POINTS_FOR_TRAJ_MSG < servo_params.publish_period)
   {
     RCLCPP_ERROR(
         logger_,
-        "The publish period (%f sec) parameter must be less than 1/3 of the max expected latency parameter (%f sec).",
-        servo_params.publish_period, servo_params.max_expected_latency);
+        "The publish period (%f sec) parameter must be less than 1/%d of the max expected latency parameter (%f sec).",
+        servo_params.publish_period, MIN_POINTS_FOR_TRAJ_MSG, servo_params.max_expected_latency);
     params_valid = false;
   }
 
@@ -662,29 +662,31 @@ KinematicState Servo::getCurrentRobotState() const
 
 std::pair<bool, KinematicState> Servo::smoothHalt(const KinematicState& halt_state)
 {
-  bool stopped = false;
   auto target_state = halt_state;
 
-  // set target velocity
-  target_state.velocities *= 0.0;
+  // If all velocities are near zero, robot has decelerated to a stop.
+  bool stopped = (target_state.velocities.cwiseAbs().array() < STOPPED_VELOCITY_EPS).all();
 
-  // apply smoothing: this will change target position/velocity to make slow down gradual
-  doSmoothing(target_state);
+  if (!stopped)
+  {
+    // set target velocity
+    target_state.velocities *= 0.0;
 
-  // scale velocity in case of obstacle
-  target_state.velocities *= collision_velocity_scale_;
+    // apply smoothing: this will change target position/velocity to make slow down gradual
+    doSmoothing(target_state);
 
-  for (size_t i = 0; i < (size_t)halt_state.positions.size(); ++i)
-  {
-    target_state.positions[i] = halt_state.positions[i] + target_state.velocities[i] * servo_params_.publish_period;
-    const double vel = target_state.velocities[i];
-    target_state.velocities[i] = (std::abs(vel) > STOPPED_VELOCITY_EPS) ? vel : 0.0;
-    target_state.accelerations[i] =
-        (target_state.velocities[i] - halt_state.velocities[i]) / servo_params_.publish_period;
-  }
+    // scale velocity in case of obstacle
+    target_state.velocities *= collision_velocity_scale_;
 
-  // If all velocities are near zero, robot has decelerated to a stop.
-  stopped = (target_state.velocities.cwiseAbs().array() < STOPPED_VELOCITY_EPS).all();
+    for (long i = 0; i < halt_state.positions.size(); ++i)
+    {
+      target_state.positions[i] = halt_state.positions[i] + target_state.velocities[i] * servo_params_.publish_period;
+      const double vel = target_state.velocities[i];
+      target_state.velocities[i] = (std::abs(vel) > STOPPED_VELOCITY_EPS) ? vel : 0.0;
+      target_state.accelerations[i] =
+          (target_state.velocities[i] - halt_state.velocities[i]) / servo_params_.publish_period;
+    }
+  }
 
   return std::make_pair(stopped, target_state);
 }

moveit_ros/moveit_servo/src/servo_node.cpp

@@ -313,6 +313,11 @@ void ServoNode::servoLoop()
   KinematicState current_state = servo_->getCurrentRobotState();
   last_commanded_state_ = current_state;
 
+  // Get the robot state and joint model group info.
+  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);
+
   while (rclcpp::ok() && !stop_servo_)
   {
     // Skip processing if servoing is disabled.
@@ -331,17 +336,12 @@ void ServoNode::servoLoop()
     }
     else
     {
+      // if all joint_cmd_rolling_window_ is empty or all commands in it are outdated, use current robot state
+      joint_cmd_rolling_window_.clear();
       current_state = servo_->getCurrentRobotState();
       current_state.velocities *= 0.0;
-      joint_cmd_rolling_window_.clear();
-      updateSlidingWindow(current_state, joint_cmd_rolling_window_, 0.0, cur_time);
     }
 
-    // Get the robot state and joint model group info.
-    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);
-
     // update robot state values
     robot_state->setJointGroupPositions(joint_model_group, current_state.positions);
     robot_state->setJointGroupVelocities(joint_model_group, current_state.velocities);
@@ -386,6 +386,11 @@ void ServoNode::servoLoop()
       }
       last_commanded_state_ = next_joint_state.value();
     }
+    else
+    {
+      // if no new command was created, use current robot state
+      updateSlidingWindow(current_state, joint_cmd_rolling_window_, servo_params_.max_expected_latency, cur_time);
+    }
 
     status_msg.code = static_cast<int8_t>(servo_->getStatus());
     status_msg.message = servo_->getStatusMessage();

moveit_ros/moveit_servo/src/utils/common.cpp

@@ -45,9 +45,6 @@ namespace
 constexpr double SCALING_OVERRIDE_THRESHOLD = 0.01;
 // The angle threshold in radians below which a rotation will be considered the identity.
 constexpr double MIN_ANGLE_THRESHOLD = 1E-16;
-// A minimum of 3 points are used to help with interpolation when creating trajectory messages.
-constexpr int MIN_POINTS_FOR_TRAJ_MSG = 3;
-
 }  // namespace
 
 namespace moveit_servo