From 15621b9c3ba9f4831ff67b47c6f4b21b9016fb10 Mon Sep 17 00:00:00 2001 From: Matthew Foran Date: Sat, 15 Feb 2025 11:55:41 -0500 Subject: [PATCH 01/10] Servo JointJog: add check for number of velocity commands --- moveit_ros/moveit_servo/src/servo_node.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index 609d1e6585..35ee8bc6c3 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -218,6 +218,18 @@ std::optional 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) From c0bf915d598f855a8191c9ceb8dd3c3cd0c12a6b Mon Sep 17 00:00:00 2001 From: Matthew Foran Date: Sat, 15 Feb 2025 12:02:47 -0500 Subject: [PATCH 02/10] clang-format --- moveit_ros/moveit_servo/src/servo_node.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index 35ee8bc6c3..ce52ff5fdd 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -225,8 +225,10 @@ std::optional ServoNode::processJointJogCommand(const moveit::co 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."); + 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; } From f335ae0c40bc1870ffb88c16553401a9297ed78d Mon Sep 17 00:00:00 2001 From: Matthew Foran Date: Sat, 15 Feb 2025 12:50:25 -0500 Subject: [PATCH 03/10] test --- .../tests/test_ros_integration.cpp | 26 +++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/moveit_ros/moveit_servo/tests/test_ros_integration.cpp b/moveit_ros/moveit_servo/tests/test_ros_integration.cpp index 4485200b7e..7eb68c4bb5 100644 --- a/moveit_ros/moveit_servo/tests/test_ros_integration.cpp +++ b/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(status)); ASSERT_EQ(status_, moveit_servo::StatusCode::NO_WARNING); From 2ce05eed3817dea94060eef634e626ed63b1acdc Mon Sep 17 00:00:00 2001 From: Matthew Foran Date: Fri, 21 Feb 2025 15:50:47 +0000 Subject: [PATCH 04/10] assert log warnings --- moveit_ros/moveit_servo/src/servo_node.cpp | 4 ++- .../moveit_servo/tests/servo_ros_fixture.hpp | 26 +++++++++++++++++++ .../tests/test_ros_integration.cpp | 17 +++++++----- 3 files changed, 40 insertions(+), 7 deletions(-) diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index ce52ff5fdd..37db0774cb 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -218,9 +218,10 @@ std::optional 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) + if (!latest_joint_jog_.displacements.empty()) { RCLCPP_WARN(node_->get_logger(), "JointJog: Displacements field is not supported."); + latest_joint_jog_.displacements.clear(); // Only warn once per message. } if (latest_joint_jog_.velocities.size() != latest_joint_jog_.joint_names.size()) @@ -229,6 +230,7 @@ std::optional ServoNode::processJointJogCommand(const moveit::co "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."); + new_joint_jog_msg_ = false; // Stop trying to process this message return std::nullopt; } diff --git a/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp b/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp index 2e0662bb1f..ae3433f022 100644 --- a/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp +++ b/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp @@ -49,6 +49,7 @@ #include #include #include +#include #include #include @@ -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( + "/rosout", rclcpp::SystemDefaultsQoS(), + [this](const rcl_interfaces::msg::Log::ConstSharedPtr& msg) { return logCallback(msg); }); + switch_input_client_ = servo_test_node_->create_client("/servo_node/switch_command_type"); @@ -145,6 +155,20 @@ class ServoRosFixture : public testing::Test } } + bool logContains(std::string msg) + { + bool found = false; + for (auto line : log_) + { + if (line.name == "servo_node" && 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::SharedPtr status_subscriber_; rclcpp::Subscription::SharedPtr joint_state_subscriber_; rclcpp::Subscription::SharedPtr trajectory_subscriber_; + rclcpp::Subscription::SharedPtr log_subscriber_; rclcpp::Client::SharedPtr switch_input_client_; sensor_msgs::msg::JointState joint_states_; @@ -168,4 +193,5 @@ class ServoRosFixture : public testing::Test std::atomic state_count_; std::atomic traj_count_; + std::vector log_; }; diff --git a/moveit_ros/moveit_servo/tests/test_ros_integration.cpp b/moveit_ros/moveit_servo/tests/test_ros_integration.cpp index 7eb68c4bb5..390a875f1b 100644 --- a/moveit_ros/moveit_servo/tests/test_ros_integration.cpp +++ b/moveit_ros/moveit_servo/tests/test_ros_integration.cpp @@ -106,10 +106,11 @@ TEST_F(ServoRosFixture, testJointJog) // 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)); - - // TODO how to test logged warnings? + // The warning message was logged + ASSERT_TRUE(logContains("JointJog: Displacements field is not supported.")); // Ensure error is reported when number of commands doesn't match number of joints int traj_count_before = traj_count_; @@ -117,17 +118,21 @@ TEST_F(ServoRosFixture, testJointJog) 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("JointJog: each joint name must have one corresponding velocity command.")); 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); - - // TODO how to test logged warnings? - // For now, assert that no additional trajectories were generated with the invalid commands + rclcpp::sleep_for(std::chrono::milliseconds(100)); + // The warning message was logged + ASSERT_TRUE(logContains("JointJog: each joint name must have one corresponding velocity command.")); + // 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(status)); From 380a852a4f43a81fbe2d7a7a3c69c27b5c9b87d7 Mon Sep 17 00:00:00 2001 From: Matthew Foran Date: Fri, 21 Feb 2025 17:09:30 +0000 Subject: [PATCH 05/10] move check from ros node to command processor --- moveit_ros/moveit_servo/src/servo_node.cpp | 13 ++----- moveit_ros/moveit_servo/src/utils/command.cpp | 38 +++++++++++++------ .../moveit_servo/tests/servo_ros_fixture.hpp | 4 +- .../tests/test_ros_integration.cpp | 20 ++++++---- 4 files changed, 43 insertions(+), 32 deletions(-) diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index 37db0774cb..690cda7a98 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -224,22 +224,15 @@ std::optional ServoNode::processJointJogCommand(const moveit::co latest_joint_jog_.displacements.clear(); // Only warn once per message. } - 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."); - new_joint_jog_msg_ = false; // Stop trying to process this message - 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) { 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 { diff --git a/moveit_ros/moveit_servo/src/utils/command.cpp b/moveit_ros/moveit_servo/src/utils/command.cpp index 0470edebe8..ef602c1655 100644 --- a/moveit_ros/moveit_servo/src/utils/command.cpp +++ b/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(), diff --git a/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp b/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp index ae3433f022..ffe18f2652 100644 --- a/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp +++ b/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp @@ -155,12 +155,12 @@ class ServoRosFixture : public testing::Test } } - bool logContains(std::string msg) + bool logContains(std::string logger_name, std::string msg) { bool found = false; for (auto line : log_) { - if (line.name == "servo_node" && line.msg.find(msg) != std::string::npos) + if (line.name == logger_name && line.msg.find(msg) != std::string::npos) { found = true; break; diff --git a/moveit_ros/moveit_servo/tests/test_ros_integration.cpp b/moveit_ros/moveit_servo/tests/test_ros_integration.cpp index 390a875f1b..71f91160cf 100644 --- a/moveit_ros/moveit_servo/tests/test_ros_integration.cpp +++ b/moveit_ros/moveit_servo/tests/test_ros_integration.cpp @@ -103,6 +103,10 @@ TEST_F(ServoRosFixture, testJointJog) ASSERT_GE(traj_count_, NUM_COMMANDS); + moveit_servo::StatusCode status = status_; + RCLCPP_INFO_STREAM(servo_test_node_->get_logger(), "Status after jointjog: " << static_cast(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(); @@ -110,7 +114,7 @@ TEST_F(ServoRosFixture, testJointJog) joint_jog_publisher->publish(jog_cmd); rclcpp::sleep_for(std::chrono::milliseconds(100)); // The warning message was logged - ASSERT_TRUE(logContains("JointJog: Displacements field is not supported.")); + ASSERT_TRUE(logContains("servo_node", "JointJog: Displacements field is not supported.")); // Ensure error is reported when number of commands doesn't match number of joints int traj_count_before = traj_count_; @@ -121,7 +125,8 @@ TEST_F(ServoRosFixture, testJointJog) log_.clear(); rclcpp::sleep_for(std::chrono::milliseconds(100)); // The warning message was logged - ASSERT_TRUE(logContains("JointJog: each joint name must have one corresponding velocity command.")); + ASSERT_TRUE(logContains("servo_node.servo_node.moveit.ros.servo", + "Invalid joint jog command. Each joint name must have one corresponding velocity command.")); jog_cmd.velocities.push_back(1.0); jog_cmd.velocities.push_back(1.0); jog_cmd.header.stamp = servo_test_node_->now(); @@ -129,14 +134,13 @@ TEST_F(ServoRosFixture, testJointJog) joint_jog_publisher->publish(jog_cmd); rclcpp::sleep_for(std::chrono::milliseconds(100)); // The warning message was logged - ASSERT_TRUE(logContains("JointJog: each joint name must have one corresponding velocity command.")); + ASSERT_TRUE(logContains("servo_node.servo_node.moveit.ros.servo", + "Invalid joint jog command. Each joint name must have one corresponding velocity command.")); // 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(status)); - ASSERT_EQ(status_, moveit_servo::StatusCode::NO_WARNING); + status = status_; + RCLCPP_INFO_STREAM(servo_test_node_->get_logger(), "Status after invalid jointjog: " << static_cast(status)); + ASSERT_EQ(status_, moveit_servo::StatusCode::INVALID); } TEST_F(ServoRosFixture, testTwist) From d09185f482a15c292c5dd668483520e3a36e1ece Mon Sep 17 00:00:00 2001 From: Matthew Foran Date: Fri, 21 Feb 2025 17:26:31 +0000 Subject: [PATCH 06/10] wording --- moveit_ros/moveit_servo/src/servo_node.cpp | 2 +- moveit_ros/moveit_servo/tests/test_ros_integration.cpp | 8 +++++--- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index 690cda7a98..7954bbca46 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -220,7 +220,7 @@ std::optional ServoNode::processJointJogCommand(const moveit::co if (!latest_joint_jog_.displacements.empty()) { - RCLCPP_WARN(node_->get_logger(), "JointJog: Displacements field is not supported."); + RCLCPP_WARN(node_->get_logger(), "Joint jog command displacements field is not supported, ignoring."); latest_joint_jog_.displacements.clear(); // Only warn once per message. } diff --git a/moveit_ros/moveit_servo/tests/test_ros_integration.cpp b/moveit_ros/moveit_servo/tests/test_ros_integration.cpp index 71f91160cf..eaee7111ac 100644 --- a/moveit_ros/moveit_servo/tests/test_ros_integration.cpp +++ b/moveit_ros/moveit_servo/tests/test_ros_integration.cpp @@ -114,7 +114,7 @@ TEST_F(ServoRosFixture, testJointJog) joint_jog_publisher->publish(jog_cmd); rclcpp::sleep_for(std::chrono::milliseconds(100)); // The warning message was logged - ASSERT_TRUE(logContains("servo_node", "JointJog: Displacements field is not supported.")); + 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_; @@ -126,7 +126,8 @@ TEST_F(ServoRosFixture, testJointJog) 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.")); + "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(); @@ -135,7 +136,8 @@ TEST_F(ServoRosFixture, testJointJog) 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.")); + "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_; From 9594433c18de568a6c8cca6a1b2aa730291e2304 Mon Sep 17 00:00:00 2001 From: Matthew Foran Date: Wed, 26 Feb 2025 13:57:01 +0000 Subject: [PATCH 07/10] simplify error handling --- .../include/moveit_servo/utils/command.hpp | 4 + moveit_ros/moveit_servo/src/servo_node.cpp | 2 +- moveit_ros/moveit_servo/src/utils/command.cpp | 220 ++++++++---------- .../moveit_servo/tests/servo_ros_fixture.hpp | 2 +- .../tests/test_ros_integration.cpp | 4 +- 5 files changed, 101 insertions(+), 131 deletions(-) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp b/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp index bf9c432527..14ea9f8f28 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp @@ -47,6 +47,10 @@ #include #include +#define COMMAND_ERROR(msg_stream) \ + RCLCPP_WARN_STREAM(getLogger(), msg_stream); \ + return std::make_pair(StatusCode::INVALID, joint_position_delta) + namespace moveit_servo { diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index 7954bbca46..9720fc7fa8 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -220,7 +220,7 @@ std::optional ServoNode::processJointJogCommand(const moveit::co if (!latest_joint_jog_.displacements.empty()) { - RCLCPP_WARN(node_->get_logger(), "Joint jog command displacements field is not supported, ignoring."); + 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. } diff --git a/moveit_ros/moveit_servo/src/utils/command.cpp b/moveit_ros/moveit_servo/src/utils/command.cpp index ef602c1655..da6fd71647 100644 --- a/moveit_ros/moveit_servo/src/utils/command.cpp +++ b/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,69 +92,48 @@ JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const mo Eigen::VectorXd velocities(joint_names.size()); velocities.setZero(); - bool names_valid = true; - bool command_valid = true; - if (command.velocities.size() == command.names.size()) - { - for (size_t i = 0; i < command.names.size(); ++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; - } - } - } - else + if (command.velocities.size() != command.names.size()) { - command_valid = false; + 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."); } - const bool velocity_valid = isValidCommand(velocities); - if (command_valid && names_valid && velocity_valid) - { - joint_position_delta = velocities * servo_params.publish_period; - if (servo_params.command_in_type == "unitless") - { - joint_position_delta *= servo_params.scale.joint; - } - } - else + + for (size_t i = 0; i < command.names.size(); ++i) { - status = StatusCode::INVALID; - if (!command_valid) + auto it = std::find(joint_names.begin(), joint_names.end(), command.names[i]); + if (it != std::end(joint_names)) { - 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."); + velocities[std::distance(joint_names.begin(), it)] = command.velocities[i]; } - 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) + else { - RCLCPP_WARN_STREAM(getLogger(), "Invalid velocity values in joint jog command"); + 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."); } } + if (!isValidCommand(velocities)) + { + COMMAND_ERROR("Invalid velocity values in joint jog command"); + } + + joint_position_delta = velocities * servo_params.publish_period; + if (servo_params.command_in_type == "unitless") + { + 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, @@ -168,10 +146,21 @@ JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit:: Eigen::VectorXd joint_position_delta(num_joints); Eigen::Vector 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; @@ -222,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); } @@ -251,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 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 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); diff --git a/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp b/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp index ffe18f2652..1f2938c4ba 100644 --- a/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp +++ b/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp @@ -155,7 +155,7 @@ class ServoRosFixture : public testing::Test } } - bool logContains(std::string logger_name, std::string msg) + bool logContains(const std::string& logger_name, const std::string& msg) { bool found = false; for (auto line : log_) diff --git a/moveit_ros/moveit_servo/tests/test_ros_integration.cpp b/moveit_ros/moveit_servo/tests/test_ros_integration.cpp index eaee7111ac..5e4cfaff9a 100644 --- a/moveit_ros/moveit_servo/tests/test_ros_integration.cpp +++ b/moveit_ros/moveit_servo/tests/test_ros_integration.cpp @@ -107,14 +107,14 @@ TEST_F(ServoRosFixture, testJointJog) RCLCPP_INFO_STREAM(servo_test_node_->get_logger(), "Status after jointjog: " << static_cast(status)); ASSERT_EQ(status_, moveit_servo::StatusCode::NO_WARNING); - // Ensure deprecation warning is reported for displacements command + // 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 supported, ignoring.")); + 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_; From 4a56113676afc8d78b7d5a949a2578f275cabc09 Mon Sep 17 00:00:00 2001 From: Matthew Foran Date: Wed, 26 Feb 2025 14:10:10 +0000 Subject: [PATCH 08/10] removed macro per @sea-bass --- .../include/moveit_servo/utils/command.hpp | 4 --- moveit_ros/moveit_servo/src/utils/command.cpp | 34 ++++++++++++------- 2 files changed, 21 insertions(+), 17 deletions(-) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp b/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp index 14ea9f8f28..bf9c432527 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp @@ -47,10 +47,6 @@ #include #include -#define COMMAND_ERROR(msg_stream) \ - RCLCPP_WARN_STREAM(getLogger(), msg_stream); \ - return std::make_pair(StatusCode::INVALID, joint_position_delta) - namespace moveit_servo { diff --git a/moveit_ros/moveit_servo/src/utils/command.cpp b/moveit_ros/moveit_servo/src/utils/command.cpp index da6fd71647..6005ddd1be 100644 --- a/moveit_ros/moveit_servo/src/utils/command.cpp +++ b/moveit_ros/moveit_servo/src/utils/command.cpp @@ -94,9 +94,11 @@ JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const mo velocities.setZero(); 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."); + 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."); + return std::make_pair(StatusCode::INVALID, joint_position_delta); } for (size_t i = 0; i < command.names.size(); ++i) @@ -108,17 +110,19 @@ JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const mo } else { - 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."); + RCLCPP_WARN_STREAM(getLogger(), "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."); + return std::make_pair(StatusCode::INVALID, joint_position_delta); } } if (!isValidCommand(velocities)) { - COMMAND_ERROR("Invalid velocity values in joint jog command"); + RCLCPP_WARN_STREAM(getLogger(), "Invalid velocity values in joint jog command"); + return std::make_pair(StatusCode::INVALID, joint_position_delta); } joint_position_delta = velocities * servo_params.publish_period; @@ -148,12 +152,14 @@ JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit:: if (command.frame_id != planning_frame) { - COMMAND_ERROR("Command frame is: " << command.frame_id << ", expected: " << planning_frame); + RCLCPP_WARN_STREAM(getLogger(), "Command frame is: " << command.frame_id << ", expected: " << planning_frame); + return std::make_pair(StatusCode::INVALID, joint_position_delta); } if (!isValidCommand(command)) { - COMMAND_ERROR("Invalid twist command."); + RCLCPP_WARN_STREAM(getLogger(), "Invalid twist command."); + return std::make_pair(StatusCode::INVALID, joint_position_delta); } if (command.velocities.isZero()) @@ -227,12 +233,14 @@ JointDeltaResult jointDeltaFromPose(const PoseCommand& command, const moveit::co if (!isValidCommand(command)) { - COMMAND_ERROR("Invalid pose command."); + RCLCPP_WARN_STREAM(getLogger(), "Invalid pose command."); + return std::make_pair(StatusCode::INVALID, joint_position_delta); } if (command.frame_id != planning_frame) { - COMMAND_ERROR("Command frame is: " << command.frame_id << " expected: " << planning_frame); + RCLCPP_WARN_STREAM(getLogger(), "Command frame is: " << command.frame_id << " expected: " << planning_frame); + return std::make_pair(StatusCode::INVALID, joint_position_delta); } Eigen::Vector cartesian_position_delta; From f9f6099a48612a938ec3722510269878a429cada Mon Sep 17 00:00:00 2001 From: Matthew Foran Date: Wed, 26 Feb 2025 15:31:11 +0000 Subject: [PATCH 09/10] fix CI clang-tidy --- moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp b/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp index 1f2938c4ba..859686263f 100644 --- a/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp +++ b/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp @@ -158,7 +158,7 @@ class ServoRosFixture : public testing::Test bool logContains(const std::string& logger_name, const std::string& msg) { bool found = false; - for (auto line : log_) + for (const auto& line : log_) { if (line.name == logger_name && line.msg.find(msg) != std::string::npos) { From 211cbe9360a05260e3c2f165bc0670e41ce4e5cc Mon Sep 17 00:00:00 2001 From: Matthew Foran Date: Thu, 27 Feb 2025 13:33:41 +0000 Subject: [PATCH 10/10] simplify tests --- .../moveit_servo/tests/servo_ros_fixture.hpp | 25 ------------------ .../tests/test_ros_integration.cpp | 26 +++---------------- 2 files changed, 4 insertions(+), 47 deletions(-) diff --git a/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp b/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp index 859686263f..1b8853ba05 100644 --- a/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp +++ b/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp @@ -74,11 +74,6 @@ 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. @@ -98,10 +93,6 @@ 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( - "/rosout", rclcpp::SystemDefaultsQoS(), - [this](const rcl_interfaces::msg::Log::ConstSharedPtr& msg) { return logCallback(msg); }); - switch_input_client_ = servo_test_node_->create_client("/servo_node/switch_command_type"); @@ -155,20 +146,6 @@ class ServoRosFixture : public testing::Test } } - bool logContains(const std::string& logger_name, const std::string& msg) - { - bool found = false; - for (const 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 } { } @@ -182,7 +159,6 @@ class ServoRosFixture : public testing::Test rclcpp::Subscription::SharedPtr status_subscriber_; rclcpp::Subscription::SharedPtr joint_state_subscriber_; rclcpp::Subscription::SharedPtr trajectory_subscriber_; - rclcpp::Subscription::SharedPtr log_subscriber_; rclcpp::Client::SharedPtr switch_input_client_; sensor_msgs::msg::JointState joint_states_; @@ -193,5 +169,4 @@ class ServoRosFixture : public testing::Test std::atomic state_count_; std::atomic traj_count_; - std::vector log_; }; diff --git a/moveit_ros/moveit_servo/tests/test_ros_integration.cpp b/moveit_ros/moveit_servo/tests/test_ros_integration.cpp index 5e4cfaff9a..aaa4faa545 100644 --- a/moveit_ros/moveit_servo/tests/test_ros_integration.cpp +++ b/moveit_ros/moveit_servo/tests/test_ros_integration.cpp @@ -107,42 +107,24 @@ TEST_F(ServoRosFixture, testJointJog) RCLCPP_INFO_STREAM(servo_test_node_->get_logger(), "Status after jointjog: " << static_cast(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 + // Ensure error 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.")); + ASSERT_EQ(status_, moveit_servo::StatusCode::INVALID); 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.")); + ASSERT_EQ(status_, moveit_servo::StatusCode::INVALID); + // 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(status)); - ASSERT_EQ(status_, moveit_servo::StatusCode::INVALID); } TEST_F(ServoRosFixture, testTwist)