Started to implement the servo benchmarks

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 $<INSTALL_INTERFACE:include>
+         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>)
+
+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})
 

config/scenario_servo_pipeline_test_cases.yaml

@@ -0,0 +1,6 @@
+test_cases:
+  - angular:
+      z : 1.2
+
+  - linear:
+      x : 1.2

include/moveit_middleware_benchmark/scenarios/scenario_servo_pipeline.hpp

@@ -0,0 +1,89 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+#include <moveit_msgs/srv/servo_command_type.hpp>
+#include <moveit_msgs/msg/servo_status.hpp>
+#include <moveit_servo/utils/datatypes.hpp>
+#include <thread>
+#include <chrono>
+#include <memory>
+#include <benchmark/benchmark.h>
+#include <thread>
+
+#include <dynmsg/msg_parser.hpp>
+#include <dynmsg/typesupport.hpp>
+#include <dynmsg/yaml_utils.hpp>
+
+#include <ament_index_cpp/get_package_share_directory.hpp>
+
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/robot_state/robot_state.h>
+#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
+#include <moveit/planning_scene/planning_scene.h>
+
+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<moveit_msgs::srv::ServoCommandType>::SharedPtr servo_switch_command_type_client_;
+    rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr servo_command_pubisher_;
+    rclcpp::Subscription<moveit_msgs::msg::ServoStatus>::SharedPtr servo_status_subscriber_;
+    std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> servo_status_executor_;
+    std::thread servo_status_executor_thread_;
+    rclcpp::Duration server_timeout_;
+    // std::shared_ptr<planning_scene_monitor::PlanningSceneMonitor> 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<moveit::middleware_benchmark::ScenarioServoPipeline> servo_pipeline_;
+
+    /* selected test case index for benchmarking */
+    size_t selected_test_case_index_;
+
+    /** the list of test cases to be tested */
+    std::vector<geometry_msgs::msg::Twist> 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);
+};
+
+
+}
+}

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,
+        ]
+    )

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