Made test case creation flexible and removed unnecessary imports from…

… launch file
moveit · Jun 17, 2024 · 6ab2aad · 6ab2aad
1 parent 1d3f0ff
commit 6ab2aad
Show file tree

Hide file tree

Showing 7 changed files with 171 additions and 75 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -10,12 +10,21 @@ find_package(ament_cmake REQUIRED)
 find_package(moveit_ros_planning_interface REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(benchmark REQUIRED)
+find_package(dynmsg REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(ament_index_cpp REQUIRED)
 
 add_executable(test_scenario_perception_pipeline
                test/test_scenario_perception_pipeline.cpp)
 
-ament_target_dependencies(test_scenario_perception_pipeline PUBLIC
-                          "moveit_ros_planning_interface" "rclcpp" "benchmark")
+ament_target_dependencies(
+  test_scenario_perception_pipeline
+  PUBLIC
+  "moveit_ros_planning_interface"
+  "rclcpp"
+  "benchmark"
+  "dynmsg"
+  "nav_msgs")
 
 target_include_directories(
   test_scenario_perception_pipeline
@@ -28,6 +37,6 @@ target_link_libraries(test_scenario_perception_pipeline
 install(TARGETS test_scenario_perception_pipeline
         DESTINATION lib/${PROJECT_NAME})
 
-install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME})
 
 ament_package()
diff --git a/config/sensors_3d.yaml b/config/sensors_3d.yaml
@@ -0,0 +1,21 @@
+sensors:
+  - camera_1_pointcloud
+  - camera_2_pointcloud
+camera_1_pointcloud:
+    sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
+    point_cloud_topic: /camera_1/points
+    max_range: 5.0
+    point_subsample: 1
+    padding_offset: 0.1
+    padding_scale: 1.0
+    max_update_rate: 1.0
+    filtered_cloud_topic: /camera_1/filtered_points
+camera_2_pointcloud:
+    sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
+    point_cloud_topic: /camera_2/points
+    max_range: 5.0
+    point_subsample: 1
+    padding_offset: 0.1
+    padding_scale: 1.0
+    max_update_rate: 1.0
+    filtered_cloud_topic: /camera_2/filtered_points
diff --git a/config/test_scenario_perception_pipeline.yaml b/config/test_scenario_perception_pipeline.yaml
@@ -0,0 +1,73 @@
+test_cases:
+  - poses:
+    - pose:
+        position:
+          x: 0.5
+          y: 0.5
+          z: 0.5
+        orientation:
+          w : 1.0
+    - pose:
+        position:
+          x: 0.5
+          y: -0.5
+          z: 0.7
+        orientation:
+          w: 1.0
+  - poses:
+    - pose:
+        position:
+          x: 0.5
+          y: 0.5
+          z: 0.5
+        orientation:
+          w : 1.0
+    - pose:
+        position:
+          x: 0.5
+          y: -0.5
+          z: 0.7
+        orientation:
+          w: 1.0
+    - pose:
+        position:
+          x: 0.5
+          y: 0.5
+          z: 0.5
+        orientation:
+          w : 1.0
+    - pose:
+        position:
+          x: 0.5
+          y: -0.5
+          z: 0.7
+        orientation:
+          w: 1.0
+    - pose:
+        position:
+          x: 0.5
+          y: 0.5
+          z: 0.5
+        orientation:
+          w : 1.0
+    - pose:
+        position:
+          x: 0.5
+          y: -0.5
+          z: 0.7
+        orientation:
+          w: 1.0
+    - pose:
+        position:
+          x: 0.5
+          y: 0.5
+          z: 0.5
+        orientation:
+          w : 1.0
+    - pose:
+        position:
+          x: 0.5
+          y: -0.5
+          z: 0.7
+        orientation:
+          w: 1.0
diff --git a/launch/test_scenario_perception_pipeline.launch.py b/launch/test_scenario_perception_pipeline.launch.py
@@ -7,32 +7,16 @@
 from launch_ros.substitutions import FindPackageShare
 from ament_index_python.packages import get_package_share_directory
 from moveit_configs_utils import MoveItConfigsBuilder
-from launch_param_builder import ParameterBuilder
 from launch.actions import (
     DeclareLaunchArgument,
-    EmitEvent,
     ExecuteProcess,
     LogInfo,
     RegisterEventHandler,
-    TimerAction,
-)
-from launch.event_handlers import (
-    OnExecutionComplete,
-    OnProcessExit,
-    OnProcessIO,
-    OnProcessStart,
-    OnShutdown,
 )
+from launch.event_handlers import OnProcessExit
 
 
 def generate_launch_description():
-    # Command-line arguments
-    rviz_config_arg = DeclareLaunchArgument(
-        "rviz_config",
-        default_value="moveit.rviz",
-        description="RViz configuration file",
-    )
-
     db_arg = DeclareLaunchArgument(
         "db", default_value="False", description="Database flag"
     )
@@ -64,7 +48,7 @@ def generate_launch_description():
         )
         .sensors_3d(
             file_path=os.path.join(
-                get_package_share_directory("moveit2_tutorials"),
+                get_package_share_directory("moveit_middleware_benchmark"),
                 "config/sensors_3d.yaml",
             )
         )
@@ -80,26 +64,6 @@ def generate_launch_description():
         arguments=["--ros-args", "--log-level", "info"],
     )
 
