Fix Servo JointJog Crash

moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp

@@ -47,6 +47,10 @@
 #include <rclcpp/rclcpp.hpp>
 #include <tf2_eigen/tf2_eigen.hpp>
 
+#define COMMAND_ERROR(msg_stream)                                                                                      \
+  RCLCPP_WARN_STREAM(getLogger(), msg_stream);                                                                         \
+  return std::make_pair(StatusCode::INVALID, joint_position_delta)
+
 namespace moveit_servo
 {
 

moveit_ros/moveit_servo/src/servo_node.cpp

@@ -218,12 +218,21 @@ std::optional<KinematicState> ServoNode::processJointJogCommand(const moveit::co
   // Reject any other command types that had arrived simultaneously.
   new_twist_msg_ = new_pose_msg_ = false;
 
+  if (!latest_joint_jog_.displacements.empty())
+  {
+    RCLCPP_WARN(node_->get_logger(), "Joint jog command displacements field is not yet supported, ignoring.");
+    latest_joint_jog_.displacements.clear();  // Only warn once per message.
+  }
+
   const bool command_stale = (node_->now() - latest_joint_jog_.header.stamp) >=
                              rclcpp::Duration::from_seconds(servo_params_.incoming_command_timeout);
   if (!command_stale)
   {
     JointJogCommand command{ latest_joint_jog_.joint_names, latest_joint_jog_.velocities };
     next_joint_state = servo_->getNextJointState(robot_state, command);
+    // If the command failed, stop trying to process this message
+    if (servo_->getStatus() == StatusCode::INVALID)
+      new_joint_jog_msg_ = false;
   }
   else
   {

moveit_ros/moveit_servo/src/utils/command.cpp

@@ -84,7 +84,6 @@ JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const mo
                                         const JointNameToMoveGroupIndexMap& joint_name_group_index_map)
 {
   // Find the target joint position based on the commanded joint velocity
-  StatusCode status = StatusCode::NO_WARNING;
   const auto& group_name =
       servo_params.active_subgroup.empty() ? servo_params.move_group_name : servo_params.active_subgroup;
   const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(group_name);
@@ -93,7 +92,12 @@ JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const mo
   Eigen::VectorXd velocities(joint_names.size());
 
   velocities.setZero();
-  bool names_valid = true;
+  if (command.velocities.size() != command.names.size())
+  {
+    COMMAND_ERROR("Invalid joint jog command. Each joint name must have one corresponding "
+                  "velocity command. Received "
+                  << command.names.size() << " joints with " << command.velocities.size() << " commands.");
+  }
 
   for (size_t i = 0; i < command.names.size(); ++i)
   {
@@ -104,44 +108,32 @@ JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const mo
     }
     else
     {
-      RCLCPP_WARN_STREAM(getLogger(), "Invalid joint name: " << command.names[i]);
-
-      names_valid = false;
-      break;
+      COMMAND_ERROR("Invalid joint name: "
+                    << command.names[i]
+                    << "Either you're sending commands for a joint "
+                       "that is not part of the move group or certain joints cannot be moved because a "
+                       "subgroup is active and they are not part of it.");
     }
   }
-  const bool velocity_valid = isValidCommand(velocities);
-  if (names_valid && velocity_valid)
+
+  if (!isValidCommand(velocities))
   {
-    joint_position_delta = velocities * servo_params.publish_period;
-    if (servo_params.command_in_type == "unitless")
-    {
-      joint_position_delta *= servo_params.scale.joint;
-    }
+    COMMAND_ERROR("Invalid velocity values in joint jog command");
   }
-  else
+
+  joint_position_delta = velocities * servo_params.publish_period;
+  if (servo_params.command_in_type == "unitless")
   {
-    status = StatusCode::INVALID;
-    if (!names_valid)
-    {
-      RCLCPP_WARN_STREAM(getLogger(),
-                         "Invalid joint names in joint jog command. Either you're sending commands for a joint "
-                         "that is not part of the move group or certain joints cannot be moved because a "
-                         "subgroup is active and they are not part of it.");
-    }
-    if (!velocity_valid)
-    {
-      RCLCPP_WARN_STREAM(getLogger(), "Invalid velocity values in joint jog command");
-    }
+    joint_position_delta *= servo_params.scale.joint;
   }
 
   if (!servo_params.active_subgroup.empty() && servo_params.active_subgroup != servo_params.move_group_name)
   {
-    return std::make_pair(status, createMoveGroupDelta(joint_position_delta, robot_state, servo_params,
-                                                       joint_name_group_index_map));
+    return std::make_pair(StatusCode::NO_WARNING, createMoveGroupDelta(joint_position_delta, robot_state, servo_params,
+                                                                       joint_name_group_index_map));
   }
 
-  return std::make_pair(status, joint_position_delta);
+  return std::make_pair(StatusCode::NO_WARNING, joint_position_delta);
 }
 
 JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit::core::RobotStatePtr& robot_state,
@@ -154,10 +146,21 @@ JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit::
   Eigen::VectorXd joint_position_delta(num_joints);
   Eigen::Vector<double, 6> cartesian_position_delta;
 
-  const bool valid_command = isValidCommand(command);
-  const bool is_planning_frame = (command.frame_id == planning_frame);
-  const bool is_zero = command.velocities.isZero();
-  if (!is_zero && is_planning_frame && valid_command)
+  if (command.frame_id != planning_frame)
+  {
+    COMMAND_ERROR("Command frame is: " << command.frame_id << ", expected: " << planning_frame);
+  }
+
+  if (!isValidCommand(command))
+  {
+    COMMAND_ERROR("Invalid twist command.");
+  }
+
+  if (command.velocities.isZero())
+  {
+    joint_position_delta.setZero();
+  }
+  else
   {
     // Compute the Cartesian position delta based on incoming twist command.
     cartesian_position_delta = command.velocities * servo_params.publish_period;
@@ -208,22 +211,7 @@ JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit::
       }
     }
   }
