From 061fb695136a8780f3565489b2896d016c6ad9f2 Mon Sep 17 00:00:00 2001 From: ibrahiminfinite Date: Sat, 5 Aug 2023 23:10:16 +0530 Subject: [PATCH] Update tests + clang-tidy --- .../config/test_config_panda.yaml | 1 + .../demos/servo_keyboard_input.cpp | 26 ++- .../include/moveit_servo/servo_node.hpp | 14 +- moveit_ros/moveit_servo/src/servo.cpp | 2 +- moveit_ros/moveit_servo/src/servo_node.cpp | 51 +++-- moveit_ros/moveit_servo/src/utils/command.cpp | 8 + moveit_ros/moveit_servo/src/utils/common.cpp | 1 + .../launch/servo_cpp_integration.test.py | 2 - .../launch/servo_ros_integration.test.py | 55 ++++-- .../tests/launch/servo_utils.test.py | 2 - .../moveit_servo/tests/servo_ros_fixture.hpp | 24 ++- .../tests/test_ros_integration.cpp | 59 ++---- moveit_ros/moveit_servo/tests/test_utils.cpp | 175 +++++++++--------- 13 files changed, 228 insertions(+), 192 deletions(-) diff --git a/moveit_ros/moveit_servo/config/test_config_panda.yaml b/moveit_ros/moveit_servo/config/test_config_panda.yaml index 17be41c3b9..97a12cd3e9 100644 --- a/moveit_ros/moveit_servo/config/test_config_panda.yaml +++ b/moveit_ros/moveit_servo/config/test_config_panda.yaml @@ -8,6 +8,7 @@ ## Properties of outgoing commands publish_period: 0.02 # 1/Nominal publish rate [seconds] +incoming_command_timeout: 0.5 # seconds command_in_type: "unitless" # "unitless" in the range [-1:1], as if from joystick. "speed_units" cmds are in m/s and rad/s scale: # Scale parameters are only used if command_in_type=="unitless" diff --git a/moveit_ros/moveit_servo/demos/servo_keyboard_input.cpp b/moveit_ros/moveit_servo/demos/servo_keyboard_input.cpp index dd8c051b66..dd865c6408 100644 --- a/moveit_ros/moveit_servo/demos/servo_keyboard_input.cpp +++ b/moveit_ros/moveit_servo/demos/servo_keyboard_input.cpp @@ -83,21 +83,21 @@ const std::string PLANNING_FRAME_ID = "panda_link0"; class KeyboardReader { public: - KeyboardReader() : kfd(0) + KeyboardReader() : file_descriptor_(0) { // get the console in raw mode - tcgetattr(kfd, &cooked); + tcgetattr(file_descriptor_, &cooked_); struct termios raw; - memcpy(&raw, &cooked, sizeof(struct termios)); + memcpy(&raw, &cooked_, sizeof(struct termios)); raw.c_lflag &= ~(ICANON | ECHO); // Setting a new line, then end of file raw.c_cc[VEOL] = 1; raw.c_cc[VEOF] = 2; - tcsetattr(kfd, TCSANOW, &raw); + tcsetattr(file_descriptor_, TCSANOW, &raw); } void readOne(char* c) { - int rc = read(kfd, c, 1); + int rc = read(file_descriptor_, c, 1); if (rc < 0) { throw std::runtime_error("read failed"); @@ -105,12 +105,12 @@ class KeyboardReader } void shutdown() { - tcsetattr(kfd, TCSANOW, &cooked); + tcsetattr(file_descriptor_, TCSANOW, &cooked_); } private: - int kfd; - struct termios cooked; + int file_descriptor_; + struct termios cooked_; }; // Converts key-presses to Twist or Jog commands for Servo, in lieu of a controller @@ -182,7 +182,7 @@ int KeyboardServo::keyLoop() bool publish_twist = false; bool publish_joint = false; - std::thread{ std::bind(&KeyboardServo::spin, this) }.detach(); + std::thread{ [this]() { return spin(); } }.detach(); puts("Reading from keyboard"); puts("---------------------------"); @@ -298,9 +298,13 @@ int KeyboardServo::keyLoop() { auto result = switch_input_->async_send_request(request_); if (result.get()->success) + { RCLCPP_INFO_STREAM(nh_->get_logger(), "Switched to input type: JointJog"); + } else + { RCLCPP_WARN_STREAM(nh_->get_logger(), "Could not switch input to: JointJog"); + } } break; case KEYCODE_T: @@ -311,9 +315,13 @@ int KeyboardServo::keyLoop() { auto result = switch_input_->async_send_request(request_); if (result.get()->success) + { RCLCPP_INFO_STREAM(nh_->get_logger(), "Switched to input type: Twist"); + } else + { RCLCPP_WARN_STREAM(nh_->get_logger(), "Could not switch input to: Twist"); + } } break; case KEYCODE_Q: diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp b/moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp index 4fc05296e7..1a15c4f29c 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp @@ -81,20 +81,20 @@ class ServoNode * \brief The service to pause servoing, this does not exit the loop or stop the servo loop thread. * The loop will be alive even after pausing, but no commands will be processed. */ - void pauseServo(const std::shared_ptr request, - const std::shared_ptr response); + void pauseServo(const std::shared_ptr& request, + const std::shared_ptr& response); /** * \brief The service to set the command type for Servo. * Supported command types can be found in utils/datatypes.hpp * This service must be used to set the command type before sending any servoing commands. */ - void switchCommandType(const std::shared_ptr request, - const std::shared_ptr response); + void switchCommandType(const std::shared_ptr& request, + const std::shared_ptr& response); - void jointJogCallback(const control_msgs::msg::JointJog::SharedPtr msg); - void twistCallback(const geometry_msgs::msg::TwistStamped::SharedPtr msg); - void poseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg); + void jointJogCallback(const control_msgs::msg::JointJog::ConstSharedPtr& msg); + void twistCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr& msg); + void poseCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr& msg); std::optional processJointJogCommand(); std::optional processTwistCommand(); diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index 94805fff45..91763f9afe 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -55,7 +55,7 @@ namespace moveit_servo Servo::Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptr servo_param_listener, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) : node_(node) - , servo_param_listener_{ servo_param_listener } + , servo_param_listener_{ std::move(servo_param_listener) } , planning_scene_monitor_{ planning_scene_monitor } , transform_buffer_(node_->get_clock()) , transform_listener_(transform_buffer_) diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index 3b86e543ba..9bd087ac65 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -80,9 +80,13 @@ ServoNode::ServoNode(const rclcpp::NodeOptions& options) if (realtime_tools::has_realtime_kernel()) { if (realtime_tools::configure_sched_fifo(servo_params_.thread_priority)) + { RCLCPP_INFO_STREAM(LOGGER, "Realtime kernel available, higher thread priority has been set."); + } else + { RCLCPP_WARN_STREAM(LOGGER, "Could not enable FIFO RT scheduling policy."); + } } else { @@ -100,42 +104,53 @@ ServoNode::ServoNode(const rclcpp::NodeOptions& options) // Create subscriber for jointjog joint_jog_subscriber_ = node_->create_subscription( - servo_params_.joint_command_in_topic, 10, std::bind(&ServoNode::jointJogCallback, this, std::placeholders::_1)); + servo_params_.joint_command_in_topic, rclcpp::SystemDefaultsQoS(), + [this](const control_msgs::msg::JointJog::ConstSharedPtr& msg) { return jointJogCallback(msg); }); // Create subscriber for twist twist_subscriber_ = node_->create_subscription( - servo_params_.cartesian_command_in_topic, 10, std::bind(&ServoNode::twistCallback, this, std::placeholders::_1)); + servo_params_.cartesian_command_in_topic, rclcpp::SystemDefaultsQoS(), + [this](const geometry_msgs::msg::TwistStamped::ConstSharedPtr& msg) { return twistCallback(msg); }); // Create subscriber for pose pose_subscriber_ = node_->create_subscription( - servo_params_.pose_command_in_topic, 10, std::bind(&ServoNode::poseCallback, this, std::placeholders::_1)); + servo_params_.pose_command_in_topic, rclcpp::SystemDefaultsQoS(), + [this](const geometry_msgs::msg::PoseStamped::ConstSharedPtr& msg) { return poseCallback(msg); }); if (servo_params_.command_out_type == "trajectory_msgs/JointTrajectory") + { trajectory_publisher_ = node_->create_publisher( servo_params_.command_out_topic, rclcpp::SystemDefaultsQoS()); + } else if (servo_params_.command_out_type == "std_msgs/Float64MultiArray") + { multi_array_publisher_ = node_->create_publisher(servo_params_.command_out_topic, rclcpp::SystemDefaultsQoS()); - + } // Create status publisher status_publisher_ = node_->create_publisher(servo_params_.status_topic, rclcpp::SystemDefaultsQoS()); // Create service to enable switching command type switch_command_type_ = node_->create_service( - "~/switch_command_type", - std::bind(&ServoNode::switchCommandType, this, std::placeholders::_1, std::placeholders::_2)); + "~/switch_command_type", [this](const std::shared_ptr& request, + const std::shared_ptr& response) { + return switchCommandType(request, response); + }); // Create service to pause/unpause servoing pause_servo_ = node_->create_service( - "~/pause_servo", std::bind(&ServoNode::pauseServo, this, std::placeholders::_1, std::placeholders::_2)); + "~/pause_servo", [this](const std::shared_ptr& request, + const std::shared_ptr& response) { + return pauseServo(request, response); + }); // Start the servoing loop servo_loop_thread_ = std::thread(&ServoNode::servoLoop, this); } -void ServoNode::pauseServo(const std::shared_ptr request, - const std::shared_ptr response) +void ServoNode::pauseServo(const std::shared_ptr& request, + const std::shared_ptr& response) { servo_paused_ = request->data; response->success = (servo_paused_ == request->data); @@ -151,8 +166,8 @@ void ServoNode::pauseServo(const std::shared_ptr request, - const std::shared_ptr response) +void ServoNode::switchCommandType(const std::shared_ptr& request, + const std::shared_ptr& response) { const bool is_valid = (request->command_type >= static_cast(CommandType::MIN)) && (request->command_type <= static_cast(CommandType::MAX)); @@ -167,19 +182,19 @@ void ServoNode::switchCommandType(const std::shared_ptrsuccess = (request->command_type == static_cast(servo_->getCommandType())); } -void ServoNode::jointJogCallback(const control_msgs::msg::JointJog::SharedPtr msg) +void ServoNode::jointJogCallback(const control_msgs::msg::JointJog::ConstSharedPtr& msg) { latest_joint_jog_ = *msg; new_joint_jog_msg_ = true; } -void ServoNode::twistCallback(const geometry_msgs::msg::TwistStamped::SharedPtr msg) +void ServoNode::twistCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr& msg) { latest_twist_ = *msg; new_twist_msg_ = true; } -void ServoNode::poseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) +void ServoNode::poseCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr& msg) { latest_pose_ = *msg; new_pose_msg_ = true; @@ -295,17 +310,17 @@ void ServoNode::servoLoop() continue; next_joint_state = std::nullopt; - const CommandType expectedType = servo_->getCommandType(); + const CommandType expected_type = servo_->getCommandType(); - if (expectedType == CommandType::JOINT_JOG && new_joint_jog_msg_) + if (expected_type == CommandType::JOINT_JOG && new_joint_jog_msg_) { next_joint_state = processJointJogCommand(); } - else if (expectedType == CommandType::TWIST && new_twist_msg_) + else if (expected_type == CommandType::TWIST && new_twist_msg_) { next_joint_state = processTwistCommand(); } - else if (expectedType == CommandType::POSE && new_pose_msg_) + else if (expected_type == CommandType::POSE && new_pose_msg_) { next_joint_state = processPoseCommand(); } diff --git a/moveit_ros/moveit_servo/src/utils/command.cpp b/moveit_ros/moveit_servo/src/utils/command.cpp index a5498a91e3..d71e2c305f 100644 --- a/moveit_ros/moveit_servo/src/utils/command.cpp +++ b/moveit_ros/moveit_servo/src/utils/command.cpp @@ -147,10 +147,14 @@ JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, moveit::core:: { status = StatusCode::INVALID; if (!valid_command) + { RCLCPP_WARN_STREAM(LOGGER, "Invalid twist command."); + } if (!is_planning_frame) + { RCLCPP_WARN_STREAM(LOGGER, "Command frame is: " << command.frame_id << " expected: " << servo_params.planning_frame); + } } return std::make_pair(status, joint_position_delta); } @@ -191,10 +195,14 @@ JointDeltaResult jointDeltaFromPose(const PoseCommand& command, moveit::core::Ro { status = StatusCode::INVALID; if (!valid_command) + { RCLCPP_WARN_STREAM(LOGGER, "Invalid pose command."); + } if (!is_planning_frame) + { RCLCPP_WARN_STREAM(LOGGER, "Command frame is: " << command.frame_id << " expected: " << servo_params.planning_frame); + } } return std::make_pair(status, joint_position_delta); } diff --git a/moveit_ros/moveit_servo/src/utils/common.cpp b/moveit_ros/moveit_servo/src/utils/common.cpp index f879d2fa2e..fe28852a69 100644 --- a/moveit_ros/moveit_servo/src/utils/common.cpp +++ b/moveit_ros/moveit_servo/src/utils/common.cpp @@ -333,6 +333,7 @@ planning_scene_monitor::PlanningSceneMonitorPtr createPlanningSceneMonitor(const // Set up planning_scene_monitor planning_scene_monitor = std::make_shared(node, robot_description_name, "planning_scene_monitor"); + planning_scene_monitor->startStateMonitor(servo_params.joint_topic); planning_scene_monitor->startSceneMonitor(servo_params.monitored_planning_scene_topic); planning_scene_monitor->setPlanningScenePublishingFrequency(25); diff --git a/moveit_ros/moveit_servo/tests/launch/servo_cpp_integration.test.py b/moveit_ros/moveit_servo/tests/launch/servo_cpp_integration.test.py index 4d0321b2d2..ea6c730428 100644 --- a/moveit_ros/moveit_servo/tests/launch/servo_cpp_integration.test.py +++ b/moveit_ros/moveit_servo/tests/launch/servo_cpp_integration.test.py @@ -111,9 +111,7 @@ def generate_test_description(): launch_testing.actions.ReadyToTest(), ] ), { - "test_container": test_container, "servo_gtest": servo_gtest, - "ros2_control_node": ros2_control_node, } diff --git a/moveit_ros/moveit_servo/tests/launch/servo_ros_integration.test.py b/moveit_ros/moveit_servo/tests/launch/servo_ros_integration.test.py index a8c7f9937e..94519096ca 100644 --- a/moveit_ros/moveit_servo/tests/launch/servo_ros_integration.test.py +++ b/moveit_ros/moveit_servo/tests/launch/servo_ros_integration.test.py @@ -6,6 +6,8 @@ from ament_index_python.packages import get_package_share_directory from moveit_configs_utils import MoveItConfigsBuilder from launch_param_builder import ParameterBuilder +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration def generate_test_description(): @@ -15,6 +17,11 @@ def generate_test_description(): .to_moveit_configs() ) + # Launch Servo as a standalone node or as a "node component" for better latency/efficiency + launch_as_standalone_node = LaunchConfiguration( + "launch_as_standalone_node", default="true" + ) + # Get parameters for the Servo node servo_params = { "moveit_servo": ParameterBuilder("moveit_servo") @@ -31,7 +38,6 @@ def generate_test_description(): "config", "ros2_controllers.yaml", ) - ros2_control_node = launch_ros.actions.Node( package="controller_manager", executable="ros2_control_node", @@ -49,6 +55,7 @@ def generate_test_description(): "--controller-manager", "/controller_manager", ], + output="screen", ) panda_arm_controller_spawner = launch_ros.actions.Node( @@ -57,13 +64,28 @@ def generate_test_description(): arguments=["panda_arm_controller", "-c", "/controller_manager"], ) - # Component nodes for tf and Servo - test_container = launch_ros.actions.ComposableNodeContainer( - name="servo_integration_tests_container", + # Launch as much as possible in components + container = launch_ros.actions.ComposableNodeContainer( + name="moveit_servo_demo_container", namespace="/", package="rclcpp_components", executable="component_container_mt", composable_node_descriptions=[ + # Example of launching Servo as a node component + # Launching as a node component makes ROS 2 intraprocess communication more efficient. + launch_ros.descriptions.ComposableNode( + package="moveit_servo", + plugin="moveit_servo::ServoNode", + name="servo_node", + parameters=[ + servo_params, + low_pass_filter_coeff, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + ], + condition=UnlessCondition(launch_as_standalone_node), + ), launch_ros.descriptions.ComposableNode( package="robot_state_publisher", plugin="robot_state_publisher::RobotStatePublisher", @@ -74,12 +96,11 @@ def generate_test_description(): package="tf2_ros", plugin="tf2_ros::StaticTransformBroadcasterNode", name="static_tf2_broadcaster", - parameters=[{"/child_frame_id": "panda_link0", "/frame_id": "world"}], + parameters=[{"child_frame_id": "/panda_link0", "frame_id": "/world"}], ), ], output="screen", ) - # Launch a standalone Servo node. # As opposed to a node component, this may be necessary (for example) if Servo is running on a different PC servo_node = launch_ros.actions.Node( @@ -94,6 +115,7 @@ def generate_test_description(): moveit_config.robot_description_kinematics, ], output="screen", + condition=IfCondition(launch_as_standalone_node), ) servo_gtest = launch_ros.actions.Node( @@ -108,22 +130,19 @@ def generate_test_description(): return launch.LaunchDescription( [ - launch.actions.DeclareLaunchArgument( - name="test_binary_dir", - description="Binary directory of package " - "containing test executables", - ), - ros2_control_node, - joint_state_broadcaster_spawner, - panda_arm_controller_spawner, - test_container, servo_node, - launch.actions.TimerAction(period=2.0, actions=[servo_gtest]), + container, + launch.actions.TimerAction(period=3.0, actions=[ros2_control_node]), + launch.actions.TimerAction( + period=5.0, actions=[joint_state_broadcaster_spawner] + ), + launch.actions.TimerAction( + period=7.0, actions=[panda_arm_controller_spawner] + ), + launch.actions.TimerAction(period=9.0, actions=[servo_gtest]), launch_testing.actions.ReadyToTest(), ] ), { - "test_container": test_container, - "ros2_control_node": ros2_control_node, "servo_gtest": servo_gtest, } diff --git a/moveit_ros/moveit_servo/tests/launch/servo_utils.test.py b/moveit_ros/moveit_servo/tests/launch/servo_utils.test.py index e6e6e6853e..a656655334 100644 --- a/moveit_ros/moveit_servo/tests/launch/servo_utils.test.py +++ b/moveit_ros/moveit_servo/tests/launch/servo_utils.test.py @@ -111,9 +111,7 @@ def generate_test_description(): launch_testing.actions.ReadyToTest(), ] ), { - "test_container": test_container, "servo_gtest": servo_gtest, - "ros2_control_node": ros2_control_node, } diff --git a/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp b/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp index 05ce4b467f..9baf82b3d2 100644 --- a/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp +++ b/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp @@ -48,6 +48,7 @@ #include #include #include +#include class ServoRosFixture : public testing::Test { @@ -60,11 +61,16 @@ class ServoRosFixture : public testing::Test void jointStateCallback(const sensor_msgs::msg::JointState::ConstSharedPtr msg) { - std::lock_guard state_guard(joint_state_mutex_); joint_states_ = *msg; state_count_++; } + void trajectoryCallback(const trajectory_msgs::msg::JointTrajectory::ConstSharedPtr msg) + { + published_trajectory_ = *msg; + traj_count_++; + } + void SetUp() { executor_->add_node(servo_test_node_); @@ -96,7 +102,7 @@ class ServoRosFixture : public testing::Test RCLCPP_INFO(logger, "SERVICE READY"); } - void waitForStatus() + void waitForJointStates() { auto logger = servo_test_node_->get_logger(); auto wait_timeout = rclcpp::Duration::from_seconds(5); @@ -114,7 +120,7 @@ class ServoRosFixture : public testing::Test } } - ServoRosFixture() : state_count_{ 0 } + ServoRosFixture() : state_count_{ 0 }, traj_count_{ 0 } { // Create a node to be given to Servo. servo_test_node_ = std::make_shared("moveit_servo_test"); @@ -128,11 +134,14 @@ class ServoRosFixture : public testing::Test joint_state_subscriber_ = servo_test_node_->create_subscription( "/joint_states", 1, std::bind(&ServoRosFixture::jointStateCallback, this, std::placeholders::_1)); + trajectory_subscriber_ = servo_test_node_->create_subscription( + "/panda_arm_controller/joint_trajectory", 1, + std::bind(&ServoRosFixture::trajectoryCallback, this, std::placeholders::_1)); + switch_input_client_ = servo_test_node_->create_client("/servo_node/switch_command_type"); waitForService(); - waitForStatus(); } std::shared_ptr servo_test_node_; @@ -143,12 +152,15 @@ class ServoRosFixture : public testing::Test rclcpp::Subscription::SharedPtr status_subscriber_; rclcpp::Subscription::SharedPtr joint_state_subscriber_; + rclcpp::Subscription::SharedPtr trajectory_subscriber_; rclcpp::Client::SharedPtr switch_input_client_; + sensor_msgs::msg::JointState joint_states_; + trajectory_msgs::msg::JointTrajectory published_trajectory_; + std::atomic status_count_; std::atomic status_; - std::mutex joint_state_mutex_; std::atomic state_count_; - sensor_msgs::msg::JointState joint_states_; + std::atomic traj_count_; }; diff --git a/moveit_ros/moveit_servo/tests/test_ros_integration.cpp b/moveit_ros/moveit_servo/tests/test_ros_integration.cpp index bc2f918f94..ed23f4d69e 100644 --- a/moveit_ros/moveit_servo/tests/test_ros_integration.cpp +++ b/moveit_ros/moveit_servo/tests/test_ros_integration.cpp @@ -60,13 +60,15 @@ namespace { -const size_t NUM_COMMANDS = 10; +const int NUM_COMMANDS = 10; } namespace { TEST_F(ServoRosFixture, testJointJog) { + waitForJointStates(); + auto joint_jog_publisher = servo_test_node_->create_publisher( "/servo_node/delta_joint_cmds", rclcpp::SystemDefaultsQoS()); @@ -78,12 +80,6 @@ TEST_F(ServoRosFixture, testJointJog) ASSERT_NE(state_count_, 0); - sensor_msgs::msg::JointState prev_state, curr_state; - { - std::lock_guard state_guard(joint_state_mutex_); - prev_state = joint_states_; - } - control_msgs::msg::JointJog jog_cmd; jog_cmd.header.frame_id = "panda_link0"; @@ -105,12 +101,7 @@ TEST_F(ServoRosFixture, testJointJog) rclcpp::sleep_for(std::chrono::milliseconds(200)); } - { - std::lock_guard state_guard(joint_state_mutex_); - curr_state = joint_states_; - } - - ASSERT_NE(curr_state.position[8], prev_state.position[8]); + 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)); @@ -119,6 +110,8 @@ TEST_F(ServoRosFixture, testJointJog) TEST_F(ServoRosFixture, testTwist) { + waitForJointStates(); + auto twist_publisher = servo_test_node_->create_publisher( "/servo_node/delta_twist_cmds", rclcpp::SystemDefaultsQoS()); @@ -129,12 +122,6 @@ TEST_F(ServoRosFixture, testTwist) ASSERT_NE(state_count_, 0); - sensor_msgs::msg::JointState prev_state, curr_state; - { - std::lock_guard state_guard(joint_state_mutex_); - prev_state = joint_states_; - } - geometry_msgs::msg::TwistStamped twist_cmd; twist_cmd.header.frame_id = "panda_link0"; // Planning frame twist_cmd.twist.linear.x = 0.0; @@ -153,12 +140,7 @@ TEST_F(ServoRosFixture, testTwist) rclcpp::sleep_for(std::chrono::milliseconds(200)); } - { - std::lock_guard state_guard(joint_state_mutex_); - curr_state = joint_states_; - } - - ASSERT_NE(curr_state.position[8], prev_state.position[8]); + ASSERT_GE(traj_count_, NUM_COMMANDS); moveit_servo::StatusCode status = status_; RCLCPP_INFO_STREAM(servo_test_node_->get_logger(), "Status after twist: " << static_cast(status)); @@ -167,6 +149,8 @@ TEST_F(ServoRosFixture, testTwist) TEST_F(ServoRosFixture, testPose) { + waitForJointStates(); + auto pose_publisher = servo_test_node_->create_publisher( "/servo_node/pose_target_cmds", rclcpp::SystemDefaultsQoS()); @@ -178,22 +162,16 @@ TEST_F(ServoRosFixture, testPose) geometry_msgs::msg::PoseStamped pose_cmd; pose_cmd.header.frame_id = "panda_link0"; // Planning frame - pose_cmd.pose.position.x = 0.5; + pose_cmd.pose.position.x = 0.3; pose_cmd.pose.position.y = 0.0; - pose_cmd.pose.position.z = 0.5; - pose_cmd.pose.orientation.w = 1.0; - pose_cmd.pose.orientation.x = 0.0; - pose_cmd.pose.orientation.y = 0.0; - pose_cmd.pose.orientation.z = 0.0; + pose_cmd.pose.position.z = 0.6; + pose_cmd.pose.orientation.x = 0.7; + pose_cmd.pose.orientation.y = -0.7; + pose_cmd.pose.orientation.z = -0.000014; + pose_cmd.pose.orientation.w = -0.0000015; ASSERT_NE(state_count_, 0); - sensor_msgs::msg::JointState prev_state, curr_state; - { - std::lock_guard state_guard(joint_state_mutex_); - prev_state = joint_states_; - } - size_t count = 0; while (rclcpp::ok() && count < NUM_COMMANDS) { @@ -203,12 +181,7 @@ TEST_F(ServoRosFixture, testPose) rclcpp::sleep_for(std::chrono::milliseconds(200)); } - { - std::lock_guard state_guard(joint_state_mutex_); - curr_state = joint_states_; - } - - ASSERT_NE(curr_state.position[8], prev_state.position[8]); + ASSERT_GE(traj_count_, NUM_COMMANDS); moveit_servo::StatusCode status = status_; RCLCPP_INFO_STREAM(servo_test_node_->get_logger(), "Status after pose: " << static_cast(status)); diff --git a/moveit_ros/moveit_servo/tests/test_utils.cpp b/moveit_ros/moveit_servo/tests/test_utils.cpp index c0ff8b5ca0..d54eb08be5 100644 --- a/moveit_ros/moveit_servo/tests/test_utils.cpp +++ b/moveit_ros/moveit_servo/tests/test_utils.cpp @@ -49,90 +49,6 @@ namespace { -TEST(ServoUtilsUnitTests, ApproachingSingularityScaling) -{ - using moveit::core::loadTestingRobotModel; - moveit::core::RobotModelPtr robot_model = loadTestingRobotModel("panda"); - moveit::core::RobotStatePtr robot_state = std::make_shared(robot_model); - - servo::Params servo_params; - servo_params.move_group_name = "panda_arm"; - const moveit::core::JointModelGroup* joint_model_group = - robot_state->getJointModelGroup(servo_params.move_group_name); - - Eigen::Vector cartesian_delta{ 0.005, 0.0, 0.0, 0.0, 0.0, 0.0 }; - // Home state - Eigen::Vector state_ready{ 0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785 }; - robot_state->setJointGroupActivePositions(joint_model_group, state_ready); - auto scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params); - ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::NO_WARNING); - - // Approach singularity - Eigen::Vector state_approaching_singularity{ 0.0, 0.334, 0.0, -1.177, 0.0, 1.510, 0.785 }; - robot_state->setJointGroupActivePositions(joint_model_group, state_approaching_singularity); - scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params); - ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::DECELERATE_FOR_APPROACHING_SINGULARITY); -} - -TEST(ServoUtilsUnitTests, HaltForSingularityScaling) -{ - using moveit::core::loadTestingRobotModel; - moveit::core::RobotModelPtr robot_model = loadTestingRobotModel("panda"); - moveit::core::RobotStatePtr robot_state = std::make_shared(robot_model); - - servo::Params servo_params; - servo_params.move_group_name = "panda_arm"; - const moveit::core::JointModelGroup* joint_model_group = - robot_state->getJointModelGroup(servo_params.move_group_name); - - Eigen::Vector cartesian_delta{ 0.005, 0.0, 0.0, 0.0, 0.0, 0.0 }; - - // Home state - Eigen::Vector state_ready{ 0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785 }; - robot_state->setJointGroupActivePositions(joint_model_group, state_ready); - auto scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params); - ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::NO_WARNING); - - // Move to singular state. - Eigen::Vector singular_state{ -0.0001, 0.5690, 0.0005, -0.7782, 0.0, 1.3453, 0.7845 }; - robot_state->setJointGroupActivePositions(joint_model_group, singular_state); - scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params); - ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::HALT_FOR_SINGULARITY); -} - -TEST(ServoUtilsUnitTests, LeavingSingularityScaling) -{ - using moveit::core::loadTestingRobotModel; - moveit::core::RobotModelPtr robot_model = loadTestingRobotModel("panda"); - moveit::core::RobotStatePtr robot_state = std::make_shared(robot_model); - - servo::Params servo_params; - servo_params.move_group_name = "panda_arm"; - const moveit::core::JointModelGroup* joint_model_group = - robot_state->getJointModelGroup(servo_params.move_group_name); - - Eigen::Vector cartesian_delta{ 0.005, 0.0, 0.0, 0.0, 0.0, 0.0 }; - - // Home state - Eigen::Vector state_ready{ 0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785 }; - robot_state->setJointGroupActivePositions(joint_model_group, state_ready); - auto scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params); - ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::NO_WARNING); - - // Approach singularity - Eigen::Vector state_approaching_singularity{ 0.0, 0.334, 0.0, -1.177, 0.0, 1.510, 0.785 }; - robot_state->setJointGroupActivePositions(joint_model_group, state_approaching_singularity); - scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params); - ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::DECELERATE_FOR_APPROACHING_SINGULARITY); - - // Move away from singularity - cartesian_delta(0) *= -1; - Eigen::Vector state_leaving_singularity{ 0.0, 0.3458, 0.0, -1.1424, 0.0, 1.4865, 0.785 }; - robot_state->setJointGroupActivePositions(joint_model_group, state_leaving_singularity); - scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params); - ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::DECELERATE_FOR_LEAVING_SINGULARITY); -} - TEST(ServoUtilsUnitTests, JointLimitVelocityScaling) { using moveit::core::loadTestingRobotModel; @@ -174,7 +90,7 @@ TEST(ServoUtilsUnitTests, validVector) TEST(ServoUtilsUnitTests, invalidVector) { Eigen::VectorXd invalid_vector(6); - invalid_vector << 0.0, 0.0, 0.0, 0.0, std::nan(""); + invalid_vector << 0.0, 0.0, 0.0, 0.0, std::nan(""), 0.0; EXPECT_FALSE(moveit_servo::isValidCommand(invalid_vector)); } @@ -189,7 +105,7 @@ TEST(ServoUtilsUnitTests, validTwist) TEST(ServoUtilsUnitTests, emptyTwistFrame) { Eigen::VectorXd invalid_vector(6); - invalid_vector << 0.0, 0.0, 0.0, 0.0, std::nan(""); + invalid_vector << 0.0, 0.0, 0.0, 0.0, std::nan(""), 0.0; moveit_servo::TwistCommand invalid_twist; invalid_twist.velocities = invalid_vector; EXPECT_FALSE(moveit_servo::isValidCommand(invalid_twist)); @@ -226,6 +142,93 @@ TEST(ServoUtilsUnitTests, validPose) EXPECT_TRUE(moveit_servo::isValidCommand(valid_pose)); } +TEST(ServoUtilsUnitTests, ApproachingSingularityScaling) +{ + using moveit::core::loadTestingRobotModel; + moveit::core::RobotModelPtr robot_model = loadTestingRobotModel("panda"); + moveit::core::RobotStatePtr robot_state = std::make_shared(robot_model); + + servo::Params servo_params; + servo_params.move_group_name = "panda_arm"; + const moveit::core::JointModelGroup* joint_model_group = + robot_state->getJointModelGroup(servo_params.move_group_name); + + rclcpp::sleep_for(std::chrono::milliseconds(500)); + Eigen::Vector cartesian_delta{ 0.005, 0.0, 0.0, 0.0, 0.0, 0.0 }; + // Home state + Eigen::Vector state_ready{ 0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785 }; + robot_state->setJointGroupActivePositions(joint_model_group, state_ready); + auto scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params); + ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::NO_WARNING); + + // Approach singularity + Eigen::Vector state_approaching_singularity{ 0.0, 0.334, 0.0, -1.177, 0.0, 1.510, 0.785 }; + robot_state->setJointGroupActivePositions(joint_model_group, state_approaching_singularity); + scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params); + ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::DECELERATE_FOR_APPROACHING_SINGULARITY); +} + +TEST(ServoUtilsUnitTests, HaltForSingularityScaling) +{ + using moveit::core::loadTestingRobotModel; + moveit::core::RobotModelPtr robot_model = loadTestingRobotModel("panda"); + moveit::core::RobotStatePtr robot_state = std::make_shared(robot_model); + + servo::Params servo_params; + servo_params.move_group_name = "panda_arm"; + const moveit::core::JointModelGroup* joint_model_group = + robot_state->getJointModelGroup(servo_params.move_group_name); + + rclcpp::sleep_for(std::chrono::milliseconds(500)); + Eigen::Vector cartesian_delta{ 0.005, 0.0, 0.0, 0.0, 0.0, 0.0 }; + + // Home state + Eigen::Vector state_ready{ 0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785 }; + robot_state->setJointGroupActivePositions(joint_model_group, state_ready); + auto scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params); + ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::NO_WARNING); + + // Move to singular state. + Eigen::Vector singular_state{ -0.0001, 0.5690, 0.0005, -0.7782, 0.0, 1.3453, 0.7845 }; + robot_state->setJointGroupActivePositions(joint_model_group, singular_state); + scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params); + ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::HALT_FOR_SINGULARITY); +} + +TEST(ServoUtilsUnitTests, LeavingSingularityScaling) +{ + using moveit::core::loadTestingRobotModel; + moveit::core::RobotModelPtr robot_model = loadTestingRobotModel("panda"); + moveit::core::RobotStatePtr robot_state = std::make_shared(robot_model); + + servo::Params servo_params; + servo_params.move_group_name = "panda_arm"; + const moveit::core::JointModelGroup* joint_model_group = + robot_state->getJointModelGroup(servo_params.move_group_name); + + rclcpp::sleep_for(std::chrono::milliseconds(500)); + Eigen::Vector cartesian_delta{ 0.005, 0.0, 0.0, 0.0, 0.0, 0.0 }; + + // Home state + Eigen::Vector state_ready{ 0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785 }; + robot_state->setJointGroupActivePositions(joint_model_group, state_ready); + auto scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params); + ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::NO_WARNING); + + // Approach singularity + Eigen::Vector state_approaching_singularity{ 0.0, 0.334, 0.0, -1.177, 0.0, 1.510, 0.785 }; + robot_state->setJointGroupActivePositions(joint_model_group, state_approaching_singularity); + scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params); + ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::DECELERATE_FOR_APPROACHING_SINGULARITY); + + // Move away from singularity + cartesian_delta(0) *= -1; + Eigen::Vector state_leaving_singularity{ 0.0, 0.3458, 0.0, -1.1424, 0.0, 1.4865, 0.785 }; + robot_state->setJointGroupActivePositions(joint_model_group, state_leaving_singularity); + scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params); + ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::DECELERATE_FOR_LEAVING_SINGULARITY); +} + } // namespace int main(int argc, char** argv)