From 027a063208178bff47a21a3bfe4281df391eb08c Mon Sep 17 00:00:00 2001 From: CihatAltiparmak Date: Mon, 1 Jul 2024 00:26:37 +0300 Subject: [PATCH 1/3] Started to implement the servo benchmarks --- CMakeLists.txt | 30 ++- .../scenario_servo_pipeline_test_cases.yaml | 6 + .../scenarios/scenario_servo_pipeline.hpp | 89 +++++++++ launch/servo_benchmark.launch.py | 171 ++++++++++++++++++ .../scenario_perception_pipeline.cpp | 4 +- src/scenarios/scenario_servo_pipeline.cpp | 141 +++++++++++++++ src/servo_benchmark_main.cpp | 11 ++ 7 files changed, 450 insertions(+), 2 deletions(-) create mode 100644 config/scenario_servo_pipeline_test_cases.yaml create mode 100644 include/moveit_middleware_benchmark/scenarios/scenario_servo_pipeline.hpp create mode 100644 launch/servo_benchmark.launch.py create mode 100644 src/scenarios/scenario_servo_pipeline.cpp create mode 100644 src/servo_benchmark_main.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 755d83f..ad2cf12 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,6 +14,9 @@ find_package(dynmsg REQUIRED) find_package(nav_msgs REQUIRED) find_package(ament_index_cpp REQUIRED) find_package(yaml-cpp REQUIRED) +find_package(moveit_msgs REQUIRED) +find_package(moveit_servo REQUIRED) +find_package(moveit_core REQUIRED) add_executable(benchmark_main src/benchmark_main.cpp src/scenarios/scenario_perception_pipeline.cpp) @@ -35,7 +38,32 @@ target_include_directories( target_link_libraries(benchmark_main PUBLIC "benchmark::benchmark" ${YAML_CPP_LIBRARIES}) -install(TARGETS benchmark_main DESTINATION lib/${PROJECT_NAME}) +add_executable(servo_benchmark_main src/servo_benchmark_main.cpp + src/scenarios/scenario_servo_pipeline.cpp) + +ament_target_dependencies( + servo_benchmark_main + PUBLIC + "moveit_ros_planning_interface" + "rclcpp" + "benchmark" + "dynmsg" + "nav_msgs" + "yaml-cpp" + "moveit_msgs" + "moveit_servo" + "moveit_core") + +target_include_directories( + servo_benchmark_main + PUBLIC $ + $) + +target_link_libraries(servo_benchmark_main PUBLIC "benchmark::benchmark" + ${YAML_CPP_LIBRARIES}) + +install(TARGETS benchmark_main servo_benchmark_main + DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}) diff --git a/config/scenario_servo_pipeline_test_cases.yaml b/config/scenario_servo_pipeline_test_cases.yaml new file mode 100644 index 0000000..8e45268 --- /dev/null +++ b/config/scenario_servo_pipeline_test_cases.yaml @@ -0,0 +1,6 @@ +test_cases: + - angular: + z : 1.2 + + - linear: + x : 1.2 \ No newline at end of file diff --git a/include/moveit_middleware_benchmark/scenarios/scenario_servo_pipeline.hpp b/include/moveit_middleware_benchmark/scenarios/scenario_servo_pipeline.hpp new file mode 100644 index 0000000..6cdb8e8 --- /dev/null +++ b/include/moveit_middleware_benchmark/scenarios/scenario_servo_pipeline.hpp @@ -0,0 +1,89 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace +{ +const std::string PLANNING_GROUP = "panda_arm"; +const std::string PACKAGE_SHARE_DIRECTORY = ament_index_cpp::get_package_share_directory("moveit_middleware_benchmark"); +const std::string TEST_CASES_YAML_FILE = + PACKAGE_SHARE_DIRECTORY + "/config/scenario_servo_pipeline_test_cases.yaml"; + +} // namespace + +namespace moveit { + +namespace middleware_benchmark { + +class ScenarioServoPipeline { +public: + rclcpp::Node::SharedPtr node_; + rclcpp::Client::SharedPtr servo_switch_command_type_client_; + rclcpp::Publisher::SharedPtr servo_command_pubisher_; + rclcpp::Subscription::SharedPtr servo_status_subscriber_; + std::shared_ptr servo_status_executor_; + std::thread servo_status_executor_thread_; + rclcpp::Duration server_timeout_; + // std::shared_ptr planning_scene_monitor_; + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + + ScenarioServoPipeline(rclcpp::Node::SharedPtr node, const rclcpp::Duration & server_timeout = rclcpp::Duration::from_seconds(-1)); + ~ScenarioServoPipeline(); + void switchCommandType( + const moveit_servo::CommandType & servo_command_type, + const rclcpp::Duration & timeout = rclcpp::Duration::from_seconds(-1)); + void sendTwistCommandToServo(const std::string & frame_id, const geometry_msgs::msg::Twist & target_twist); + moveit_servo::StatusCode getServoStatus(); + void runTestCase(const geometry_msgs::msg::Twist & test_case); + +private: + moveit_servo::StatusCode latest_servo_status_code_; +}; + +class ScenarioServoPipelineFixture : public benchmark::Fixture { +protected: + rclcpp::Node::SharedPtr node_; + std::shared_ptr servo_pipeline_; + + /* selected test case index for benchmarking */ + size_t selected_test_case_index_; + + /** the list of test cases to be tested */ + std::vector test_cases_ = {}; +public: + + ScenarioServoPipelineFixture(); + void SetUp(benchmark::State& /*state*/); + void TearDown(benchmark::State & /*state*/); + + geometry_msgs::msg::Twist selectTestCase(size_t test_case_index); + void createTestCases(); + void readTestCasesFromFile(const std::string & yaml_file_name); + geometry_msgs::msg::Twist getTestCaseFromYamlString(const std::string& yaml_string); +}; + + +} +} \ No newline at end of file diff --git a/launch/servo_benchmark.launch.py b/launch/servo_benchmark.launch.py new file mode 100644 index 0000000..2e420b8 --- /dev/null +++ b/launch/servo_benchmark.launch.py @@ -0,0 +1,171 @@ +import os +import launch +import launch_ros +from ament_index_python.packages import get_package_share_directory +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_param_builder import ParameterBuilder +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .joint_limits(file_path="config/hard_joint_limits.yaml") + .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="false" + ) + + # Get parameters for the Servo node + servo_params = { + "moveit_servo": ParameterBuilder("moveit_servo") + .yaml("config/panda_simulated_config.yaml") + .to_dict() + } + + # This sets the update rate and planning group name for the acceleration limiting filter. + acceleration_filter_update_period = {"update_period": 0.01} + planning_group_name = {"planning_group_name": "panda_arm"} + + # RViz + rviz_config_file = ( + get_package_share_directory("moveit_servo") + + "/config/demo_rviz_config_ros.rviz" + ) + rviz_node = launch_ros.actions.Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + ], + ) + + # ros2_control using FakeSystem as hardware + ros2_controllers_path = os.path.join( + get_package_share_directory("moveit_resources_panda_moveit_config"), + "config", + "ros2_controllers.yaml", + ) + ros2_control_node = launch_ros.actions.Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ros2_controllers_path], + remappings=[ + ("/controller_manager/robot_description", "/robot_description"), + ], + output="screen", + ) + + joint_state_broadcaster_spawner = launch_ros.actions.Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint_state_broadcaster", + "--controller-manager-timeout", + "300", + "--controller-manager", + "/controller_manager", + ], + ) + + panda_arm_controller_spawner = launch_ros.actions.Node( + package="controller_manager", + executable="spawner", + arguments=["panda_arm_controller", "-c", "/controller_manager"], + ) + + # 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, + acceleration_filter_update_period, + planning_group_name, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.joint_limits, + ], + condition=UnlessCondition(launch_as_standalone_node), + ), + launch_ros.descriptions.ComposableNode( + package="robot_state_publisher", + plugin="robot_state_publisher::RobotStatePublisher", + name="robot_state_publisher", + parameters=[moveit_config.robot_description], + ), + launch_ros.descriptions.ComposableNode( + package="tf2_ros", + plugin="tf2_ros::StaticTransformBroadcasterNode", + name="static_tf2_broadcaster", + 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( + package="moveit_servo", + executable="servo_node", + name="servo_node", + parameters=[ + servo_params, + acceleration_filter_update_period, + planning_group_name, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.joint_limits, + ], + output="screen", + condition=IfCondition(launch_as_standalone_node), + ) + + benchmark_node = launch_ros.actions.Node( + package="moveit_middleware_benchmark", + executable="servo_main", + name="servo_main_node", + parameters=[ + servo_params, + acceleration_filter_update_period, + planning_group_name, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.joint_limits, + {"selected_test_case_index": 0}, + ], + output="screen", + ) + + return launch.LaunchDescription( + [ + benchmark_node, + rviz_node, + ros2_control_node, + joint_state_broadcaster_spawner, + panda_arm_controller_spawner, + servo_node, + container, + ] + ) diff --git a/src/scenarios/scenario_perception_pipeline.cpp b/src/scenarios/scenario_perception_pipeline.cpp index c91f065..44d8cea 100644 --- a/src/scenarios/scenario_perception_pipeline.cpp +++ b/src/scenarios/scenario_perception_pipeline.cpp @@ -150,7 +150,9 @@ BENCHMARK_DEFINE_F(ScenarioPerceptionPipelineFixture, test_scenario_perception_p } } -BENCHMARK_REGISTER_F(ScenarioPerceptionPipelineFixture, test_scenario_perception_pipeline); +BENCHMARK_REGISTER_F(ScenarioPerceptionPipelineFixture, test_scenario_perception_pipeline) + ->MeasureProcessCPUTime() + ->UseRealTime(); } // namespace middleware_benchmark } // namespace moveit diff --git a/src/scenarios/scenario_servo_pipeline.cpp b/src/scenarios/scenario_servo_pipeline.cpp new file mode 100644 index 0000000..9caeacc --- /dev/null +++ b/src/scenarios/scenario_servo_pipeline.cpp @@ -0,0 +1,141 @@ +#include "moveit_middleware_benchmark/scenarios/scenario_servo_pipeline.hpp" + +namespace moveit { +namespace middleware_benchmark { + +ScenarioServoPipeline::ScenarioServoPipeline(rclcpp::Node::SharedPtr node, const rclcpp::Duration & server_timeout) + : node_(node), server_timeout_(server_timeout), latest_servo_status_code_(moveit_servo::StatusCode::NO_WARNING) +{ + servo_switch_command_type_client_ = node_->create_client("/servo_node/switch_command_type"); + if (servo_switch_command_type_client_->wait_for_service(server_timeout_.to_chrono>())) { + RCLCPP_INFO(node_->get_logger(), "SwitchCommandType Server is ready!"); + } else { + RCLCPP_ERROR(node_->get_logger(), "SwitchCommandType Server is not ready!"); + } + + servo_command_pubisher_ = node_->create_publisher("/servo_node/delta_twist_cmds", rclcpp::SystemDefaultsQoS()); + + servo_status_executor_ = std::make_shared(); + auto callback_group = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, + false /* don't spin with node executor */); + auto options = rclcpp::SubscriptionOptions(); + options.callback_group = callback_group; + servo_status_subscriber_ = node_->create_subscription("/servo_node/status", rclcpp::SystemDefaultsQoS(), + [this](moveit_msgs::msg::ServoStatus::SharedPtr new_servo_status_msg) { + latest_servo_status_code_ = static_cast(new_servo_status_msg->code); + }, options); + + servo_status_executor_->add_callback_group(callback_group, node_->get_node_base_interface()); + servo_status_executor_thread_ = std::thread([this]() {servo_status_executor_->spin();}); +} + +ScenarioServoPipeline::~ScenarioServoPipeline() { + if (servo_status_executor_) { + servo_status_executor_->cancel(); + } + + if (servo_status_executor_thread_.joinable()) { + servo_status_executor_thread_.join(); + } +} + +void ScenarioServoPipeline::runTestCase(const geometry_msgs::msg::Twist & test_case) { + rclcpp::WallRate servo_frequency(10); + while (getServoStatus() == moveit_servo::StatusCode::NO_WARNING) { + // RCLCPP_INFO(node_->get_logger(), "test case is %f", test_case.angular.z); + this->sendTwistCommandToServo("panda_link4", test_case); + servo_frequency.sleep(); + } +} + +void ScenarioServoPipeline::switchCommandType( + const moveit_servo::CommandType& servo_command_type, + const rclcpp::Duration & timeout) { + + auto servo_command_type_request = std::make_shared(); + servo_command_type_request->command_type = static_cast(servo_command_type); + + auto result = servo_switch_command_type_client_->async_send_request(servo_command_type_request); + if (rclcpp::spin_until_future_complete(node_, result, timeout.to_chrono>()) + == rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_INFO(node_->get_logger(), "Switched command!"); + } else { + RCLCPP_ERROR(node_->get_logger(), "There is a problem!"); + } +} + +moveit_servo::StatusCode ScenarioServoPipeline::getServoStatus() { + return latest_servo_status_code_; +} + +void ScenarioServoPipeline::sendTwistCommandToServo(const std::string & frame_id, const geometry_msgs::msg::Twist & target_twist) { + geometry_msgs::msg::TwistStamped target_twist_stamped; + target_twist_stamped.twist = target_twist; + target_twist_stamped.header.stamp = node_->now(); + target_twist_stamped.header.frame_id = frame_id; + + servo_command_pubisher_->publish(target_twist_stamped); +} + +ScenarioServoPipelineFixture::ScenarioServoPipelineFixture() { + createTestCases(); +} + +void ScenarioServoPipelineFixture::SetUp(benchmark::State& /*state*/) { + if (node_.use_count() == 0) { + node_ = std::make_shared("test_servo_pipeline_node", + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)); + node_->get_parameter("selected_test_case_index", selected_test_case_index_); + RCLCPP_INFO(node_->get_logger(), "SELECTED TEST CASE : %ld", selected_test_case_index_); + } +} + +geometry_msgs::msg::Twist ScenarioServoPipelineFixture::selectTestCase(size_t test_case_index) +{ + return test_cases_.at(test_case_index); +} + +void ScenarioServoPipelineFixture::createTestCases() { + readTestCasesFromFile(TEST_CASES_YAML_FILE); +} + +void ScenarioServoPipelineFixture::readTestCasesFromFile(const std::string & yaml_file_name) { + YAML::Node config = YAML::LoadFile(yaml_file_name.c_str()); + for (YAML::const_iterator it = config["test_cases"].begin(); it != config["test_cases"].end(); ++it) + { + const std::string yaml_string = dynmsg::yaml_to_string(*it); + geometry_msgs::msg::Twist test_case = getTestCaseFromYamlString(yaml_string); + + test_cases_.push_back(test_case); + } +} + +geometry_msgs::msg::Twist ScenarioServoPipelineFixture::getTestCaseFromYamlString(const std::string& yaml_string) { + geometry_msgs::msg::Twist twist_msg; + void* ros_message = reinterpret_cast(&twist_msg); + + dynmsg::cpp::yaml_and_typeinfo_to_rosmsg(dynmsg::cpp::get_type_info({ "geometry_msgs", "Twist" }), yaml_string, ros_message); + + return twist_msg; +} + +void ScenarioServoPipelineFixture::TearDown(benchmark::State& /*state*/) { + +} + +BENCHMARK_DEFINE_F(ScenarioServoPipelineFixture, test_scenario_servo_pipeline)(::benchmark::State& st) { + + auto selected_test_case = selectTestCase(selected_test_case_index_); + + servo_pipeline_ = std::make_shared(node_); + servo_pipeline_->switchCommandType(moveit_servo::CommandType::TWIST); + + for (auto _ : st) { + servo_pipeline_->runTestCase(selected_test_case); + } +} + +BENCHMARK_REGISTER_F(ScenarioServoPipelineFixture, test_scenario_servo_pipeline)->MeasureProcessCPUTime()->UseRealTime(); + +} // namespace middleware_benchmark +} // namespace moveit \ No newline at end of file diff --git a/src/servo_benchmark_main.cpp b/src/servo_benchmark_main.cpp new file mode 100644 index 0000000..d51acee --- /dev/null +++ b/src/servo_benchmark_main.cpp @@ -0,0 +1,11 @@ +#include "moveit_middleware_benchmark/scenarios/scenario_servo_pipeline.hpp" + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + benchmark::Initialize(&argc, argv); + benchmark::RunSpecifiedBenchmarks(); + benchmark::Shutdown(); + rclcpp::shutdown(); + return 0; +} From 2dfd38cd64ae5ca92361f3087739e4a4c60256c4 Mon Sep 17 00:00:00 2001 From: CihatAltiparmak Date: Sun, 7 Jul 2024 04:29:45 +0300 Subject: [PATCH 2/3] Added set-to-default ability to benchmark - ControllerManager and other controller_interfaces is started from benchmark executable instead of starting from launch file so that robot can be set to default joint position etc. --- .../scenario_servo_pipeline_test_cases.yaml | 4 +- .../scenarios/scenario_servo_pipeline.hpp | 105 ++++--- launch/servo_benchmark.launch.py | 11 +- src/scenarios/scenario_servo_pipeline.cpp | 275 ++++++++++++------ 4 files changed, 263 insertions(+), 132 deletions(-) diff --git a/config/scenario_servo_pipeline_test_cases.yaml b/config/scenario_servo_pipeline_test_cases.yaml index 8e45268..b44ec3e 100644 --- a/config/scenario_servo_pipeline_test_cases.yaml +++ b/config/scenario_servo_pipeline_test_cases.yaml @@ -1,6 +1,6 @@ test_cases: - angular: z : 1.2 - + - linear: - x : 1.2 \ No newline at end of file + x : 1.2 diff --git a/include/moveit_middleware_benchmark/scenarios/scenario_servo_pipeline.hpp b/include/moveit_middleware_benchmark/scenarios/scenario_servo_pipeline.hpp index 6cdb8e8..273c1ea 100644 --- a/include/moveit_middleware_benchmark/scenarios/scenario_servo_pipeline.hpp +++ b/include/moveit_middleware_benchmark/scenarios/scenario_servo_pipeline.hpp @@ -24,66 +24,87 @@ #include #include +#include +#include +#include +#include + namespace { const std::string PLANNING_GROUP = "panda_arm"; const std::string PACKAGE_SHARE_DIRECTORY = ament_index_cpp::get_package_share_directory("moveit_middleware_benchmark"); -const std::string TEST_CASES_YAML_FILE = - PACKAGE_SHARE_DIRECTORY + "/config/scenario_servo_pipeline_test_cases.yaml"; +const std::string TEST_CASES_YAML_FILE = PACKAGE_SHARE_DIRECTORY + "/config/scenario_servo_pipeline_test_cases.yaml"; } // namespace -namespace moveit { +namespace moveit +{ -namespace middleware_benchmark { +namespace middleware_benchmark +{ -class ScenarioServoPipeline { +// TODO @CihatAltiparmak : Move this class definition into separate place like utils directory +class ProcessUtils +{ public: - rclcpp::Node::SharedPtr node_; - rclcpp::Client::SharedPtr servo_switch_command_type_client_; - rclcpp::Publisher::SharedPtr servo_command_pubisher_; - rclcpp::Subscription::SharedPtr servo_status_subscriber_; - std::shared_ptr servo_status_executor_; - std::thread servo_status_executor_thread_; - rclcpp::Duration server_timeout_; - // std::shared_ptr planning_scene_monitor_; - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - - ScenarioServoPipeline(rclcpp::Node::SharedPtr node, const rclcpp::Duration & server_timeout = rclcpp::Duration::from_seconds(-1)); - ~ScenarioServoPipeline(); - void switchCommandType( - const moveit_servo::CommandType & servo_command_type, - const rclcpp::Duration & timeout = rclcpp::Duration::from_seconds(-1)); - void sendTwistCommandToServo(const std::string & frame_id, const geometry_msgs::msg::Twist & target_twist); - moveit_servo::StatusCode getServoStatus(); - void runTestCase(const geometry_msgs::msg::Twist & test_case); + ProcessUtils(rclcpp::Node::SharedPtr); + void startROSControllers(); + void killROSControllers(); + +private: + pid_t ros2_controller_pid_; + rclcpp::Node::SharedPtr node_; +}; + +class ScenarioServoPipeline +{ +public: + rclcpp::Node::SharedPtr node_; + rclcpp::Client::SharedPtr servo_switch_command_type_client_; + rclcpp::Publisher::SharedPtr servo_command_pubisher_; + rclcpp::Subscription::SharedPtr servo_status_subscriber_; + std::shared_ptr servo_status_executor_; + std::thread servo_status_executor_thread_; + rclcpp::Duration server_timeout_; + + ScenarioServoPipeline(rclcpp::Node::SharedPtr node, + const rclcpp::Duration& server_timeout = rclcpp::Duration::from_seconds(-1)); + ~ScenarioServoPipeline(); + void switchCommandType(const moveit_servo::CommandType& servo_command_type, + const rclcpp::Duration& timeout = rclcpp::Duration::from_seconds(-1)); + void sendTwistCommandToServo(const std::string& frame_id, const geometry_msgs::msg::Twist& target_twist); + moveit_servo::StatusCode getServoStatus(); + void runTestCase(const geometry_msgs::msg::Twist& test_case); private: moveit_servo::StatusCode latest_servo_status_code_; + std::string tested_link_; }; -class ScenarioServoPipelineFixture : public benchmark::Fixture { +class ScenarioServoPipelineFixture : public benchmark::Fixture +{ protected: - rclcpp::Node::SharedPtr node_; - std::shared_ptr servo_pipeline_; + rclcpp::Node::SharedPtr node_; + std::shared_ptr servo_pipeline_; - /* selected test case index for benchmarking */ - size_t selected_test_case_index_; + /* selected test case index for benchmarking */ + size_t selected_test_case_index_; - /** the list of test cases to be tested */ - std::vector test_cases_ = {}; -public: + /** the list of test cases to be tested */ + std::vector test_cases_ = {}; + pid_t pid_; + ProcessUtils process_utils_; - ScenarioServoPipelineFixture(); - void SetUp(benchmark::State& /*state*/); - void TearDown(benchmark::State & /*state*/); - - geometry_msgs::msg::Twist selectTestCase(size_t test_case_index); - void createTestCases(); - void readTestCasesFromFile(const std::string & yaml_file_name); - geometry_msgs::msg::Twist getTestCaseFromYamlString(const std::string& yaml_string); +public: + ScenarioServoPipelineFixture(); + void SetUp(benchmark::State& /*state*/); + void TearDown(benchmark::State& /*state*/); + + geometry_msgs::msg::Twist selectTestCase(size_t test_case_index); + void createTestCases(); + void readTestCasesFromFile(const std::string& yaml_file_name); + geometry_msgs::msg::Twist getTestCaseFromYamlString(const std::string& yaml_string); }; - -} -} \ No newline at end of file +} // namespace middleware_benchmark +} // namespace moveit diff --git a/launch/servo_benchmark.launch.py b/launch/servo_benchmark.launch.py index 2e420b8..63bb33b 100644 --- a/launch/servo_benchmark.launch.py +++ b/launch/servo_benchmark.launch.py @@ -37,6 +37,7 @@ def generate_launch_description(): get_package_share_directory("moveit_servo") + "/config/demo_rviz_config_ros.rviz" ) + rviz_node = launch_ros.actions.Node( package="rviz2", executable="rviz2", @@ -143,7 +144,7 @@ def generate_launch_description(): benchmark_node = launch_ros.actions.Node( package="moveit_middleware_benchmark", - executable="servo_main", + executable="servo_benchmark_main", name="servo_main_node", parameters=[ servo_params, @@ -154,17 +155,19 @@ def generate_launch_description(): moveit_config.robot_description_kinematics, moveit_config.joint_limits, {"selected_test_case_index": 0}, + {"selected_link": "panda_link4"}, ], output="screen", + on_exit=launch.actions.Shutdown(), ) return launch.LaunchDescription( [ benchmark_node, rviz_node, - ros2_control_node, - joint_state_broadcaster_spawner, - panda_arm_controller_spawner, + # ros2_control_node, + # joint_state_broadcaster_spawner, + # panda_arm_controller_spawner, servo_node, container, ] diff --git a/src/scenarios/scenario_servo_pipeline.cpp b/src/scenarios/scenario_servo_pipeline.cpp index 9caeacc..8db27f7 100644 --- a/src/scenarios/scenario_servo_pipeline.cpp +++ b/src/scenarios/scenario_servo_pipeline.cpp @@ -1,93 +1,199 @@ #include "moveit_middleware_benchmark/scenarios/scenario_servo_pipeline.hpp" +#include +#include +#include +#include +#include -namespace moveit { -namespace middleware_benchmark { +namespace moveit +{ +namespace middleware_benchmark +{ -ScenarioServoPipeline::ScenarioServoPipeline(rclcpp::Node::SharedPtr node, const rclcpp::Duration & server_timeout) - : node_(node), server_timeout_(server_timeout), latest_servo_status_code_(moveit_servo::StatusCode::NO_WARNING) +ProcessUtils::ProcessUtils(rclcpp::Node::SharedPtr node) : node_(node) { - servo_switch_command_type_client_ = node_->create_client("/servo_node/switch_command_type"); - if (servo_switch_command_type_client_->wait_for_service(server_timeout_.to_chrono>())) { - RCLCPP_INFO(node_->get_logger(), "SwitchCommandType Server is ready!"); - } else { - RCLCPP_ERROR(node_->get_logger(), "SwitchCommandType Server is not ready!"); - } +} - servo_command_pubisher_ = node_->create_publisher("/servo_node/delta_twist_cmds", rclcpp::SystemDefaultsQoS()); +void ProcessUtils::startROSControllers() +{ + pid_t pid = fork(); + if (pid == 0) + { + // child process + // TODO @CihatAltiparmak : Move this command arguments into parameter file + const std::string PANDA_PACKAGE_SHARE_DIRECTORY = + ament_index_cpp::get_package_share_directory("moveit_resources_panda_moveit_config"); + + const std::string ros2_control_param_file = PANDA_PACKAGE_SHARE_DIRECTORY + "/config/ros2_controllers.yaml"; + char* ros2_control_param_file_cstr = new char[ros2_control_param_file.length() + 1]; + std::strcpy(ros2_control_param_file_cstr, ros2_control_param_file.c_str()); + + char* command[] = { "/opt/ros/rolling/lib/controller_manager/ros2_control_node", + "--ros-args", + "--params-file", + ros2_control_param_file_cstr, + "--remap", + "/controller_manager/robot_description:=/robot_description", + NULL }; + + execvp("/opt/ros/rolling/lib/controller_manager/ros2_control_node", command); + + RCLCPP_FATAL(node_->get_logger(), "A problem occurred while running controller manager node in child node!"); + exit(1); + } + else if (pid > 0) + { + ros2_controller_pid_ = pid; - servo_status_executor_ = std::make_shared(); - auto callback_group = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, - false /* don't spin with node executor */); - auto options = rclcpp::SubscriptionOptions(); - options.callback_group = callback_group; - servo_status_subscriber_ = node_->create_subscription("/servo_node/status", rclcpp::SystemDefaultsQoS(), - [this](moveit_msgs::msg::ServoStatus::SharedPtr new_servo_status_msg) { - latest_servo_status_code_ = static_cast(new_servo_status_msg->code); - }, options); + // TODO @CihatAltiparmak : Move this command arguments into parameter file + std::system("ros2 run controller_manager spawner joint_state_broadcaster --controller-manager-timeout 3000 " + "--controller-manager /controller_manager"); + std::system("ros2 run controller_manager spawner panda_arm_controller --controller-manager-timeout 3000 " + "--controller-manager /controller_manager"); + } + else + { + RCLCPP_FATAL(node_->get_logger(), + "A problem occurred while creating a child process in order to run controller manager!"); + exit(1); + } +} - servo_status_executor_->add_callback_group(callback_group, node_->get_node_base_interface()); - servo_status_executor_thread_ = std::thread([this]() {servo_status_executor_->spin();}); +void ProcessUtils::killROSControllers() +{ + RCLCPP_INFO( + node_->get_logger(), + "ProcessUtils::killROSControllers() started to kill the child process controller_manager is run with pid %d", + ros2_controller_pid_); + kill(ros2_controller_pid_, SIGTERM); + int status; + std::cout << "[ProcessUtils::killROSControllers] [ waiting for ] " << ros2_controller_pid_ << std::endl; + waitpid(ros2_controller_pid_, &status, 0); + RCLCPP_INFO(node_->get_logger(), + "ProcessUtils::killROSControllers() waiting the process with %d pid is finished! Got status : %d", + ros2_controller_pid_, status); } -ScenarioServoPipeline::~ScenarioServoPipeline() { - if (servo_status_executor_) { - servo_status_executor_->cancel(); - } +ScenarioServoPipeline::ScenarioServoPipeline(rclcpp::Node::SharedPtr node, const rclcpp::Duration& server_timeout) + : node_(node), server_timeout_(server_timeout), latest_servo_status_code_(moveit_servo::StatusCode::NO_WARNING) +{ + node_->get_parameter("selected_link", tested_link_); + ; - if (servo_status_executor_thread_.joinable()) { - servo_status_executor_thread_.join(); - } + servo_switch_command_type_client_ = + node_->create_client("/servo_node/switch_command_type"); + if (servo_switch_command_type_client_->wait_for_service(server_timeout_.to_chrono>())) + { + RCLCPP_INFO(node_->get_logger(), "SwitchCommandType Server is ready!"); + } + else + { + RCLCPP_ERROR(node_->get_logger(), "SwitchCommandType Server is not ready!"); + } + + servo_command_pubisher_ = node_->create_publisher("/servo_node/delta_twist_cmds", + rclcpp::SystemDefaultsQoS()); + + servo_status_executor_ = std::make_shared(); + auto callback_group = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, + false /* don't spin with node executor */); + auto options = rclcpp::SubscriptionOptions(); + options.callback_group = callback_group; + servo_status_subscriber_ = node_->create_subscription( + "/servo_node/status", rclcpp::SystemDefaultsQoS(), + [this](moveit_msgs::msg::ServoStatus::SharedPtr new_servo_status_msg) { + latest_servo_status_code_ = static_cast(new_servo_status_msg->code); + }, + options); + + servo_status_executor_->add_callback_group(callback_group, node_->get_node_base_interface()); + servo_status_executor_thread_ = std::thread([this]() { servo_status_executor_->spin(); }); } -void ScenarioServoPipeline::runTestCase(const geometry_msgs::msg::Twist & test_case) { - rclcpp::WallRate servo_frequency(10); - while (getServoStatus() == moveit_servo::StatusCode::NO_WARNING) { - // RCLCPP_INFO(node_->get_logger(), "test case is %f", test_case.angular.z); - this->sendTwistCommandToServo("panda_link4", test_case); - servo_frequency.sleep(); - } +ScenarioServoPipeline::~ScenarioServoPipeline() +{ + if (servo_status_executor_) + { + servo_status_executor_->cancel(); + } + + if (servo_status_executor_thread_.joinable()) + { + servo_status_executor_thread_.join(); + } } -void ScenarioServoPipeline::switchCommandType( - const moveit_servo::CommandType& servo_command_type, - const rclcpp::Duration & timeout) { +void ScenarioServoPipeline::runTestCase(const geometry_msgs::msg::Twist& test_case) +{ + rclcpp::WallRate servo_frequency(10); + while (getServoStatus() == moveit_servo::StatusCode::NO_WARNING) + { + this->sendTwistCommandToServo(tested_link_, test_case); + servo_frequency.sleep(); + } +} - auto servo_command_type_request = std::make_shared(); - servo_command_type_request->command_type = static_cast(servo_command_type); +void ScenarioServoPipeline::switchCommandType(const moveit_servo::CommandType& servo_command_type, + const rclcpp::Duration& timeout) +{ + auto servo_command_type_request = std::make_shared(); + servo_command_type_request->command_type = static_cast(servo_command_type); - auto result = servo_switch_command_type_client_->async_send_request(servo_command_type_request); - if (rclcpp::spin_until_future_complete(node_, result, timeout.to_chrono>()) - == rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_INFO(node_->get_logger(), "Switched command!"); - } else { - RCLCPP_ERROR(node_->get_logger(), "There is a problem!"); - } + auto result = servo_switch_command_type_client_->async_send_request(servo_command_type_request); + if (rclcpp::spin_until_future_complete(node_, result, timeout.to_chrono>()) == + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_INFO(node_->get_logger(), "Got the response from servo switch command server!"); + } + else + { + RCLCPP_ERROR(node_->get_logger(), "A problem occurred while sending switch command to servo node!"); + } } -moveit_servo::StatusCode ScenarioServoPipeline::getServoStatus() { - return latest_servo_status_code_; +moveit_servo::StatusCode ScenarioServoPipeline::getServoStatus() +{ + return latest_servo_status_code_; } -void ScenarioServoPipeline::sendTwistCommandToServo(const std::string & frame_id, const geometry_msgs::msg::Twist & target_twist) { - geometry_msgs::msg::TwistStamped target_twist_stamped; - target_twist_stamped.twist = target_twist; - target_twist_stamped.header.stamp = node_->now(); - target_twist_stamped.header.frame_id = frame_id; +void ScenarioServoPipeline::sendTwistCommandToServo(const std::string& link, + const geometry_msgs::msg::Twist& target_twist) +{ + geometry_msgs::msg::TwistStamped target_twist_stamped; + target_twist_stamped.twist = target_twist; + target_twist_stamped.header.stamp = node_->now(); + target_twist_stamped.header.frame_id = link; + + servo_command_pubisher_->publish(target_twist_stamped); +} - servo_command_pubisher_->publish(target_twist_stamped); +ScenarioServoPipelineFixture::ScenarioServoPipelineFixture() +{ + createTestCases(); } -ScenarioServoPipelineFixture::ScenarioServoPipelineFixture() { - createTestCases(); +void ScenarioServoPipelineFixture::SetUp(benchmark::State& /*state*/) +{ + process_utils_.startROSControllers(); + + if (node_.use_count() == 0) + { + node_ = std::make_shared("test_servo_pipeline_node", + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)); + // TODO @CihatAltiparmak : assign initial value for ros arguments + // for example, selected_test_case_index_ = 0; + node_->get_parameter("selected_test_case_index", selected_test_case_index_); + RCLCPP_INFO(node_->get_logger(), "SELECTED TEST CASE INDEX: %ld", selected_test_case_index_); + + process_utils_ = ProcessUtils(node_); + } } -void ScenarioServoPipelineFixture::SetUp(benchmark::State& /*state*/) { - if (node_.use_count() == 0) { - node_ = std::make_shared("test_servo_pipeline_node", - rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)); - node_->get_parameter("selected_test_case_index", selected_test_case_index_); - RCLCPP_INFO(node_->get_logger(), "SELECTED TEST CASE : %ld", selected_test_case_index_); - } +void ScenarioServoPipelineFixture::TearDown(benchmark::State& /*state*/) +{ + process_utils_.killROSControllers(); + RCLCPP_INFO(node_->get_logger(), "ScenarioServoPipelineFixture::TearDown() ROS 2 Controller is terminated!"); + std::cout << "ROS 2 Controller is terminated!" << std::endl; } geometry_msgs::msg::Twist ScenarioServoPipelineFixture::selectTestCase(size_t test_case_index) @@ -95,11 +201,13 @@ geometry_msgs::msg::Twist ScenarioServoPipelineFixture::selectTestCase(size_t te return test_cases_.at(test_case_index); } -void ScenarioServoPipelineFixture::createTestCases() { - readTestCasesFromFile(TEST_CASES_YAML_FILE); +void ScenarioServoPipelineFixture::createTestCases() +{ + readTestCasesFromFile(TEST_CASES_YAML_FILE); } -void ScenarioServoPipelineFixture::readTestCasesFromFile(const std::string & yaml_file_name) { +void ScenarioServoPipelineFixture::readTestCasesFromFile(const std::string& yaml_file_name) +{ YAML::Node config = YAML::LoadFile(yaml_file_name.c_str()); for (YAML::const_iterator it = config["test_cases"].begin(); it != config["test_cases"].end(); ++it) { @@ -110,32 +218,31 @@ void ScenarioServoPipelineFixture::readTestCasesFromFile(const std::string & yam } } -geometry_msgs::msg::Twist ScenarioServoPipelineFixture::getTestCaseFromYamlString(const std::string& yaml_string) { +geometry_msgs::msg::Twist ScenarioServoPipelineFixture::getTestCaseFromYamlString(const std::string& yaml_string) +{ geometry_msgs::msg::Twist twist_msg; void* ros_message = reinterpret_cast(&twist_msg); - dynmsg::cpp::yaml_and_typeinfo_to_rosmsg(dynmsg::cpp::get_type_info({ "geometry_msgs", "Twist" }), yaml_string, ros_message); + dynmsg::cpp::yaml_and_typeinfo_to_rosmsg(dynmsg::cpp::get_type_info({ "geometry_msgs", "Twist" }), yaml_string, + ros_message); return twist_msg; } -void ScenarioServoPipelineFixture::TearDown(benchmark::State& /*state*/) { - -} - -BENCHMARK_DEFINE_F(ScenarioServoPipelineFixture, test_scenario_servo_pipeline)(::benchmark::State& st) { - - auto selected_test_case = selectTestCase(selected_test_case_index_); +BENCHMARK_DEFINE_F(ScenarioServoPipelineFixture, test_scenario_servo_pipeline)(::benchmark::State& st) +{ + auto selected_test_case = selectTestCase(selected_test_case_index_); - servo_pipeline_ = std::make_shared(node_); - servo_pipeline_->switchCommandType(moveit_servo::CommandType::TWIST); + servo_pipeline_ = std::make_shared(node_); + servo_pipeline_->switchCommandType(moveit_servo::CommandType::TWIST); - for (auto _ : st) { - servo_pipeline_->runTestCase(selected_test_case); - } + for (auto _ : st) + { + servo_pipeline_->runTestCase(selected_test_case); + } } BENCHMARK_REGISTER_F(ScenarioServoPipelineFixture, test_scenario_servo_pipeline)->MeasureProcessCPUTime()->UseRealTime(); -} // namespace middleware_benchmark -} // namespace moveit \ No newline at end of file +} // namespace middleware_benchmark +} // namespace moveit From 3d94f78713ff063d70b076d15d9999541cadcf88 Mon Sep 17 00:00:00 2001 From: CihatAltiparmak Date: Mon, 8 Jul 2024 18:14:27 +0300 Subject: [PATCH 3/3] Fixed compilation error for ProcessUtils --- .../scenarios/scenario_servo_pipeline.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/include/moveit_middleware_benchmark/scenarios/scenario_servo_pipeline.hpp b/include/moveit_middleware_benchmark/scenarios/scenario_servo_pipeline.hpp index 273c1ea..233f771 100644 --- a/include/moveit_middleware_benchmark/scenarios/scenario_servo_pipeline.hpp +++ b/include/moveit_middleware_benchmark/scenarios/scenario_servo_pipeline.hpp @@ -47,6 +47,9 @@ namespace middleware_benchmark class ProcessUtils { public: + ProcessUtils() + { + } ProcessUtils(rclcpp::Node::SharedPtr); void startROSControllers(); void killROSControllers();