-    # RViz
-    rviz_base = LaunchConfiguration("rviz_config")
-    rviz_config = PathJoinSubstitution(
-        [FindPackageShare("moveit_resources_panda_moveit_config"), "launch", rviz_base]
-    )
-    rviz_node = Node(
-        package="rviz2",
-        executable="rviz2",
-        name="rviz2",
-        output="log",
-        arguments=["-d", rviz_config],
-        parameters=[
-            moveit_config.robot_description,
-            moveit_config.robot_description_semantic,
-            moveit_config.planning_pipelines,
-            moveit_config.robot_description_kinematics,
-            moveit_config.joint_limits,
-        ],
-    )
-
     # Static TF
     static_tf_node = Node(
         package="tf2_ros",
@@ -175,20 +139,23 @@ def generate_launch_description():
         package="moveit_middleware_benchmark",
         executable="test_scenario_perception_pipeline",
         output="both",
+        arguments=[
+            "--benchmark_out=benchmark_scenario_perception_pipeline.json",
+            "--benchmark_out_format=json",
+        ],
         parameters=[
             moveit_config.robot_description,
             moveit_config.robot_description_semantic,
             moveit_config.robot_description_kinematics,
             {"use_sim_time": True},
+            {"selected_test_case_index": 0},
         ],
     )
 
     return LaunchDescription(
         [
-            # rviz_config_arg,
             db_arg,
             ros2_control_hardware_type,
-            # rviz_node,
             static_tf_node,
             robot_state_publisher,
             move_group_node,
@@ -202,7 +169,9 @@ def generate_launch_description():
                 OnProcessExit(
                     target_action=panda_arm_controller_spawner,
                     on_exit=[
-                        LogInfo(msg="panda_arm_controller_spawner is finished. Now test_scenario_perception_pipeline will start"),
+                        LogInfo(
+                            msg="panda_arm_controller_spawner is finished. Now test_scenario_perception_pipeline will start"
+                        ),
                         test_scenario_perception_pipeline_node,
                     ],
                 )

diff --git a/moveit_middleware_benchmark.repos b/moveit_middleware_benchmark.repos
@@ -11,3 +11,7 @@ repositories:
     type: git
     url: https://github.com/ros2/rmw_zenoh.git
     version: main
+  dynamic_message_introspection:
+    type: git
+    url: [email protected]:osrf/dynamic_message_introspection.git
+    version: main
diff --git a/package.xml b/package.xml
@@ -12,6 +12,9 @@
   <depend>moveit_ros_planning_interface</depend>
   <depend>rclcpp</depend>
   <depend>benchmark</depend>
+  <depend>dynmsg</depend>
+  <depend>nav_msgs</depend>
+  <depend>ament_index_cpp</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>