Implemented the scenario related to moveit_task_constructor_benchmark

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

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
 

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

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).

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.

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 <rclcpp/node.hpp>
+
+// MoveIt
+#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+
+// MTC
+#include <moveit/task_constructor/task.h>
+#include <moveit/task_constructor/stages/compute_ik.h>
+#include <moveit/task_constructor/stages/connect.h>
+#include <moveit/task_constructor/stages/current_state.h>
+#include <moveit/task_constructor/stages/generate_grasp_pose.h>
+#include <moveit/task_constructor/stages/generate_pose.h>
+#include <moveit/task_constructor/stages/generate_place_pose.h>
+#include <moveit/task_constructor/stages/modify_planning_scene.h>
+#include <moveit/task_constructor/stages/move_relative.h>
+#include <moveit/task_constructor/stages/move_to.h>
+#include <moveit/task_constructor/stages/predicate_filter.h>
+#include <moveit/task_constructor/solvers/cartesian_path.h>
+#include <moveit/task_constructor/solvers/pipeline_planner.h>
+#include <moveit_task_constructor_msgs/action/execute_task_solution.hpp>
+#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

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 <rclcpp/rclcpp.hpp>
+#include <benchmark/benchmark.h>
+#include <memory>
+
+#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_demo::ParamListener> 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<moveit_task_constructor_demo::PickPlaceTask> 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<rclcpp::executors::SingleThreadedExecutor> node_executor_;
+  std::unique_ptr<std::thread> spinning_thread_;
+};
+
+}  // namespace middleware_benchmark
+}  // namespace moveit