diff --git a/CMakeLists.txt b/CMakeLists.txt index 3990151..7e06765 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,6 +16,8 @@ find_package(ament_index_cpp REQUIRED) find_package(yaml-cpp REQUIRED) find_package(std_msgs REQUIRED) find_package(example_interfaces REQUIRED) +find_package(moveit_task_constructor_core REQUIRED) +find_package(generate_parameter_library REQUIRED) add_executable( scenario_perception_pipeline_benchmark_main @@ -61,11 +63,42 @@ target_include_directories( $) target_link_libraries(scenario_basic_service_client_benchmark_main - PUBLIC "benchmark::benchmark" ${YAML_CPP_LIBRARIES}) + PUBLIC benchmark::benchmark ${YAML_CPP_LIBRARIES}) + +add_executable( + scenario_moveit_task_constructor_benchmark_main + src/scenario_moveit_task_constructor_benchmark_main.cpp + src/scenarios/moveit_task_constructor/scenario_moveit_task_constructor.cpp + src/scenarios/moveit_task_constructor/pick_place_task.cpp) + +ament_target_dependencies( + scenario_moveit_task_constructor_benchmark_main + PUBLIC + moveit_ros_planning_interface + rclcpp + benchmark + std_msgs + example_interfaces + moveit_task_constructor_core) + +target_include_directories( + scenario_moveit_task_constructor_benchmark_main + PUBLIC $ + $) + +generate_parameter_library( + pick_place_demo_parameters + src/scenarios/moveit_task_constructor/pick_place_demo_parameters.yaml) + +target_link_libraries( + scenario_moveit_task_constructor_benchmark_main + PUBLIC benchmark::benchmark ${YAML_CPP_LIBRARIES} pick_place_demo_parameters) -install(TARGETS scenario_perception_pipeline_benchmark_main - scenario_basic_service_client_benchmark_main - DESTINATION lib/moveit_middleware_benchmark) +install( + TARGETS scenario_perception_pipeline_benchmark_main + scenario_basic_service_client_benchmark_main + scenario_moveit_task_constructor_benchmark_main + DESTINATION lib/moveit_middleware_benchmark) install(DIRECTORY launch config DESTINATION share/moveit_middleware_benchmark) diff --git a/README.md b/README.md index 69c3eed..e104243 100644 --- a/README.md +++ b/README.md @@ -10,6 +10,7 @@ This middleware benchmark tool aims to measure middleware effects on various sce * [Perception Pipeline](./docs/scenarios/perception_pipeline_benchmark.md) * [Basic Service Client Works](./docs/scenarios/basic_service_client_benchmark.md) +* [Moveit Task Constructor Pick-Place Task](./docs/scenarios/moveit_task_constructor_benchmark.md) ## Getting Started diff --git a/config/pick_place_demo_configs.yaml b/config/pick_place_demo_configs.yaml new file mode 100644 index 0000000..1c2b69b --- /dev/null +++ b/config/pick_place_demo_configs.yaml @@ -0,0 +1,48 @@ +/**: + ros__parameters: + # Total planning attempts + max_solutions: 10 + + # Planning group and link names + arm_group_name: "panda_arm" + eef_name: "hand" + hand_group_name: "hand" + hand_frame: "panda_link8" + + # Poses + hand_open_pose: "open" + hand_close_pose: "close" + arm_home_pose: "ready" + + # Scene frames + world_frame: "world" + table_reference_frame: "world" + object_reference_frame: "world" + surface_link: "table" + + # Collision object for picking + # CYLINDER object specifications + object_name: "object" + object_dimensions: [0.25, 0.02] # [height, radius] + object_pose: [0.5, -0.25, 0.0, 0.0, 0.0, 0.0] + + # Table model + spawn_table: true + table_name: "table" + table_dimensions: [0.4, 0.5, 0.1] # [length, width, height] + table_pose: [0.5, -0.25, 0.0, 0.0, 0.0, 0.0] + + # Gripper grasp frame transform [x,y,z,r,p,y] + grasp_frame_transform: [0.0, 0.0, 0.1, 1.571, 0.785, 1.571] + + # Place pose [x,y,z,r,p,y] + place_pose: [0.6, -0.15, 0.0, 0.0, 0.0, 0.0] + place_surface_offset: 0.0001 # place offset from table + + # Valid distance range when approaching an object for picking + approach_object_min_dist: 0.1 + approach_object_max_dist: 0.15 + + # Valid height range when lifting an object after pick + lift_object_min_dist: 0.01 + lift_object_max_dist: 0.1 diff --git a/docs/how_to_run.md b/docs/how_to_run.md index 3767bf8..fee4f4a 100644 --- a/docs/how_to_run.md +++ b/docs/how_to_run.md @@ -94,3 +94,7 @@ For instance, the selected test_case includes 20 goal poses. These 20 goals is s This benchmark measures the total elapsed time based on the time interval between sending the request by the client to the server and getting the response of server. This benchmark utilizes the [ros2/demos](https://github.com/ros2/demos) packages' [example server](https://github.com/ros2/demos/blob/rolling/demo_nodes_cpp/src/services/add_two_ints_server.cpp). In this benchmark scenario, the benchmarker node only has client interface. The necessary server for this client is run in [the launch file of this benchmark scenario](../launch/scenario_basic_service_client_benchmark.launch.py). Client sends a request to server and waits for the response from server. Client sends second request to server once the client receives response of first request from client. This actions are repeated `sending_request_number` times. You can configure this `sending_request_number` parameter in [this scenario's launch file]((../launch/scenario_basic_service_client_benchmark.launch.py)). + +### [MoveIt Task Constructor Benchmark](scenarios/moveit_task_constructor_benchmark.md) + +This benchmark measures the effect of middleware against moveit_task_constructor scenarios. Pick-Place task demo is selected for this benchmarking scenario. In this scenario, firstly demo scene is spawned for pick place task to be operated successfully and then the pick place task is initialized. After initializing task, [moveit_task_constructor](https://github.com/moveit/moveit_task_constructor/blob/ros2) creates the plan. If moveit_task_constructor creates the plan successfully, then the plan is executed by [moveit_task_constructor](https://github.com/moveit/moveit_task_constructor/blob/ros2). In the end, demo scene is destroyed so that the benchmarks are able to be conducted from initial state again. This stuff provides us to conduct same benchmark more than ones. Thus, the reliability of benchmark results can be increased. The codes of pick-place task demo is based on [this implementation](https://github.com/moveit/moveit_task_constructor/blob/ros2/demo/src/pick_place_task.cpp) in [moveit_task_constructor](https://github.com/moveit/moveit_task_constructor/blob/ros2). diff --git a/docs/scenarios/moveit_task_constructor_benchmark.md b/docs/scenarios/moveit_task_constructor_benchmark.md new file mode 100644 index 0000000..0c52984 --- /dev/null +++ b/docs/scenarios/moveit_task_constructor_benchmark.md @@ -0,0 +1,44 @@ +## How To Run the MoveIt Task Constructor Benchmark + +Firstly, source your ros version. It's suggested to test with rolling version of ROS. + +For instance, to test with rmw_zenoh, start to zenoh router using following command in the terminal. +```sh +# go to your workspace +cd ws +# Be sure that ros2 daemon is killed. +pkill -9 -f ros && ros2 daemon stop +# Then start zenoh router +source /opt/ros/rolling/setup.bash +source install/setup.bash +export RMW_IMPLEMENTATION=rmw_zenoh_cpp +ros2 run rmw_zenoh_cpp rmw_zenohd +``` + +Select your rmw_implementation as `rmw_zenoh_cpp` and run the moveit task constructor benchmark launch file in the another terminal. +```sh +# go to your workspace +cd ws +source /opt/ros/rolling/setup.bash +source install/setup.bash +export RMW_IMPLEMENTATION=rmw_zenoh_cpp # select your rmw_implementation to benchmark +ros2 launch moveit_middleware_benchmark scenario_moveit_task_constructor_benchmark.launch.py +``` + +It will be defaultly benchmarked with 20 repetitions. It will be created the json file named `middleware_benchmark_results.json` for benchmarking results after finishing benchmark code execution. You can see the benchmark results in more detail inside this json file. + +If you want to customize your benchmark arguments or select different test case, you can use below command. + +```shell +ros2 launch moveit_middleware_benchmark scenario_moveit_task_constructor_benchmark.launch.py benchmark_command_args:="--benchmark_out=middleware_benchmark_results.json --benchmark_out_format=json --benchmark_repetitions=1" +``` + +## How to benchmark the MoveIt Task Constructor + +The main idea here is to setup demo scene for operating task, to do planning task followed by executing task. After all of that, it's destroyed demo scene to conduct the benchmark of same task more than once. Thus, The effect of middleware on pick-place task demo can be measured the elapsed time reliably. + +[moveit_task_constructor_benchmark.webm](https://gist.github.com/user-attachments/assets/c4848652-9522-4ccb-988d-3a80d1e5d23f) + +## How to create test cases + +You can apply some settings on pick-place task demo using [this parameter file in config directory](../../config/pick_place_demo_configs.yaml). In this benchmark scenario, it's enough to change `--benchmark-repetitions` through `benchmark_command_args` argument. diff --git a/include/moveit_middleware_benchmark/scenarios/moveit_task_constructor/pick_place_task.hpp b/include/moveit_middleware_benchmark/scenarios/moveit_task_constructor/pick_place_task.hpp new file mode 100644 index 0000000..8897c3d --- /dev/null +++ b/include/moveit_middleware_benchmark/scenarios/moveit_task_constructor/pick_place_task.hpp @@ -0,0 +1,90 @@ +/********************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019 PickNik LLC. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Henning Kayser, Simon Goldstein + Desc: A demo to show MoveIt Task Constructor in action +*/ + +// ROS +#include + +// MoveIt +#include +#include +#include + +// MTC +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "pick_place_demo_parameters.hpp" + +#pragma once + +namespace moveit_task_constructor_demo +{ +using namespace moveit::task_constructor; + +// prepare a demo environment from ROS parameters under node +void setupDemoScene(const pick_place_task_demo::Params& params); + +// destroy the created demo environment from ROS parameters under node +void destroyDemoScene(const pick_place_task_demo::Params& params); + +class PickPlaceTask +{ +public: + PickPlaceTask(const std::string& task_name); + ~PickPlaceTask() = default; + + bool init(const rclcpp::Node::SharedPtr& node, const pick_place_task_demo::Params& params); + + bool plan(const std::size_t max_solutions); + + bool execute(); + +private: + std::string task_name_; + moveit::task_constructor::TaskPtr task_; +}; +} // namespace moveit_task_constructor_demo diff --git a/include/moveit_middleware_benchmark/scenarios/moveit_task_constructor/scenario_moveit_task_constructor.hpp b/include/moveit_middleware_benchmark/scenarios/moveit_task_constructor/scenario_moveit_task_constructor.hpp new file mode 100644 index 0000000..695dd1f --- /dev/null +++ b/include/moveit_middleware_benchmark/scenarios/moveit_task_constructor/scenario_moveit_task_constructor.hpp @@ -0,0 +1,108 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, Cihat Kurtuluş Altıparmak + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Cihat Kurtuluş Altıparmak + Description: Benchmarking module to compare the effects of middlewares + against moveit_task_constructor + */ + +#pragma once + +#include +#include +#include + +#include "moveit_middleware_benchmark/scenarios/moveit_task_constructor/pick_place_task.hpp" + +namespace moveit +{ +namespace middleware_benchmark +{ + +class ScenarioMoveItTaskConstructor +{ +public: + /** \brief Firstly, prepares the demo scene for pick-place actions + * and then initializes the task which created by moveit_task_constructor, + * \param [in] node The ros node + */ + ScenarioMoveItTaskConstructor(const rclcpp::Node::SharedPtr& node); + + /** \brief Firstly, destroys the created demo scene in order to reprepare + * the environment for pick-place actions + */ + ~ScenarioMoveItTaskConstructor(); + + /** \brief Starts to plan to in order to execute the task which + * created by moveit_task_constructor and then if task planning is successful, + * task plan is executed + */ + void runTestCase(); + +private: + /* the interface to get needed parameters for pick-place demo*/ + std::shared_ptr pick_place_task_param_listener_; + + /* the gotten parameters in order to be used for pick-place demo*/ + pick_place_task_demo::Params pick_place_task_demo_parameters_; + + /* The pick-place demo instance to process the executing and planning operations on robot*/ + std::shared_ptr pick_place_task_; + + rclcpp::Node::SharedPtr node_; +}; + +class ScenarioMoveItTaskConstructorFixture : public benchmark::Fixture +{ +public: + ScenarioMoveItTaskConstructorFixture(); + + /** \brief This method runs once each benchmark starts + * \param [in] state + */ + void SetUp(::benchmark::State& /*state*/); + + /** \brief This method runs as soon as each benchmark finishes + * \param [in] state + */ + void TearDown(::benchmark::State& /*state*/); + +protected: + rclcpp::Node::SharedPtr node_; + std::shared_ptr node_executor_; + std::unique_ptr spinning_thread_; +}; + +} // namespace middleware_benchmark +} // namespace moveit diff --git a/launch/scenario_moveit_task_constructor_benchmark.launch.py b/launch/scenario_moveit_task_constructor_benchmark.launch.py new file mode 100644 index 0000000..5d0c4af --- /dev/null +++ b/launch/scenario_moveit_task_constructor_benchmark.launch.py @@ -0,0 +1,198 @@ +import os +from launch import LaunchDescription +from launch.substitutions import ( + LaunchConfiguration, + PathJoinSubstitution, + EnvironmentVariable, + EqualsSubstitution, +) +from launch.conditions import IfCondition +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + Shutdown, + LogInfo, + RegisterEventHandler, + OpaqueFunction, +) +from launch.event_handlers import OnProcessExit +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + + +def launch_setup(context, *args, **kwargs): + benchmark_command_args = context.perform_substitution( + LaunchConfiguration("benchmark_command_args") + ).split() + + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description( + file_path="config/panda.urdf.xacro", + mappings={ + "ros2_control_hardware_type": LaunchConfiguration( + "ros2_control_hardware_type" + ) + }, + ) + .robot_description_semantic(file_path="config/panda.srdf") + .robot_description_kinematics(file_path="config/kinematics.yaml") + .planning_scene_monitor( + publish_robot_description=True, publish_robot_description_semantic=True + ) + .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") + .planning_pipelines(pipelines=["ompl"]) + .to_moveit_configs() + ) + + # Load ExecuteTaskSolutionCapability so we can execute found solutions in simulation + move_group_capabilities = { + "capabilities": "move_group/ExecuteTaskSolutionCapability" + } + + # Start the actual move_group node/action server + move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[moveit_config.to_dict(), move_group_capabilities], + arguments=["--ros-args", "--log-level", "info"], + ) + + # Static TF + static_tf_node = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_transform_publisher", + output="log", + arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"], + ) + + # Publish TF + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="both", + parameters=[moveit_config.robot_description], + ) + + # 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 = 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 = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint_state_broadcaster", + "--controller-manager", + "/controller_manager", + ], + ) + + panda_arm_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["panda_arm_controller", "-c", "/controller_manager"], + ) + + panda_hand_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["panda_hand_controller", "-c", "/controller_manager"], + ) + + zenoh_router = Node( + name="zenoh_router", + package="rmw_zenoh_cpp", + executable="rmw_zenohd", + output="both", + condition=IfCondition( + EqualsSubstitution( + EnvironmentVariable("RMW_IMPLEMENTATION", default_value=""), + "rmw_zenoh_cpp", + ), + ), + ) + + benchmark_main_node = Node( + name="benchmark_main", + package="moveit_middleware_benchmark", + executable="scenario_moveit_task_constructor_benchmark_main", + output="both", + arguments=benchmark_command_args, + parameters=[ + os.path.join( + get_package_share_directory("moveit_middleware_benchmark"), + "config", + "pick_place_demo_configs.yaml", + ), + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.planning_pipelines, + moveit_config.joint_limits, + {"use_sim_time": True}, + ], + on_exit=Shutdown(), + ) + + return [ + zenoh_router, + move_group_node, + static_tf_node, + robot_state_publisher, + ros2_control_node, + joint_state_broadcaster_spawner, + panda_arm_controller_spawner, + panda_hand_controller_spawner, + # for https://github.com/ros-controls/ros2_controllers/issues/981 + RegisterEventHandler( + OnProcessExit( + target_action=panda_arm_controller_spawner, + on_exit=[ + LogInfo( + msg="panda_arm_controller_spawner is finished. Now test_scenario_perception_pipeline will start" + ), + benchmark_main_node, + ], + ) + ), + ] + + +def generate_launch_description(): + declared_arguments = [] + + benchmark_command_args = DeclareLaunchArgument( + "benchmark_command_args", + default_value="--benchmark_out=middleware_benchmark_results.json --benchmark_out_format=json --benchmark_repetitions=20", + description="Google Benchmark Tool Arguments", + ) + declared_arguments.append(benchmark_command_args) + + ros2_control_hardware_type = DeclareLaunchArgument( + "ros2_control_hardware_type", + default_value="mock_components", + description="ROS 2 control hardware interface type to use for the launch file -- possible values: [mock_components, isaac]", + ) + declared_arguments.append(ros2_control_hardware_type) + + return LaunchDescription( + declared_arguments + [OpaqueFunction(function=launch_setup)] + ) diff --git a/moveit_middleware_benchmark.repos b/moveit_middleware_benchmark.repos index 58ac283..c0ce192 100644 --- a/moveit_middleware_benchmark.repos +++ b/moveit_middleware_benchmark.repos @@ -1,4 +1,8 @@ repositories: + moveit_task_constructor: + type: git + url: https://github.com/moveit/moveit_task_constructor.git + version: ros2 moveit_resources: type: git url: https://github.com/moveit/moveit_resources.git diff --git a/package.xml b/package.xml index 750482e..e5ed018 100644 --- a/package.xml +++ b/package.xml @@ -17,6 +17,8 @@ ament_index_cpp yaml-cpp example_interfaces + generate_parameter_library + moveit_task_constructor_core rmw_zenoh_cpp rmw_fastrtps_cpp diff --git a/scripts/run_all_benchmarks.sh b/scripts/run_all_benchmarks.sh index 800d8b6..0a4712e 100644 --- a/scripts/run_all_benchmarks.sh +++ b/scripts/run_all_benchmarks.sh @@ -38,3 +38,7 @@ ros2 launch moveit_middleware_benchmark scenario_basic_service_client_benchmark. mkdir ${benchmark_results_directory}/scenario_perception_pipeline -p ros2 daemon stop ros2 launch moveit_middleware_benchmark scenario_perception_pipeline_benchmark.launch.py benchmark_command_args:="--benchmark_out=${benchmark_results_directory}/scenario_perception_pipeline/${RMW_IMPLEMENTATION}.json --benchmark_out_format=json --benchmark_repetitions=6" + +mkdir ${benchmark_results_directory}/scenario_moveit_task_constructor -p +ros2 daemon stop +ros2 launch moveit_middleware_benchmark scenario_moveit_task_constructor_benchmark.launch.py benchmark_command_args:="--benchmark_out=${benchmark_results_directory}/scenario_moveit_task_constructor/${RMW_IMPLEMENTATION}.json --benchmark_out_format=json --benchmark_repetitions=20" diff --git a/src/scenario_moveit_task_constructor_benchmark_main.cpp b/src/scenario_moveit_task_constructor_benchmark_main.cpp new file mode 100644 index 0000000..82bdecd --- /dev/null +++ b/src/scenario_moveit_task_constructor_benchmark_main.cpp @@ -0,0 +1,50 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, Cihat Kurtuluş Altıparmak + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Cihat Kurtuluş Altıparmak + Description: Benchmarking module to compare the effects of middlewares + against moveit task constructor scenarios + */ + +#include "moveit_middleware_benchmark/scenarios/moveit_task_constructor/scenario_moveit_task_constructor.hpp" + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + benchmark::Initialize(&argc, argv); + benchmark::RunSpecifiedBenchmarks(); + benchmark::Shutdown(); + rclcpp::shutdown(); + return 0; +} diff --git a/src/scenarios/moveit_task_constructor/pick_place_demo_parameters.yaml b/src/scenarios/moveit_task_constructor/pick_place_demo_parameters.yaml new file mode 100644 index 0000000..a05c527 --- /dev/null +++ b/src/scenarios/moveit_task_constructor/pick_place_demo_parameters.yaml @@ -0,0 +1,97 @@ +pick_place_task_demo: + execute: + type: bool + default_value: false + table_name: + type: string + validation: + not_empty<>: [] + table_reference_frame: + type: string + validation: + not_empty<>: [] + table_dimensions: + type: double_array + validation: + fixed_size<>: [3] + table_pose: + type: double_array + validation: + fixed_size<>: [6] + object_name: + type: string + validation: + not_empty<>: [] + object_reference_frame: + type: string + validation: + not_empty<>: [] + object_dimensions: + type: double_array + validation: + fixed_size<>: [2] + object_pose: + type: double_array + validation: + fixed_size<>: [6] + spawn_table: + type: bool + default_value: true + max_solutions: + type: int + default_value: 10 + arm_group_name: + type: string + validation: + not_empty<>: [] + eef_name: + type: string + validation: + not_empty<>: [] + hand_group_name: + type: string + validation: + not_empty<>: [] + hand_frame: + type: string + validation: + not_empty<>: [] + hand_open_pose: + type: string + validation: + not_empty<>: [] + hand_close_pose: + type: string + validation: + not_empty<>: [] + arm_home_pose: + type: string + validation: + not_empty<>: [] + # Scene frames + world_frame: + type: string + validation: + not_empty<>: [] + surface_link: + type: string + validation: + not_empty<>: [] + grasp_frame_transform: + type: double_array + validation: + fixed_size<>: [6] + place_pose: + type: double_array + validation: + fixed_size<>: [6] + place_surface_offset: + type: double + approach_object_min_dist: + type: double + approach_object_max_dist: + type: double + lift_object_min_dist: + type: double + lift_object_max_dist: + type: double diff --git a/src/scenarios/moveit_task_constructor/pick_place_task.cpp b/src/scenarios/moveit_task_constructor/pick_place_task.cpp new file mode 100644 index 0000000..cfc8fec --- /dev/null +++ b/src/scenarios/moveit_task_constructor/pick_place_task.cpp @@ -0,0 +1,521 @@ +/********************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019 PickNik LLC. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Henning Kayser, Simon Goldstein, Cihat Kurtuluş Altıparmak + Desc: A demo to show MoveIt Task Constructor in action +*/ + +#include +#include +#include + +#include "moveit_middleware_benchmark/scenarios/moveit_task_constructor/pick_place_task.hpp" + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_demo"); + +namespace +{ +Eigen::Isometry3d vectorToEigen(const std::vector& values) +{ + return Eigen::Translation3d(values[0], values[1], values[2]) * + Eigen::AngleAxisd(values[3], Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(values[4], Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(values[5], Eigen::Vector3d::UnitZ()); +} +geometry_msgs::msg::Pose vectorToPose(const std::vector& values) +{ + return tf2::toMsg(vectorToEigen(values)); +}; +} // namespace + +namespace moveit_task_constructor_demo +{ + +void spawnObject(moveit::planning_interface::PlanningSceneInterface& psi, + const moveit_msgs::msg::CollisionObject& object) +{ + if (!psi.applyCollisionObject(object)) + throw std::runtime_error("Failed to spawn object: " + object.id); +} + +moveit_msgs::msg::CollisionObject createTable(const pick_place_task_demo::Params& params) +{ + geometry_msgs::msg::Pose pose = vectorToPose(params.table_pose); + moveit_msgs::msg::CollisionObject object; + object.id = params.table_name; + object.header.frame_id = params.table_reference_frame; + object.primitives.resize(1); + object.primitives[0].type = shape_msgs::msg::SolidPrimitive::BOX; + object.primitives[0].dimensions = { params.table_dimensions.at(0), params.table_dimensions.at(1), + params.table_dimensions.at(2) }; + pose.position.z -= 0.5 * params.table_dimensions[2]; // align surface with world + object.primitive_poses.push_back(pose); + return object; +} + +moveit_msgs::msg::CollisionObject destroyTable(const pick_place_task_demo::Params& params) +{ + moveit_msgs::msg::CollisionObject object; + object.id = params.table_name; + object.header.frame_id = params.table_reference_frame; + object.operation = object.REMOVE; + return object; +} + +moveit_msgs::msg::CollisionObject createObject(const pick_place_task_demo::Params& params) +{ + geometry_msgs::msg::Pose pose = vectorToPose(params.object_pose); + moveit_msgs::msg::CollisionObject object; + object.id = params.object_name; + object.header.frame_id = params.object_reference_frame; + object.primitives.resize(1); + object.primitives[0].type = shape_msgs::msg::SolidPrimitive::CYLINDER; + object.primitives[0].dimensions = { params.object_dimensions.at(0), params.object_dimensions.at(1) }; + pose.position.z += 0.5 * params.object_dimensions[0]; + object.primitive_poses.push_back(pose); + return object; +} + +moveit_msgs::msg::CollisionObject destroyObject(const pick_place_task_demo::Params& params) +{ + moveit_msgs::msg::CollisionObject object; + object.id = params.object_name; + object.header.frame_id = params.object_reference_frame; + object.operation = object.REMOVE; + return object; +} + +void setupDemoScene(const pick_place_task_demo::Params& params) +{ + // Add table and object to planning scene + rclcpp::sleep_for(std::chrono::microseconds(100)); // Wait for ApplyPlanningScene service + moveit::planning_interface::PlanningSceneInterface psi; + if (params.spawn_table) + spawnObject(psi, createTable(params)); + spawnObject(psi, createObject(params)); +} + +void destroyDemoScene(const pick_place_task_demo::Params& params) +{ + // Add table and object to planning scene + rclcpp::sleep_for(std::chrono::microseconds(100)); // Wait for ApplyPlanningScene service + moveit::planning_interface::PlanningSceneInterface psi; + if (params.spawn_table) + spawnObject(psi, destroyTable(params)); + spawnObject(psi, destroyObject(params)); +} + +PickPlaceTask::PickPlaceTask(const std::string& task_name) : task_name_(task_name) +{ +} + +bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_task_demo::Params& params) +{ + RCLCPP_INFO(LOGGER, "Initializing task pipeline"); + + // Reset ROS introspection before constructing the new object + // TODO(v4hn): global storage for Introspection services to enable one-liner + task_.reset(); + task_.reset(new moveit::task_constructor::Task()); + + // Individual movement stages are collected within the Task object + Task& t = *task_; + t.stages()->setName(task_name_); + t.loadRobotModel(node); + + /* Create planners used in various stages. Various options are available, + namely Cartesian, MoveIt pipeline, and joint interpolation. */ + // Sampling planner + auto sampling_planner = std::make_shared(node); + sampling_planner->setProperty("goal_joint_tolerance", 1e-5); + + // Cartesian planner + auto cartesian_planner = std::make_shared(); + cartesian_planner->setMaxVelocityScalingFactor(1.0); + cartesian_planner->setMaxAccelerationScalingFactor(1.0); + cartesian_planner->setStepSize(.01); + + // Set task properties + t.setProperty("group", params.arm_group_name); + t.setProperty("eef", params.eef_name); + t.setProperty("hand", params.hand_group_name); + t.setProperty("hand_grasping_frame", params.hand_frame); + t.setProperty("ik_frame", params.hand_frame); + + /**************************************************** + * * + * Current State * + * * + ***************************************************/ + { + auto current_state = std::make_unique("current state"); + + // Verify that object is not attached + auto applicability_filter = + std::make_unique("applicability test", std::move(current_state)); + applicability_filter->setPredicate([object = params.object_name](const SolutionBase& s, std::string& comment) { + if (s.start()->scene()->getCurrentState().hasAttachedBody(object)) + { + comment = "object with id '" + object + "' is already attached and cannot be picked"; + return false; + } + return true; + }); + t.add(std::move(applicability_filter)); + } + + /**************************************************** + * * + * Open Hand * + * * + ***************************************************/ + Stage* initial_state_ptr = nullptr; + { + auto stage = std::make_unique("open hand", sampling_planner); + stage->setGroup(params.hand_group_name); + stage->setGoal(params.hand_open_pose); + initial_state_ptr = stage.get(); // remember start state for monitoring grasp pose generator + t.add(std::move(stage)); + } + + /**************************************************** + * * + * Move to Pick * + * * + ***************************************************/ + // Connect initial open-hand state with pre-grasp pose defined in the following + { + auto stage = std::make_unique( + "move to pick", stages::Connect::GroupPlannerVector{ { params.arm_group_name, sampling_planner } }); + stage->setTimeout(5.0); + stage->properties().configureInitFrom(Stage::PARENT); + t.add(std::move(stage)); + } + + /**************************************************** + * * + * Pick Object * + * * + ***************************************************/ + Stage* pick_stage_ptr = nullptr; + { + // A SerialContainer combines several sub-stages, here for picking the object + auto grasp = std::make_unique("pick object"); + t.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" }); + grasp->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group", "ik_frame" }); + + /**************************************************** + ---- * Approach Object * + ***************************************************/ + { + // Move the eef link forward along its z-axis by an amount within the given min-max range + auto stage = std::make_unique("approach object", cartesian_planner); + stage->properties().set("marker_ns", "approach_object"); + stage->properties().set("link", params.hand_frame); // link to perform IK for + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); // inherit group from parent stage + stage->setMinMaxDistance(params.approach_object_min_dist, params.approach_object_max_dist); + + // Set hand forward direction + geometry_msgs::msg::Vector3Stamped vec; + vec.header.frame_id = params.hand_frame; + vec.vector.z = 1.0; + stage->setDirection(vec); + grasp->insert(std::move(stage)); + } + + /**************************************************** + ---- * Generate Grasp Pose * + ***************************************************/ + { + // Sample grasp pose candidates in angle increments around the z-axis of the object + auto stage = std::make_unique("generate grasp pose"); + stage->properties().configureInitFrom(Stage::PARENT); + stage->properties().set("marker_ns", "grasp_pose"); + stage->setPreGraspPose(params.hand_open_pose); + stage->setObject(params.object_name); // object to sample grasps for + stage->setAngleDelta(M_PI / 12); + stage->setMonitoredStage(initial_state_ptr); // hook into successful initial-phase solutions + + // Compute IK for sampled grasp poses + auto wrapper = std::make_unique("grasp pose IK", std::move(stage)); + wrapper->setMaxIKSolutions(8); // limit number of solutions + wrapper->setMinSolutionDistance(1.0); + // define virtual frame to reach the target_pose + wrapper->setIKFrame(vectorToEigen(params.grasp_frame_transform), params.hand_frame); + wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" }); // inherit properties from parent + wrapper->properties().configureInitFrom(Stage::INTERFACE, + { "target_pose" }); // inherit property from child solution + grasp->insert(std::move(wrapper)); + } + + /**************************************************** + ---- * Allow Collision (hand object) * + ***************************************************/ + { + // Modify planning scene (w/o altering the robot's pose) to allow touching the object for picking + auto stage = std::make_unique("allow collision (hand,object)"); + stage->allowCollisions( + params.object_name, + t.getRobotModel()->getJointModelGroup(params.hand_group_name)->getLinkModelNamesWithCollisionGeometry(), + true); + grasp->insert(std::move(stage)); + } + + /**************************************************** + ---- * Close Hand * + ***************************************************/ + { + auto stage = std::make_unique("close hand", sampling_planner); + stage->setGroup(params.hand_group_name); + stage->setGoal(params.hand_close_pose); + grasp->insert(std::move(stage)); + } + + /**************************************************** + .... * Attach Object * + ***************************************************/ + { + auto stage = std::make_unique("attach object"); + stage->attachObject(params.object_name, params.hand_frame); // attach object to hand_frame_ + grasp->insert(std::move(stage)); + } + + /**************************************************** + .... * Allow collision (object support) * + ***************************************************/ + { + auto stage = std::make_unique("allow collision (object,support)"); + stage->allowCollisions({ params.object_name }, { params.surface_link }, true); + grasp->insert(std::move(stage)); + } + + /**************************************************** + .... * Lift object * + ***************************************************/ + { + auto stage = std::make_unique("lift object", cartesian_planner); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->setMinMaxDistance(params.lift_object_min_dist, params.lift_object_max_dist); + stage->setIKFrame(params.hand_frame); + stage->properties().set("marker_ns", "lift_object"); + + // Set upward direction + geometry_msgs::msg::Vector3Stamped vec; + vec.header.frame_id = params.world_frame; + vec.vector.z = 1.0; + stage->setDirection(vec); + grasp->insert(std::move(stage)); + } + + /**************************************************** + .... * Forbid collision (object support) * + ***************************************************/ + { + auto stage = std::make_unique("forbid collision (object,surface)"); + stage->allowCollisions({ params.object_name }, { params.surface_link }, false); + grasp->insert(std::move(stage)); + } + + pick_stage_ptr = grasp.get(); // remember for monitoring place pose generator + + // Add grasp container to task + t.add(std::move(grasp)); + } + + /****************************************************** + * * + * Move to Place * + * * + *****************************************************/ + { + // Connect the grasped state to the pre-place state, i.e. realize the object transport + auto stage = std::make_unique( + "move to place", stages::Connect::GroupPlannerVector{ { params.arm_group_name, sampling_planner } }); + stage->setTimeout(5.0); + stage->properties().configureInitFrom(Stage::PARENT); + t.add(std::move(stage)); + } + + /****************************************************** + * * + * Place Object * + * * + *****************************************************/ + // All placing sub-stages are collected within a serial container again + { + auto place = std::make_unique("place object"); + t.properties().exposeTo(place->properties(), { "eef", "hand", "group" }); + place->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group" }); + + /****************************************************** + ---- * Lower Object * + *****************************************************/ + { + auto stage = std::make_unique("lower object", cartesian_planner); + stage->properties().set("marker_ns", "lower_object"); + stage->properties().set("link", params.hand_frame); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->setMinMaxDistance(.03, .13); + + // Set downward direction + geometry_msgs::msg::Vector3Stamped vec; + vec.header.frame_id = params.world_frame; + vec.vector.z = -1.0; + stage->setDirection(vec); + place->insert(std::move(stage)); + } + + /****************************************************** + ---- * Generate Place Pose * + *****************************************************/ + { + // Generate Place Pose + auto stage = std::make_unique("generate place pose"); + stage->properties().configureInitFrom(Stage::PARENT, { "ik_frame" }); + stage->properties().set("marker_ns", "place_pose"); + stage->setObject(params.object_name); + + // Set target pose + geometry_msgs::msg::PoseStamped p; + p.header.frame_id = params.object_reference_frame; + p.pose = vectorToPose(params.place_pose); + p.pose.position.z += 0.5 * params.object_dimensions[0] + params.place_surface_offset; + stage->setPose(p); + stage->setMonitoredStage(pick_stage_ptr); // hook into successful pick solutions + + // Compute IK + auto wrapper = std::make_unique("place pose IK", std::move(stage)); + wrapper->setMaxIKSolutions(2); + wrapper->setIKFrame(vectorToEigen(params.grasp_frame_transform), params.hand_frame); + wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" }); + wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" }); + place->insert(std::move(wrapper)); + } + + /****************************************************** + ---- * Open Hand * + *****************************************************/ + { + auto stage = std::make_unique("open hand", sampling_planner); + stage->setGroup(params.hand_group_name); + stage->setGoal(params.hand_open_pose); + place->insert(std::move(stage)); + } + + /****************************************************** + ---- * Forbid collision (hand, object) * + *****************************************************/ + { + auto stage = std::make_unique("forbid collision (hand,object)"); + stage->allowCollisions(params.object_name, *t.getRobotModel()->getJointModelGroup(params.hand_group_name), false); + place->insert(std::move(stage)); + } + + /****************************************************** + ---- * Detach Object * + *****************************************************/ + { + auto stage = std::make_unique("detach object"); + stage->detachObject(params.object_name, params.hand_frame); + place->insert(std::move(stage)); + } + + /****************************************************** + ---- * Retreat Motion * + *****************************************************/ + { + auto stage = std::make_unique("retreat after place", cartesian_planner); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->setMinMaxDistance(.12, .25); + stage->setIKFrame(params.hand_frame); + stage->properties().set("marker_ns", "retreat"); + geometry_msgs::msg::Vector3Stamped vec; + vec.header.frame_id = params.hand_frame; + vec.vector.z = -1.0; + stage->setDirection(vec); + place->insert(std::move(stage)); + } + + // Add place container to task + t.add(std::move(place)); + } + + /****************************************************** + * * + * Move to Home * + * * + *****************************************************/ + { + auto stage = std::make_unique("move home", sampling_planner); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->setGoal(params.arm_home_pose); + stage->restrictDirection(stages::MoveTo::FORWARD); + t.add(std::move(stage)); + } + + // prepare Task structure for planning + try + { + t.init(); + } + catch (InitStageException& e) + { + RCLCPP_ERROR_STREAM(LOGGER, "Initialization failed: " << e); + return false; + } + + return true; +} + +bool PickPlaceTask::plan(const std::size_t max_solutions) +{ + RCLCPP_INFO(LOGGER, "Start searching for task solutions"); + + return static_cast(task_->plan(max_solutions)); +} + +bool PickPlaceTask::execute() +{ + RCLCPP_INFO(LOGGER, "Executing solution trajectory"); + moveit_msgs::msg::MoveItErrorCodes execute_result; + + execute_result = task_->execute(*task_->solutions().front()); + + if (execute_result.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + { + RCLCPP_ERROR_STREAM(LOGGER, "Task execution failed and returned: " << execute_result.val); + return false; + } + + return true; +} +} // namespace moveit_task_constructor_demo diff --git a/src/scenarios/moveit_task_constructor/scenario_moveit_task_constructor.cpp b/src/scenarios/moveit_task_constructor/scenario_moveit_task_constructor.cpp new file mode 100644 index 0000000..4c86c33 --- /dev/null +++ b/src/scenarios/moveit_task_constructor/scenario_moveit_task_constructor.cpp @@ -0,0 +1,83 @@ +#include "moveit_middleware_benchmark/scenarios/moveit_task_constructor/scenario_moveit_task_constructor.hpp" + +namespace moveit +{ +namespace middleware_benchmark +{ + +ScenarioMoveItTaskConstructor::ScenarioMoveItTaskConstructor(const rclcpp::Node::SharedPtr& node) : node_(node) +{ + pick_place_task_param_listener_ = std::make_shared(node_); + pick_place_task_demo_parameters_ = pick_place_task_param_listener_->get_params(); + + moveit_task_constructor_demo::setupDemoScene(pick_place_task_demo_parameters_); + pick_place_task_ = std::make_shared("pick_place_task"); + + if (!pick_place_task_->init(node_, pick_place_task_demo_parameters_)) + { + RCLCPP_INFO(node->get_logger(), "Initialization failed"); + } +} + +void ScenarioMoveItTaskConstructor::runTestCase() +{ + if (pick_place_task_->plan(pick_place_task_demo_parameters_.max_solutions)) + { + pick_place_task_->execute(); + RCLCPP_INFO(node_->get_logger(), "Planning succeeded"); + } + else + { + RCLCPP_ERROR(node_->get_logger(), "Planning failed"); + } +} + +ScenarioMoveItTaskConstructor::~ScenarioMoveItTaskConstructor() +{ + moveit_task_constructor_demo::destroyDemoScene(pick_place_task_demo_parameters_); +} + +ScenarioMoveItTaskConstructorFixture::ScenarioMoveItTaskConstructorFixture() +{ +} + +void ScenarioMoveItTaskConstructorFixture::SetUp(::benchmark::State& /*state*/) +{ + if (node_.use_count() == 0) + { + node_ = std::make_shared("test_scenario_moveit_task_constructor_node", + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)); + + node_executor_ = std::make_shared(); + node_executor_->add_node(node_); + + spinning_thread_ = std::make_unique([this]() { node_executor_->spin(); }); + } +} + +void ScenarioMoveItTaskConstructorFixture::TearDown(::benchmark::State& /*state*/) +{ + node_executor_->cancel(); + + if (spinning_thread_->joinable()) + { + spinning_thread_->join(); + } + + node_executor_.reset(); + node_.reset(); +} + +BENCHMARK_DEFINE_F(ScenarioMoveItTaskConstructorFixture, test_scenario_moveit_task_constructor)(benchmark::State& st) +{ + for (auto _ : st) + { + auto sc = ScenarioMoveItTaskConstructor(node_); + sc.runTestCase(); + } +} + +BENCHMARK_REGISTER_F(ScenarioMoveItTaskConstructorFixture, test_scenario_moveit_task_constructor); + +} // namespace middleware_benchmark +} // namespace moveit