-  else if (is_zero)
-  {
-    joint_position_delta.setZero();
-  }
-  else
-  {
-    status = StatusCode::INVALID;
-    if (!valid_command)
-    {
-      RCLCPP_ERROR_STREAM(getLogger(), "Invalid twist command.");
-    }
-    if (!is_planning_frame)
-    {
-      RCLCPP_ERROR_STREAM(getLogger(), "Command frame is: " << command.frame_id << ", expected: " << planning_frame);
-    }
-  }
+
   return std::make_pair(status, joint_position_delta);
 }
 
@@ -237,74 +225,66 @@ JointDeltaResult jointDeltaFromPose(const PoseCommand& command, const moveit::co
       robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveJointModelNames().size();
   Eigen::VectorXd joint_position_delta(num_joints);
 
-  const bool valid_command = isValidCommand(command);
-  const bool is_planning_frame = (command.frame_id == planning_frame);
-
-  if (valid_command && is_planning_frame)
+  if (!isValidCommand(command))
   {
-    Eigen::Vector<double, 6> cartesian_position_delta;
+    COMMAND_ERROR("Invalid pose command.");
+  }
 
-    // Compute linear and angular change needed.
-    const Eigen::Isometry3d ee_pose{ robot_state->getGlobalLinkTransform(planning_frame).inverse() *
-                                     robot_state->getGlobalLinkTransform(ee_frame) };
-    const Eigen::Quaterniond q_current(ee_pose.rotation());
-    Eigen::Quaterniond q_target(command.pose.rotation());
-    Eigen::Vector3d translation_error = command.pose.translation() - ee_pose.translation();
+  if (command.frame_id != planning_frame)
+  {
+    COMMAND_ERROR("Command frame is: " << command.frame_id << " expected: " << planning_frame);
+  }
 
-    // Limit the commands by the maximum linear and angular speeds provided.
-    if (servo_params.scale.linear > 0.0)
-    {
-      const auto linear_speed_scale =
-          (translation_error.norm() / servo_params.publish_period) / servo_params.scale.linear;
-      if (linear_speed_scale > 1.0)
-      {
-        translation_error /= linear_speed_scale;
-      }
-    }
-    if (servo_params.scale.rotational > 0.0)
-    {
-      const auto angular_speed_scale =
-          (std::abs(q_target.angularDistance(q_current)) / servo_params.publish_period) / servo_params.scale.rotational;
-      if (angular_speed_scale > 1.0)
-      {
-        q_target = q_current.slerp(1.0 / angular_speed_scale, q_target);
-      }
-    }
+  Eigen::Vector<double, 6> cartesian_position_delta;
 
-    // Compute the Cartesian deltas from the velocity-scaled values.
-    const auto angle_axis_error = Eigen::AngleAxisd(q_target * q_current.inverse());
-    cartesian_position_delta.head<3>() = translation_error;
-    cartesian_position_delta.tail<3>() = angle_axis_error.axis() * angle_axis_error.angle();
+  // Compute linear and angular change needed.
+  const Eigen::Isometry3d ee_pose{ robot_state->getGlobalLinkTransform(planning_frame).inverse() *
+                                   robot_state->getGlobalLinkTransform(ee_frame) };
+  const Eigen::Quaterniond q_current(ee_pose.rotation());
+  Eigen::Quaterniond q_target(command.pose.rotation());
+  Eigen::Vector3d translation_error = command.pose.translation() - ee_pose.translation();
 
-    // Compute the required change in joint angles.
-    const auto delta_result =
-        jointDeltaFromIK(cartesian_position_delta, robot_state, servo_params, joint_name_group_index_map);
-    status = delta_result.first;
-    if (status != StatusCode::INVALID)
+  // Limit the commands by the maximum linear and angular speeds provided.
+  if (servo_params.scale.linear > 0.0)
+  {
+    const auto linear_speed_scale =
+        (translation_error.norm() / servo_params.publish_period) / servo_params.scale.linear;
+    if (linear_speed_scale > 1.0)
     {
-      joint_position_delta = delta_result.second;
-      // Get velocity scaling information for singularity.
-      const auto singularity_scaling_info =
-          velocityScalingFactorForSingularity(robot_state, cartesian_position_delta, servo_params);
-      // Apply velocity scaling for singularity, if there was any scaling.
-      if (singularity_scaling_info.second != StatusCode::NO_WARNING)
-      {
-        status = singularity_scaling_info.second;
-        RCLCPP_WARN_STREAM(getLogger(), SERVO_STATUS_CODE_MAP.at(status));
-        joint_position_delta *= singularity_scaling_info.first;
-      }
+      translation_error /= linear_speed_scale;
     }
   }
-  else
+  if (servo_params.scale.rotational > 0.0)
   {
-    status = StatusCode::INVALID;
-    if (!valid_command)
+    const auto angular_speed_scale =
+        (std::abs(q_target.angularDistance(q_current)) / servo_params.publish_period) / servo_params.scale.rotational;
+    if (angular_speed_scale > 1.0)
     {
-      RCLCPP_WARN_STREAM(getLogger(), "Invalid pose command.");
+      q_target = q_current.slerp(1.0 / angular_speed_scale, q_target);
     }
-    if (!is_planning_frame)
+  }
+
+  // Compute the Cartesian deltas from the velocity-scaled values.
+  const auto angle_axis_error = Eigen::AngleAxisd(q_target * q_current.inverse());
+  cartesian_position_delta.head<3>() = translation_error;
+  cartesian_position_delta.tail<3>() = angle_axis_error.axis() * angle_axis_error.angle();
+
+  // Compute the required change in joint angles.
+  const auto delta_result =
+      jointDeltaFromIK(cartesian_position_delta, robot_state, servo_params, joint_name_group_index_map);
+  status = delta_result.first;
+  if (status != StatusCode::INVALID)
+  {
+    joint_position_delta = delta_result.second;
+    // Get velocity scaling information for singularity.
+    const auto singularity_scaling_info =
+        velocityScalingFactorForSingularity(robot_state, cartesian_position_delta, servo_params);
+    // Apply velocity scaling for singularity, if there was any scaling.
+    if (singularity_scaling_info.second != StatusCode::NO_WARNING)
     {
-      RCLCPP_WARN_STREAM(getLogger(), "Command frame is: " << command.frame_id << " expected: " << planning_frame);
+      status = singularity_scaling_info.second;
+      RCLCPP_WARN_STREAM(getLogger(), SERVO_STATUS_CODE_MAP.at(status));
+      joint_position_delta *= singularity_scaling_info.first;
     }
   }
   return std::make_pair(status, joint_position_delta);

moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp

@@ -49,6 +49,7 @@
 #include <rclcpp/node.hpp>
 #include <rclcpp/publisher.hpp>
 #include <rclcpp/qos.hpp>
+#include <rcl_interfaces/msg/log.hpp>
 #include <sensor_msgs/msg/joint_state.hpp>
 #include <trajectory_msgs/msg/joint_trajectory.hpp>
 
@@ -73,6 +74,11 @@ class ServoRosFixture : public testing::Test
     traj_count_++;
   }
 
