Rearranged directory tree structure and some modifications

- Added middleware implementations to exec_depend ( #1 (comment) )
- Refactored package directory tree structure ( #1 (comment) )
- Added namespace ( #1 (comment) )

CMakeLists.txt

@@ -15,11 +15,11 @@ find_package(nav_msgs REQUIRED)
 find_package(ament_index_cpp REQUIRED)
 find_package(yaml-cpp REQUIRED)
 
-add_executable(test_scenario_perception_pipeline
-               test/test_scenario_perception_pipeline.cpp)
+add_executable(benchmark_main src/benchmark_main.cpp
+                              src/scenarios/scenario_perception_pipeline.cpp)
 
 ament_target_dependencies(
-  test_scenario_perception_pipeline
+  benchmark_main
   PUBLIC
   "moveit_ros_planning_interface"
   "rclcpp"
@@ -29,15 +29,13 @@ ament_target_dependencies(
   "yaml-cpp")
 
 target_include_directories(
-  test_scenario_perception_pipeline
-  PUBLIC $<INSTALL_INTERFACE:include>
-         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>)
+  benchmark_main PUBLIC $<INSTALL_INTERFACE:include>
+                        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>)
 
-target_link_libraries(test_scenario_perception_pipeline
-                      PUBLIC "benchmark::benchmark" ${YAML_CPP_LIBRARIES})
+target_link_libraries(benchmark_main PUBLIC "benchmark::benchmark"
+                                            ${YAML_CPP_LIBRARIES})
 
-install(TARGETS test_scenario_perception_pipeline
-        DESTINATION lib/${PROJECT_NAME})
+install(TARGETS benchmark_main DESTINATION lib/${PROJECT_NAME})
 
 install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME})
 

README.md

@@ -26,7 +26,7 @@ colcon build --mixin release
 source /opt/ros/rolling/setup.bash
 source install/setup.bash
 export RMW_IMPLEMENTATION=rmw_fastrtps_cpp # select your rmw_implementation to benchmark
-ros2 launch moveit_middleware_benchmark test_scenario_perception_pipeline.launch.py
+ros2 launch moveit_middleware_benchmark moveit_middleware_benchmark_demo.launch.py
 
 ```
 

include/moveit_middleware_benchmark/scenarios/scenario_perception_pipeline.hpp

@@ -0,0 +1,107 @@
+/*********************************************************************
+ * 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 perception pipeline of
+ */
+
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <benchmark/benchmark.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <memory>
+
+#include <moveit/moveit_cpp/moveit_cpp.h>
+#include <moveit/moveit_cpp/planning_component.h>
+
+#include <geometry_msgs/msg/pose.hpp>
+#include <nav_msgs/msg/path.hpp>
+#include <dynmsg/msg_parser.hpp>
+#include <dynmsg/typesupport.hpp>
+#include <dynmsg/yaml_utils.hpp>
+
+#include <ament_index_cpp/get_package_share_directory.hpp>
+
+namespace moveit
+{
+namespace middleware_benchmark
+{
+
+using moveit::planning_interface::MoveGroupInterface;
+
+const std::string PLANNING_GROUP = "panda_arm";
+
+class ScenarioPerceptionPipeline
+{
+public:
+  ScenarioPerceptionPipeline(std::shared_ptr<MoveGroupInterface>);
+  std::tuple<int, int> runTestCase(const nav_msgs::msg::Path& pose_list);
+  bool sendTargetPose(const geometry_msgs::msg::Pose& target_pose);
+
+private:
+  std::shared_ptr<MoveGroupInterface> move_group_interface_ptr_;
+};
+
+class ScenarioPerceptionPipelineTestCaseCreator
+{
+private:
+  static inline std::vector<nav_msgs::msg::Path> test_cases_ = {};
+
+public:
+  static void createTestCases();
+
+  static nav_msgs::msg::Path selectTestCases(size_t test_case_index);
+
+  static void readTestCasesFromFile(const std::string& yaml_file_name);
+
+  static nav_msgs::msg::Path getTestCaseFromYamlString(const std::string& yaml_string);
+};
+
+class ScenarioPerceptionPipelineFixture : public benchmark::Fixture
+{
+protected:
+  rclcpp::Node::SharedPtr node_;
+  std::shared_ptr<MoveGroupInterface> move_group_interface_ptr_;
+  size_t selected_test_case_index_;
+
+public:
+  ScenarioPerceptionPipelineFixture();
+  void SetUp(::benchmark::State& /*state*/);
+  void TearDown(::benchmark::State& /*state*/);
+};
+
+}  // namespace middleware_benchmark
+}  // namespace moveit

launch/test_scenario_perception_pipeline.launch.py → launch/moveit_middleware_benchmark_demo.launch.py

@@ -134,13 +134,13 @@ def generate_launch_description():
         condition=IfCondition(db_config),
     )
 
-    test_scenario_perception_pipeline_node = Node(
-        name="test_scenario_perception_pipeline_node",
+    benchmark_main_node = Node(
+        name="benchmark_main",
         package="moveit_middleware_benchmark",
-        executable="test_scenario_perception_pipeline",
+        executable="benchmark_main",
         output="both",
         arguments=[
-            "--benchmark_out=benchmark_scenario_perception_pipeline.json",
+            "--benchmark_out=middleware_benchmark_results.json",
             "--benchmark_out_format=json",
         ],
         parameters=[
@@ -172,7 +172,7 @@ def generate_launch_description():
                         LogInfo(
                             msg="panda_arm_controller_spawner is finished. Now test_scenario_perception_pipeline will start"
                         ),
-                        test_scenario_perception_pipeline_node,
+                        benchmark_main_node,
                     ],
                 )
             ),

package.xml

@@ -3,7 +3,7 @@
 <package format="3">
   <name>moveit_middleware_benchmark</name>
   <version>0.0.0</version>
-  <description>Middleware Benchmark Tool For MoveIt pipelines</description>
+  <description>Middleware Benchmark Tool For MoveIt Pipelines</description>
   <maintainer email="[email protected]">Cihat Kurtuluş Altıparmak</maintainer>
   <license>BSD</license>
 
@@ -17,6 +17,10 @@
   <depend>ament_index_cpp</depend>
   <depend>yaml-cpp</depend>
 
+  <exec_depend>rmw_zenoh</exec_depend>
+  <exec_depend>rmw_fastrtps</exec_depend>
+  <exec_depend>rmw_cyclonedds</exec_depend>
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

src/benchmark_main.cpp

@@ -0,0 +1,11 @@
+#include "moveit_middleware_benchmark/scenarios/scenario_perception_pipeline.hpp"
+
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+  benchmark::Initialize(&argc, argv);
+  benchmark::RunSpecifiedBenchmarks();
+  benchmark::Shutdown();
+  rclcpp::shutdown();
+  return 0;
+}

src/scenarios/scenario_perception_pipeline.cpp

@@ -0,0 +1,160 @@
+/*********************************************************************
+ * 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 perception pipeline of
+ */
+
+#include "moveit_middleware_benchmark/scenarios/scenario_perception_pipeline.hpp"
+
+namespace moveit
+{
+namespace middleware_benchmark
+{
+
+ScenarioPerceptionPipeline::ScenarioPerceptionPipeline(std::shared_ptr<MoveGroupInterface> move_group_interface_ptr)
+  : move_group_interface_ptr_(move_group_interface_ptr)
+{
+}
+
+std::tuple<int, int> ScenarioPerceptionPipeline::runTestCase(const nav_msgs::msg::Path& test_case)
+{
+  int success_number = 0;
+  int failure_number = 0;
+  for (auto& pose_stamped : test_case.poses)
+  {
+    bool is_successful = sendTargetPose(pose_stamped.pose);
+    if (is_successful)
+      success_number++;
+    else
+      failure_number++;
+  }
+
+  return { success_number, failure_number };
+}
+
+bool ScenarioPerceptionPipeline::sendTargetPose(const geometry_msgs::msg::Pose& target_pose)
+{
+  move_group_interface_ptr_->setPoseTarget(target_pose);
+
+  moveit::planning_interface::MoveGroupInterface::Plan plan_msg;
+  const auto ok = static_cast<bool>(move_group_interface_ptr_->plan(plan_msg));
+
+  if (ok)
+  {
+    move_group_interface_ptr_->execute(plan_msg);
+    return true;
+  }
+  else
+  {
+    return false;
+  }
+}
+
+void ScenarioPerceptionPipelineTestCaseCreator::createTestCases()
+{
+  const std::string yaml_file_path = ament_index_cpp::get_package_share_directory("moveit_middleware_benchmark") +
+                                     "/config/test_scenario_perception_pipeline.yaml";
+  readTestCasesFromFile(yaml_file_path);
+}
+
+nav_msgs::msg::Path ScenarioPerceptionPipelineTestCaseCreator::selectTestCases(size_t test_case_index)
+{
+  return test_cases_.at(test_case_index);
+}
+
+void ScenarioPerceptionPipelineTestCaseCreator::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);
+    nav_msgs::msg::Path test_case = getTestCaseFromYamlString(yaml_string);
+
+    test_cases_.push_back(test_case);
+  }
+}
+
+nav_msgs::msg::Path ScenarioPerceptionPipelineTestCaseCreator::getTestCaseFromYamlString(const std::string& yaml_string)
+{
+  nav_msgs::msg::Path path_msg;
+  void* ros_message = reinterpret_cast<void*>(&path_msg);
+
+  dynmsg::cpp::yaml_and_typeinfo_to_rosmsg(dynmsg::cpp::get_type_info({ "nav_msgs", "Path" }), yaml_string, ros_message);
+
+  return path_msg;
+}
+
+ScenarioPerceptionPipelineFixture::ScenarioPerceptionPipelineFixture()
+{
+  ScenarioPerceptionPipelineTestCaseCreator::createTestCases();
+}
+
+void ScenarioPerceptionPipelineFixture::SetUp(::benchmark::State& /*state*/)
+{
+  if (node_.use_count() == 0)
+  {
+    node_ = std::make_shared<rclcpp::Node>("test_scenario_perception_pipeline_node",
+                                           rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));
+
+    node_->get_parameter("selected_test_case_index", selected_test_case_index_);
+  }
+
+  if (move_group_interface_ptr_.use_count() == 0)
+  {
+    move_group_interface_ptr_ = std::make_shared<MoveGroupInterface>(node_, PLANNING_GROUP);
+  }
+}
+
+void ScenarioPerceptionPipelineFixture::TearDown(::benchmark::State& /*state*/)
+{
+}
+
+BENCHMARK_DEFINE_F(ScenarioPerceptionPipelineFixture, test_scenario_perception_pipeline)(benchmark::State& st)
+{
+  auto selected_test_case = ScenarioPerceptionPipelineTestCaseCreator::selectTestCases(selected_test_case_index_);
+  for (auto _ : st)
+  {
+    auto sc = ScenarioPerceptionPipeline(move_group_interface_ptr_);
+    auto [success_number, failure_number] = sc.runTestCase(selected_test_case);
+    st.counters["success_number"] = success_number;
+    st.counters["failure_number"] = failure_number;
+  }
+}
+
+BENCHMARK_REGISTER_F(ScenarioPerceptionPipelineFixture, test_scenario_perception_pipeline);
+
+}  // namespace middleware_benchmark
+}  // namespace moveit