Fix Servo JointJog Crash

moveit_ros/moveit_servo/src/servo_node.cpp

@@ -218,6 +218,20 @@ 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.size() > 0)
+  {
+    RCLCPP_WARN(node_->get_logger(), "JointJog: Displacements field is not supported.");
+  }
+
+  if (latest_joint_jog_.velocities.size() != latest_joint_jog_.joint_names.size())
+  {
+    RCLCPP_ERROR_STREAM(node_->get_logger(),
+                        "JointJog: each joint name must have one corresponding velocity command. Received "
+                            << latest_joint_jog_.joint_names.size() << " joints with "
+                            << latest_joint_jog_.velocities.size() << " commands.");
+    return std::nullopt;
+  }
+
   const bool command_stale = (node_->now() - latest_joint_jog_.header.stamp) >=
                              rclcpp::Duration::from_seconds(servo_params_.incoming_command_timeout);
   if (!command_stale)

moveit_ros/moveit_servo/tests/test_ros_integration.cpp

@@ -103,6 +103,32 @@ TEST_F(ServoRosFixture, testJointJog)
 
   ASSERT_GE(traj_count_, NUM_COMMANDS);
 
+  // Ensure deprecation warning is reported for displacements command
+  jog_cmd.displacements.push_back(1.0);
+  jog_cmd.header.stamp = servo_test_node_->now();
+  joint_jog_publisher->publish(jog_cmd);
+  rclcpp::sleep_for(std::chrono::milliseconds(100));
+
+  // TODO how to test logged warnings?
+
+  // 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);
+  rclcpp::sleep_for(std::chrono::milliseconds(100));
+  jog_cmd.velocities.push_back(1.0);
+  jog_cmd.velocities.push_back(1.0);
+  jog_cmd.header.stamp = servo_test_node_->now();
+  joint_jog_publisher->publish(jog_cmd);
+
+  // TODO how to test logged warnings?
+  // For now, assert that no additional trajectories were generated with the invalid commands
+  ASSERT_EQ(traj_count_, traj_count_before);
+
+
+  rclcpp::sleep_for(std::chrono::milliseconds(100));
   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);