+  void logCallback(const rcl_interfaces::msg::Log::ConstSharedPtr& msg)
+  {
+    log_.push_back(*msg);
+  }
+
   void SetUp() override
   {
     // Create a node to be given to Servo.
@@ -92,6 +98,10 @@ class ServoRosFixture : public testing::Test
         "/panda_arm_controller/joint_trajectory", rclcpp::SystemDefaultsQoS(),
         [this](const trajectory_msgs::msg::JointTrajectory::ConstSharedPtr& msg) { return trajectoryCallback(msg); });
 
+    log_subscriber_ = servo_test_node_->create_subscription<rcl_interfaces::msg::Log>(
+        "/rosout", rclcpp::SystemDefaultsQoS(),
+        [this](const rcl_interfaces::msg::Log::ConstSharedPtr& msg) { return logCallback(msg); });
+
     switch_input_client_ =
         servo_test_node_->create_client<moveit_msgs::srv::ServoCommandType>("/servo_node/switch_command_type");
 
@@ -145,6 +155,20 @@ class ServoRosFixture : public testing::Test
     }
   }
 
+  bool logContains(const std::string& logger_name, const std::string& msg)
+  {
+    bool found = false;
+    for (auto line : log_)
+    {
+      if (line.name == logger_name && line.msg.find(msg) != std::string::npos)
+      {
+        found = true;
+        break;
+      }
+    }
+    return found;
+  }
+
   ServoRosFixture() : state_count_{ 0 }, traj_count_{ 0 }
   {
   }
