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_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,11 @@
-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
+/**:
+  sensors:
+    ros__parameters:
+      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

panda_moveit_config/launch/demo.launch.py

@@ -1,13 +1,31 @@
 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
 
+_PANDA_MOVEIT_CONFIG_RSC = "moveit_resources_panda_moveit_config"
+
+def _octomap_launch_params():
+    _path_panda_sensor_conf = PathJoinSubstitution(
+        [
+            launch_ros.substitutions.FindPackageShare(_PANDA_MOVEIT_CONFIG_RSC),
+            "config",
+            "sensors_kinect_pointcloud.yaml"
+        ])
+    _params = [
+        launch_ros.parameter_descriptions.ParameterFile(
+            param_file=_path_panda_sensor_conf,
+            allow_substs=True),
+        {"octomap_frame": "odom_combined"},
+        {"octomap_resolution": 0.05},
+        {"max_range": 5.0}]
+    return _params
 
 def generate_launch_description():
 
@@ -49,14 +67,14 @@ def generate_launch_description():
         package="moveit_ros_move_group",
         executable="move_group",
         output="screen",
-        parameters=[moveit_config.to_dict()],
+        parameters=[moveit_config.to_dict()] + _octomap_launch_params(),
         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")