Draft: Improve: panda_moveit: Add parameters for perception tutorial

panda_description/urdf/panda_arm.urdf.xacro

@@ -0,0 +1,18 @@
+<?xml version='1.0' encoding='utf-8'?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
+  <xacro:arg name="arm_id" default="panda"/> <!-- Name of this panda -->
+  <xacro:arg name="hand" default="false"/> <!-- Should a franka_gripper be mounted at the flange?" -->
+  <xacro:arg name="robot_ip" default=""/> <!-- IP address or hostname of the robot" -->
+  <xacro:arg name="use_fake_hardware" default="false"/>
+  <xacro:arg name="fake_sensor_commands" default="false"/>
+
+  <xacro:include filename="$(find franka_description)/robots/panda_arm.xacro"/>
+  <xacro:panda_arm arm_id="$(arg arm_id)" safety_distance="0.03"/>
+
+  <xacro:if value="$(arg hand)">
+    <xacro:include filename="$(find franka_description)/robots/hand.xacro"/>
+    <xacro:hand ns="$(arg arm_id)" rpy="0 0 ${-pi/4}" connected_to="$(arg arm_id)_link8" safety_distance="0.03"/>
+  </xacro:if>
+  <xacro:include filename="$(find franka_description)/robots/panda_arm.ros2_control.xacro"/>
+  <xacro:panda_arm_ros2_control ns="$(arg arm_id)" robot_ip="$(arg robot_ip)" use_fake_hardware="$(arg use_fake_hardware)" fake_sensor_commands="$(arg fake_sensor_commands)"/>
+</robot>

panda_moveit_config/config/sensors_3d.yaml

@@ -0,0 +1,27 @@
+sensors:
+  - kinect_pointcloud
+  #- kinect_depthimage
+kinect_pointcloud:
+    filtered_cloud_topic: filtered_cloud
+    max_range: 5.0
+    max_update_rate: 1.0
+    ns: kinect
+    padding_offset: 0.1
+    padding_scale: 0.5
+    point_cloud_topic: /camera/depth_registered/points
+    point_subsample: 1
+    sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
+kinect_depthimage:
+    far_clipping_plane_distance: 5.0
+    filtered_cloud_topic: filtered_cloud
+    image_topic: /camera/depth_registered/image_raw
+    max_update_rate: 1.0
+    near_clipping_plane_distance: 0.3
+    ns: kinect
+    padding_offset: 0.03
+    padding_scale: 4.0
+    queue_size: 5
+    sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
+    shadow_threshold: 0.2
+    skip_vertical_pixels: 4
+    skip_horizontal_pixels: 6

panda_moveit_config/config/sensors_kinect_depthmap.yaml

