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
@@ -63,9 +65,41 @@ target_include_directories(
 target_link_libraries(scenario_basic_service_client_benchmark_main
                       PUBLIC "benchmark::benchmark" ${YAML_CPP_LIBRARIES})
 
-install(TARGETS scenario_perception_pipeline_benchmark_main
-                scenario_basic_service_client_benchmark_main
-        DESTINATION lib/moveit_middleware_benchmark)
+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
+          scenario_moveit_task_constructor_benchmark_main
+  DESTINATION lib/moveit_middleware_benchmark)
 
 install(DIRECTORY launch config DESTINATION share/moveit_middleware_benchmark)
 

config/pick_place_demo_configs.yaml

@@ -0,0 +1,49 @@
+/**:
+  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
+

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,94 @@
+/*********************************************************************
+ * 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 <ament_index_cpp/get_package_share_directory.hpp>
+
+#include <moveit/moveit_cpp/moveit_cpp.h>
+#include <moveit/moveit_cpp/planning_component.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+
+#include "moveit_middleware_benchmark/scenarios/moveit_task_constructor/pick_place_task.hpp"
+
+namespace
+{
+}  // namespace
+
+namespace moveit
+{
+namespace middleware_benchmark
+{
+
+class ScenarioMoveItTaskConstructor
+{
+public:
+  ScenarioMoveItTaskConstructor(const rclcpp::Node::SharedPtr & node);
+  ~ScenarioMoveItTaskConstructor();
+
+  void runTestCase();
+
+private:
+  std::shared_ptr<pick_place_task_demo::ParamListener> pick_place_task_param_listener_;
+  pick_place_task_demo::Params pick_place_task_demo_parameters_;
+  std::shared_ptr<moveit_task_constructor_demo::PickPlaceTask> pick_place_task_;
+  rclcpp::Node::SharedPtr node_;
+};
+
+class ScenarioMoveItTaskConstructorFixture : public benchmark::Fixture
+{
+public:
+  ScenarioMoveItTaskConstructorFixture();
+
+  void SetUp(::benchmark::State& /*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