From fe43f7bd66c7a7d7b9d3382615505627e491ad67 Mon Sep 17 00:00:00 2001
From: "Isaac I.Y. Saito" <130s@2000.jukuin.keio.ac.jp>
Date: Fri, 9 Jun 2023 17:19:07 -0400
Subject: [PATCH 01/11] ros2-ify panda_moveit_config
---
.../config/sensors_kinect_depthmap.yaml | 24 ++++++++++---------
.../config/sensors_kinect_pointcloud.yaml | 20 +++++++++-------
2 files changed, 24 insertions(+), 20 deletions(-)
diff --git a/panda_moveit_config/config/sensors_kinect_depthmap.yaml b/panda_moveit_config/config/sensors_kinect_depthmap.yaml
index 9538fe0a..bee7be91 100644
--- a/panda_moveit_config/config/sensors_kinect_depthmap.yaml
+++ b/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
diff --git a/panda_moveit_config/config/sensors_kinect_pointcloud.yaml b/panda_moveit_config/config/sensors_kinect_pointcloud.yaml
index 92e7095a..7a210d54 100644
--- a/panda_moveit_config/config/sensors_kinect_pointcloud.yaml
+++ b/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
From 1cd754cd1489fe9f12ca7c229821f60df6c74e62 Mon Sep 17 00:00:00 2001
From: "Isaac I.Y. Saito" <130s@2000.jukuin.keio.ac.jp>
Date: Fri, 9 Jun 2023 14:36:18 -0400
Subject: [PATCH 02/11] Improve: panda_moveit: Add parameters for perception
tutorial
---
panda_moveit_config/launch/demo.launch.py | 24 ++++++++++++++++++++---
1 file changed, 21 insertions(+), 3 deletions(-)
diff --git a/panda_moveit_config/launch/demo.launch.py b/panda_moveit_config/launch/demo.launch.py
index 12e74017..bf9bd242 100644
--- a/panda_moveit_config/launch/demo.launch.py
+++ b/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")
From 646722b9d31aed2fdcd1591e64064a120f521051 Mon Sep 17 00:00:00 2001
From: "Isaac I.Y. Saito" <130s@2000.jukuin.keio.ac.jp>
Date: Sat, 10 Jun 2023 07:05:33 -0400
Subject: [PATCH 03/11] Adjust to upstream change
(https://github.com/frankaemika/franka_ros/issues/216)
---
panda_description/urdf/panda_arm.urdf.xacro | 18 ++++++++++++++++++
1 file changed, 18 insertions(+)
create mode 100644 panda_description/urdf/panda_arm.urdf.xacro
diff --git a/panda_description/urdf/panda_arm.urdf.xacro b/panda_description/urdf/panda_arm.urdf.xacro
new file mode 100644
index 00000000..1ea880d2
--- /dev/null
+++ b/panda_description/urdf/panda_arm.urdf.xacro
@@ -0,0 +1,18 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
From 4dd3eaa003b3a2cf3a03fa0e37dc8b5aba820451 Mon Sep 17 00:00:00 2001
From: "Isaac I.Y. Saito" <130s@2000.jukuin.keio.ac.jp>
Date: Fri, 16 Jun 2023 16:02:00 -0400
Subject: [PATCH 04/11] panda_moveit_config: Improve defining octomap params by
using 'launch_param_builder' pkg
---
panda_moveit_config/launch/demo.launch.py | 28 ++++++++++-------------
panda_moveit_config/package.xml | 1 +
2 files changed, 13 insertions(+), 16 deletions(-)
diff --git a/panda_moveit_config/launch/demo.launch.py b/panda_moveit_config/launch/demo.launch.py
index bf9bd242..92b85382 100644
--- a/panda_moveit_config/launch/demo.launch.py
+++ b/panda_moveit_config/launch/demo.launch.py
@@ -9,23 +9,17 @@
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"
-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 _octomap_launch_params(params: ParameterBuilder):
+ params.yaml("config/sensors_kinect_pointcloud.yaml")
+ params.parameter("octomap_frame", "odom_combined")
+ params.parameter("octomap_resolution", 0.05)
+ params.parameter("max_range", 5.0)
+ return params.to_dict()
def generate_launch_description():
@@ -62,12 +56,14 @@ def generate_launch_description():
.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()] + _octomap_launch_params(),
+ parameters=[moveit_config.to_dict()] + [_octomap_launch_params(_params_movegroup)],
arguments=["--ros-args", "--log-level", "info"],
)
diff --git a/panda_moveit_config/package.xml b/panda_moveit_config/package.xml
index 0b3ead36..c86e3cfa 100644
--- a/panda_moveit_config/package.xml
+++ b/panda_moveit_config/package.xml
@@ -31,6 +31,7 @@
-->
joint_state_publisher
joint_state_publisher_gui
+ launch_param_builder
robot_state_publisher
xacro