@@ -1,11 +1,13 @@
-sensors:
-  - sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
-    image_topic: /camera/depth_registered/image_raw
-    queue_size: 5
-    near_clipping_plane_distance: 0.3
-    far_clipping_plane_distance: 5.0
-    shadow_threshold: 0.2
-    padding_scale: 4.0
-    padding_offset: 0.03
-    max_update_rate: 1.0
-    filtered_cloud_topic: filtered_cloud
+/**:
+  sensors:
+    ros__parameters:
+      sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
+      image_topic: /camera/depth_registered/image_raw
+      queue_size: 5
+      near_clipping_plane_distance: 0.3
+      far_clipping_plane_distance: 5.0
+      shadow_threshold: 0.2
+      padding_scale: 4.0
+      padding_offset: 0.03
+      max_update_rate: 1.0
+      filtered_cloud_topic: filtered_cloud

panda_moveit_config/config/sensors_kinect_pointcloud.yaml

@@ -1,9 +1,12 @@
-sensors:
-  - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
-    point_cloud_topic: /camera/depth_registered/points
-    max_range: 5.0
-    point_subsample: 1
-    padding_offset: 0.1
-    padding_scale: 1.0
-    max_update_rate: 1.0
-    filtered_cloud_topic: filtered_cloud
+/**:
+  ros__parameters:
+    sensor1:
+      sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
+      point_cloud_topic: /camera/depth_registered/points
+      max_range: 5.0
+      point_subsample: 1
+      padding_offset: 0.1
+      padding_scale: 1.0
+      max_update_rate: 1.0
+      filtered_cloud_topic: filtered_cloud
+      ns: kinect

panda_moveit_config/launch/demo.launch.py

@@ -1,13 +1,27 @@
 import os
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
-from launch.substitutions import LaunchConfiguration
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch.conditions import IfCondition, UnlessCondition
+import launch_ros
 from launch_ros.actions import Node
 from launch.actions import ExecuteProcess
 from ament_index_python.packages import get_package_share_directory
 from moveit_configs_utils import MoveItConfigsBuilder
 
+from launch_param_builder import ParameterBuilder
+
+
+_PANDA_MOVEIT_CONFIG_RSC = "moveit_resources_panda_moveit_config"
+_PARAM_SHAPEBUFFER_WAITTIME = "robot_description_planning/shape_transform_cache_lookup_wait_time"
+
+def _octomap_launch_params(params: ParameterBuilder):
+    #params.yaml("config/sensors_kinect_pointcloud.yaml")
+    #params.yaml("config/sensors_3d.yaml")  # Done in MoveItConfigsBuilder instead.
+    params.parameter("octomap_frame", "camera_rgb_optical_frame")
+    params.parameter("octomap_resolution", 0.05)
+    params.parameter("max_range", 5.0)
+    return params.to_dict()
 
 def generate_launch_description():
 
@@ -39,24 +53,35 @@ def generate_launch_description():
         .robot_description_semantic(file_path="config/panda.srdf")
         .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")
         .planning_pipelines(
-            pipelines=["ompl", "chomp", "pilz_industrial_motion_planner", "stomp"]
-        )
+            pipelines=["ompl", "chomp", "pilz_industrial_motion_planner", "stomp"])
+        .sensors_3d("config/sensors_3d.yaml")
         .to_moveit_configs()
     )
 
+    _params_movegroup = ParameterBuilder(_PANDA_MOVEIT_CONFIG_RSC)
+
     # 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()],
+        parameters=(
+            [moveit_config.to_dict()] + 
+            [_octomap_launch_params(_params_movegroup)] +
+            # Accepting parameter https://github.com/ros-planning/moveit_tutorials/pull/633/files#diff-18137573adf3517e2254c0e1c75458e06b4571feed5a0b20ef0441fe2776aed7R7
+            # Because Parameter in ROS2 must be defined nested under a node, downstream
+            # should be able to pass parameter request to where a node is defined.
+            [{_PARAM_SHAPEBUFFER_WAITTIME: LaunchConfiguration(
+                'shape_transform_cache_lookup_wait_time')}] +
+            [{"use_sim_time": False}]
+            ),
         arguments=["--ros-args", "--log-level", "info"],
     )
 
     # RViz
     tutorial_mode = LaunchConfiguration("rviz_tutorial")
     rviz_base = os.path.join(
-        get_package_share_directory("moveit_resources_panda_moveit_config"), "launch"
+        get_package_share_directory(_PANDA_MOVEIT_CONFIG_RSC), "launch"
     )
     rviz_full_config = os.path.join(rviz_base, "moveit.rviz")
     rviz_empty_config = os.path.join(rviz_base, "moveit_empty.rviz")

panda_moveit_config/package.xml

@@ -31,6 +31,7 @@
   -->
   <exec_depend>joint_state_publisher</exec_depend>
   <exec_depend>joint_state_publisher_gui</exec_depend>
+  <exec_depend>launch_param_builder</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>xacro</exec_depend>
   <!-- TODO(#40): Package not available in ROS 2, find equivalent when migrating launch files