Fix Servo JointJog Crash

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 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

@@ -94,24 +94,31 @@ JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const mo
 
   velocities.setZero();
   bool names_valid = true;
-
-  for (size_t i = 0; i < command.names.size(); ++i)
+  bool command_valid = true;
+  if (command.velocities.size() == command.names.size())
   {
-    auto it = std::find(joint_names.begin(), joint_names.end(), command.names[i]);
-    if (it != std::end(joint_names))
-    {
-      velocities[std::distance(joint_names.begin(), it)] = command.velocities[i];
-    }
-    else
+    for (size_t i = 0; i < command.names.size(); ++i)
     {
-      RCLCPP_WARN_STREAM(getLogger(), "Invalid joint name: " << command.names[i]);
+      auto it = std::find(joint_names.begin(), joint_names.end(), command.names[i]);
+      if (it != std::end(joint_names))
+      {
+        velocities[std::distance(joint_names.begin(), it)] = command.velocities[i];
+      }
+      else
+      {
+        RCLCPP_WARN_STREAM(getLogger(), "Invalid joint name: " << command.names[i]);
 
-      names_valid = false;
-      break;
+        names_valid = false;
+        break;
+      }
     }
   }
+  else
+  {
+    command_valid = false;
+  }
   const bool velocity_valid = isValidCommand(velocities);
-  if (names_valid && velocity_valid)
+  if (command_valid && names_valid && velocity_valid)
   {
     joint_position_delta = velocities * servo_params.publish_period;
     if (servo_params.command_in_type == "unitless")
@@ -122,6 +129,13 @@ JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const mo
   else
   {
     status = StatusCode::INVALID;
+    if (!command_valid)
+    {
+      RCLCPP_WARN_STREAM(getLogger(), "Invalid joint jog command. Each joint name must have one corresponding "
+                                      "velocity command. Received "
+                                          << command.names.size() << " joints with " << command.velocities.size()
+                                          << " commands.");
+    }
     if (!names_valid)
     {
       RCLCPP_WARN_STREAM(getLogger(),

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(std::string logger_name, 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 deprecation 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 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)