diff --git a/config/sensors_kinect_depthmap.yaml b/config/sensors_kinect_depthmap.yaml index 68b7f06bbe4b25bd66817597bc1afe2341d40d2f..9538fe0ac91bf6ebe587516b7d05356f5fe565a2 100644 --- a/config/sensors_kinect_depthmap.yaml +++ b/config/sensors_kinect_depthmap.yaml @@ -1,10 +1,11 @@ sensors: - sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater - image_topic: /head_mount_kinect/depth_registered/image_raw + 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/config/sensors_kinect_pointcloud.yaml b/config/sensors_kinect_pointcloud.yaml index 103344215d9a1e2a0e5095990e5601f2647767fe..92e7095a60ba5fa22d64039189caba902cbf1d21 100644 --- a/config/sensors_kinect_pointcloud.yaml +++ b/config/sensors_kinect_pointcloud.yaml @@ -1,8 +1,9 @@ sensors: - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater - point_cloud_topic: /head_mount_kinect/depth_registered/points + 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 diff --git a/launch/sensor_manager.launch.xml b/launch/sensor_manager.launch.xml index 616fcf01038fe9af853d4965f5e3d1eb38fd5d5b..67aa2fd26eecac5ebeb7ef29b10820dae693d684 100644 --- a/launch/sensor_manager.launch.xml +++ b/launch/sensor_manager.launch.xml @@ -2,9 +2,13 @@ <!-- This file makes it easy to include the settings for sensor managers --> + <!-- Param for kinect config --> + <rosparam command="load" file="$(find panda_moveit_config)/config/sensors_kinect_pointcloud.yaml" /> + <!-- Params for the octomap monitor --> <!-- <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> --> - <param name="octomap_resolution" type="double" value="0.025" /> + <param name="octomap_frame" type="string" value="camera_rgb_optical_frame" /> + <param name="octomap_resolution" type="double" value="0.05" /> <param name="max_range" type="double" value="5.0" /> <!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->