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 @@ + + + + + + + + + + + + + + + + + + diff --git a/panda_moveit_config/config/sensors_3d.yaml b/panda_moveit_config/config/sensors_3d.yaml new file mode 100644 index 00000000..d1c153dc --- /dev/null +++ b/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 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..fe5a6db7 100644 --- a/panda_moveit_config/config/sensors_kinect_pointcloud.yaml +++ b/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 \ No newline at end of file diff --git a/panda_moveit_config/launch/demo.launch.py b/panda_moveit_config/launch/demo.launch.py index 12e74017..ced40c6c 100644 --- a/panda_moveit_config/launch/demo.launch.py +++ b/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") 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