@@ -158,6 +182,7 @@ class ServoRosFixture : public testing::Test
   rclcpp::Subscription<moveit_msgs::msg::ServoStatus>::SharedPtr status_subscriber_;
   rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_subscriber_;
   rclcpp::Subscription<trajectory_msgs::msg::JointTrajectory>::SharedPtr trajectory_subscriber_;
+  rclcpp::Subscription<rcl_interfaces::msg::Log>::SharedPtr log_subscriber_;
   rclcpp::Client<moveit_msgs::srv::ServoCommandType>::SharedPtr switch_input_client_;
 
   sensor_msgs::msg::JointState joint_states_;
@@ -168,4 +193,5 @@ class ServoRosFixture : public testing::Test
 
   std::atomic<int> state_count_;
   std::atomic<int> traj_count_;
+  std::vector<rcl_interfaces::msg::Log> log_;
 };

moveit_ros/moveit_servo/tests/test_ros_integration.cpp

@@ -106,6 +106,43 @@ TEST_F(ServoRosFixture, testJointJog)
   moveit_servo::StatusCode status = status_;
   RCLCPP_INFO_STREAM(servo_test_node_->get_logger(), "Status after jointjog: " << static_cast<size_t>(status));
   ASSERT_EQ(status_, moveit_servo::StatusCode::NO_WARNING);
+
+  // Ensure warning is reported for displacements command
+  jog_cmd.displacements.push_back(1.0);
+  jog_cmd.header.stamp = servo_test_node_->now();
+  log_.clear();
+  joint_jog_publisher->publish(jog_cmd);
+  rclcpp::sleep_for(std::chrono::milliseconds(100));
+  // The warning message was logged
+  ASSERT_TRUE(logContains("servo_node", "Joint jog command displacements field is not yet supported, ignoring."));
+
+  // Ensure error is reported when number of commands doesn't match number of joints
+  int traj_count_before = traj_count_;
+  jog_cmd.displacements.pop_back();
+  jog_cmd.velocities.pop_back();
+  jog_cmd.header.stamp = servo_test_node_->now();
+  joint_jog_publisher->publish(jog_cmd);
+  log_.clear();
+  rclcpp::sleep_for(std::chrono::milliseconds(100));
+  // The warning message was logged
+  ASSERT_TRUE(logContains("servo_node.servo_node.moveit.ros.servo",
+                          "Invalid joint jog command. Each joint name must have one corresponding velocity command. "
+                          "Received 7 joints with 6 commands."));
+  jog_cmd.velocities.push_back(1.0);
+  jog_cmd.velocities.push_back(1.0);
+  jog_cmd.header.stamp = servo_test_node_->now();
+  log_.clear();
+  joint_jog_publisher->publish(jog_cmd);
+  rclcpp::sleep_for(std::chrono::milliseconds(100));
+  // The warning message was logged
+  ASSERT_TRUE(logContains("servo_node.servo_node.moveit.ros.servo",
+                          "Invalid joint jog command. Each joint name must have one corresponding velocity command. "
+                          "Received 7 joints with 8 commands."));
+  // No additional trajectories were generated with the invalid commands
+  ASSERT_EQ(traj_count_, traj_count_before);
+  status = status_;
+  RCLCPP_INFO_STREAM(servo_test_node_->get_logger(), "Status after invalid jointjog: " << static_cast<size_t>(status));
+  ASSERT_EQ(status_, moveit_servo::StatusCode::INVALID);
 }
 
 TEST_F(ServoRosFixture, testTwist)