Fwd: MoveIt octomap not displayed in rviz (ROS indigo)

[Clément BAZERQUE]

Hi everybody,

I am working with ROS INDIGO, a Sawyer robot (Rethink robotics), a Kinect v1, and MoveIt. I am working on a Human-Robot Collaboration project.

After installing MoveIt, I have edited "camera_link_pose" from sawyer_moveit.launch to suit my base_to_camera transform. When I run:

roslaunch sawyer_moveit_config sawyer_moveit.launch electric_gripper:=true kinect:=true

Rviz appears, everything works except that the octomap isn't displayed:

rviz_capture.png

Here is what I have in the terminal:

~/ros_ws$ roslaunch sawyer_moveit_config sawyer_moveit.launch electric_gripper:=true kinect:=true

... logging to /home/sawyer/.ros/log/33db5810-3ace-11e7-860e-1866da494b18/roslaunch-sawyer-HP-Compaq-Pro-6300-SFF-20862.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://169.254.5.155:38982/

SUMMARY
========

PARAMETERS
 * /move_group/allow_trajectory_execution: True
 * /move_group/allowed_execution_duration_scaling: 1.2
 * /move_group/allowed_goal_duration_margin: 0.5
 * /move_group/camera/camera_nodelet_manager/num_worker_threads: 4
 * /move_group/camera/depth_rectify_depth/interpolation: 0
 * /move_group/camera/depth_registered_rectify_depth/interpolation: 0
 * /move_group/camera/disparity_depth/max_range: 4.0
 * /move_group/camera/disparity_depth/min_range: 0.5
 * /move_group/camera/disparity_registered_hw/max_range: 4.0
 * /move_group/camera/disparity_registered_hw/min_range: 0.5
 * /move_group/camera/disparity_registered_sw/max_range: 4.0
 * /move_group/camera/disparity_registered_sw/min_range: 0.5
 * /move_group/camera/driver/data_skip: 0
 * /move_group/camera/driver/debug: False
 * /move_group/camera/driver/depth_camera_info_url: 
 * /move_group/camera/driver/depth_frame_id: camera_depth_opti...
 * /move_group/camera/driver/depth_registration: False
 * /move_group/camera/driver/device_id: #1
 * /move_group/camera/driver/diagnostics_max_frequency: 30.0
 * /move_group/camera/driver/diagnostics_min_frequency: 30.0
 * /move_group/camera/driver/diagnostics_tolerance: 0.05
 * /move_group/camera/driver/diagnostics_window_time: 5.0
 * /move_group/camera/driver/enable_depth_diagnostics: False
 * /move_group/camera/driver/enable_ir_diagnostics: False
 * /move_group/camera/driver/enable_rgb_diagnostics: False
 * /move_group/camera/driver/rgb_camera_info_url: 
 * /move_group/camera/driver/rgb_frame_id: camera_rgb_optica...
 * /move_group/capabilities: move_group/MoveGr...
 * /move_group/controller_list: [{'default': True...
 * /move_group/controller_manager_name: simple_controller...
 * /move_group/head/planner_configs: ['SBLkConfigDefau...
 * /move_group/jiggle_fraction: 0.05
 * /move_group/max_range: 5.0
 * /move_group/max_safe_path_cost: 1
 * /move_group/move_group/octomap_frame: camera_link
 * /move_group/move_group/octomap_resolution: 0.02
 * /move_group/move_group/point_subsample: 1
 * /move_group/move_group/sensors: [{'max_range': 5....
 * /move_group/moveit_controller_manager: moveit_simple_con...
 * /move_group/moveit_manage_controllers: True
 * /move_group/octomap_resolution: 0.025
 * /move_group/planner_configs/BKPIECEkConfigDefault/border_fraction: 0.9
 * /move_group/planner_configs/BKPIECEkConfigDefault/failed_expansion_score_factor: 0.5
 * /move_group/planner_configs/BKPIECEkConfigDefault/min_valid_path_fraction: 0.5
 * /move_group/planner_configs/BKPIECEkConfigDefault/range: 0.0
 * /move_group/planner_configs/BKPIECEkConfigDefault/type: geometric::BKPIECE
 * /move_group/planner_configs/ESTkConfigDefault/goal_bias: 0.05
 * /move_group/planner_configs/ESTkConfigDefault/range: 0.0
 * /move_group/planner_configs/ESTkConfigDefault/type: geometric::EST
 * /move_group/planner_configs/KPIECEkConfigDefault/border_fraction: 0.9
 * /move_group/planner_configs/KPIECEkConfigDefault/failed_expansion_score_factor: 0.5
 * /move_group/planner_configs/KPIECEkConfigDefault/goal_bias: 0.05
 * /move_group/planner_configs/KPIECEkConfigDefault/min_valid_path_fraction: 0.5
 * /move_group/planner_configs/KPIECEkConfigDefault/range: 0.0
 * /move_group/planner_configs/KPIECEkConfigDefault/type: geometric::KPIECE
 * /move_group/planner_configs/LBKPIECEkConfigDefault/border_fraction: 0.9
 * /move_group/planner_configs/LBKPIECEkConfigDefault/min_valid_path_fraction: 0.5
 * /move_group/planner_configs/LBKPIECEkConfigDefault/range: 0.0
 * /move_group/planner_configs/LBKPIECEkConfigDefault/type: geometric::LBKPIECE
 * /move_group/planner_configs/PRMkConfigDefault/max_nearest_neighbors: 10
 * /move_group/planner_configs/PRMkConfigDefault/type: geometric::PRM
 * /move_group/planner_configs/PRMstarkConfigDefault/type: geometric::PRMstar
 * /move_group/planner_configs/RRTConnectkConfigDefault/range: 0.0
 * /move_group/planner_configs/RRTConnectkConfigDefault/type: geometric::RRTCon...
 * /move_group/planner_configs/RRTkConfigDefault/goal_bias: 0.05
 * /move_group/planner_configs/RRTkConfigDefault/range: 0.0
 * /move_group/planner_configs/RRTkConfigDefault/type: geometric::RRT
 * /move_group/planner_configs/RRTstarkConfigDefault/delay_collision_checking: 1
 * /move_group/planner_configs/RRTstarkConfigDefault/goal_bias: 0.05
 * /move_group/planner_configs/RRTstarkConfigDefault/range: 0.0
 * /move_group/planner_configs/RRTstarkConfigDefault/type: geometric::RRTstar
 * /move_group/planner_configs/SBLkConfigDefault/range: 0.0
 * /move_group/planner_configs/SBLkConfigDefault/type: geometric::SBL
 * /move_group/planner_configs/TRRTkConfigDefault/frountierNodeRatio: 0.1
 * /move_group/planner_configs/TRRTkConfigDefault/frountier_threshold: 0.0
 * /move_group/planner_configs/TRRTkConfigDefault/goal_bias: 0.05
 * /move_group/planner_configs/TRRTkConfigDefault/init_temperature: 10e-6
 * /move_group/planner_configs/TRRTkConfigDefault/k_constant: 0.0
 * /move_group/planner_configs/TRRTkConfigDefault/max_states_failed: 10
 * /move_group/planner_configs/TRRTkConfigDefault/min_temperature: 10e-10
 * /move_group/planner_configs/TRRTkConfigDefault/range: 0.0
 * /move_group/planner_configs/TRRTkConfigDefault/temp_change_factor: 2.0
 * /move_group/planner_configs/TRRTkConfigDefault/type: geometric::TRRT
 * /move_group/planning_plugin: ompl_interface/OM...
 * /move_group/planning_scene_monitor/publish_geometry_updates: True
 * /move_group/planning_scene_monitor/publish_planning_scene: True
 * /move_group/planning_scene_monitor/publish_state_updates: True
 * /move_group/planning_scene_monitor/publish_transforms_updates: True
 * /move_group/request_adapters: default_planner_r...
 * /move_group/right_arm/planner_configs: ['SBLkConfigDefau...
 * /move_group/start_state_max_bounds_error: 0.1
 * /move_group/use_controller_manager: True
 * /robot_description_kinematics/head/kinematics_solver: kdl_kinematics_pl...
 * /robot_description_kinematics/head/kinematics_solver_attempts: 3
 * /robot_description_kinematics/head/kinematics_solver_search_resolution: 0.005
 * /robot_description_kinematics/head/kinematics_solver_timeout: 0.005
 * /robot_description_kinematics/right_arm/kinematics_solver: kdl_kinematics_pl...
 * /robot_description_kinematics/right_arm/kinematics_solver_attempts: 10
 * /robot_description_kinematics/right_arm/kinematics_solver_search_resolution: 0.005
 * /robot_description_kinematics/right_arm/kinematics_solver_timeout: 0.005
 * /robot_description_planning/joint_limits/right_j0/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/right_j0/has_velocity_limits: True
 * /robot_description_planning/joint_limits/right_j0/max_acceleration: 3.5
 * /robot_description_planning/joint_limits/right_j0/max_velocity: 0.88
 * /robot_description_planning/joint_limits/right_j1/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/right_j1/has_velocity_limits: True
 * /robot_description_planning/joint_limits/right_j1/max_acceleration: 2.5
 * /robot_description_planning/joint_limits/right_j1/max_velocity: 0.678
 * /robot_description_planning/joint_limits/right_j2/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/right_j2/has_velocity_limits: True
 * /robot_description_planning/joint_limits/right_j2/max_acceleration: 5.0
 * /robot_description_planning/joint_limits/right_j2/max_velocity: 0.996
 * /robot_description_planning/joint_limits/right_j3/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/right_j3/has_velocity_limits: True
 * /robot_description_planning/joint_limits/right_j3/max_acceleration: 5.0
 * /robot_description_planning/joint_limits/right_j3/max_velocity: 0.996
 * /robot_description_planning/joint_limits/right_j4/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/right_j4/has_velocity_limits: True
 * /robot_description_planning/joint_limits/right_j4/max_acceleration: 5.0
 * /robot_description_planning/joint_limits/right_j4/max_velocity: 1.776
 * /robot_description_planning/joint_limits/right_j5/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/right_j5/has_velocity_limits: True
 * /robot_description_planning/joint_limits/right_j5/max_acceleration: 5.0
 * /robot_description_planning/joint_limits/right_j5/max_velocity: 1.776
 * /robot_description_planning/joint_limits/right_j6/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/right_j6/has_velocity_limits: True
 * /robot_description_planning/joint_limits/right_j6/max_acceleration: 5.0
 * /robot_description_planning/joint_limits/right_j6/max_velocity: 2.316
 * /robot_description_semantic: <?xml version="1....
 * /rosdistro: indigo
 * /rosversion: 1.11.21
 * /rviz_sawyer_HP_Compaq_Pro_6300_SFF_20862_7403073962417655334/head/kinematics_solver: kdl_kinematics_pl...
 * /rviz_sawyer_HP_Compaq_Pro_6300_SFF_20862_7403073962417655334/head/kinematics_solver_attempts: 3
 * /rviz_sawyer_HP_Compaq_Pro_6300_SFF_20862_7403073962417655334/head/kinematics_solver_search_resolution: 0.005
 * /rviz_sawyer_HP_Compaq_Pro_6300_SFF_20862_7403073962417655334/head/kinematics_solver_timeout: 0.005
 * /rviz_sawyer_HP_Compaq_Pro_6300_SFF_20862_7403073962417655334/right_arm/kinematics_solver: kdl_kinematics_pl...
 * /rviz_sawyer_HP_Compaq_Pro_6300_SFF_20862_7403073962417655334/right_arm/kinematics_solver_attempts: 10
 * /rviz_sawyer_HP_Compaq_Pro_6300_SFF_20862_7403073962417655334/right_arm/kinematics_solver_search_resolution: 0.005
 * /rviz_sawyer_HP_Compaq_Pro_6300_SFF_20862_7403073962417655334/right_arm/kinematics_solver_timeout: 0.005

NODES
  /move_group/camera/
    camera_nodelet_manager (nodelet/nodelet)
    depth_metric (nodelet/nodelet)
    depth_metric_rect (nodelet/nodelet)
    depth_points (nodelet/nodelet)
    depth_rectify_depth (nodelet/nodelet)
    depth_registered_hw_metric_rect (nodelet/nodelet)
    depth_registered_metric (nodelet/nodelet)
    depth_registered_rectify_depth (nodelet/nodelet)
    depth_registered_sw_metric_rect (nodelet/nodelet)
    disparity_depth (nodelet/nodelet)
    disparity_registered_hw (nodelet/nodelet)
    disparity_registered_sw (nodelet/nodelet)
    driver (nodelet/nodelet)
    ir_rectify_ir (nodelet/nodelet)
    points_xyzrgb_hw_registered (nodelet/nodelet)
    points_xyzrgb_sw_registered (nodelet/nodelet)
    register_depth_rgb (nodelet/nodelet)
    rgb_debayer (nodelet/nodelet)
    rgb_rectify_color (nodelet/nodelet)
    rgb_rectify_mono (nodelet/nodelet)
  /move_group/
    camera_base_link (tf/static_transform_publisher)
    camera_base_link1 (tf/static_transform_publisher)
    camera_base_link2 (tf/static_transform_publisher)
    camera_base_link3 (tf/static_transform_publisher)
    camera_link_broadcaster (tf/static_transform_publisher)
  /
    move_group (moveit_ros_move_group/move_group)
    rviz_sawyer_HP_Compaq_Pro_6300_SFF_20862_7403073962417655334 (rviz/rviz)

ROS_MASTER_URI=http://021611CP00073.local:11311

core service [/rosout] found
process[move_group/camera/camera_nodelet_manager-1]: started with pid [20874]
process[move_group/camera/driver-2]: started with pid [20875]
process[move_group/camera/rgb_debayer-3]: started with pid [20876]
process[move_group/camera/rgb_rectify_mono-4]: started with pid [20877]
process[move_group/camera/rgb_rectify_color-5]: started with pid [20878]
process[move_group/camera/ir_rectify_ir-6]: started with pid [20879]
process[move_group/camera/depth_rectify_depth-7]: started with pid [20883]
process[move_group/camera/depth_metric_rect-8]: started with pid [20890]
process[move_group/camera/depth_metric-9]: started with pid [20891]
process[move_group/camera/depth_points-10]: started with pid [20902]
process[move_group/camera/register_depth_rgb-11]: started with pid [20906]
process[move_group/camera/points_xyzrgb_sw_registered-12]: started with pid [20907]
process[move_group/camera/depth_registered_sw_metric_rect-13]: started with pid [20919]
process[move_group/camera/depth_registered_rectify_depth-14]: started with pid [20923]
process[move_group/camera/points_xyzrgb_hw_registered-15]: started with pid [20926]
[ INFO] [1495016462.598547792]: Initializing nodelet with 4 worker threads.
process[move_group/camera/depth_registered_hw_metric_rect-16]: started with pid [20927]
process[move_group/camera/depth_registered_metric-17]: started with pid [20941]
process[move_group/camera/disparity_depth-18]: started with pid [20946]
process[move_group/camera/disparity_registered_sw-19]: started with pid [20948]
process[move_group/camera/disparity_registered_hw-20]: started with pid [20952]
process[move_group/camera_base_link-21]: started with pid [20953]
process[move_group/camera_base_link1-22]: started with pid [20965]
process[move_group/camera_base_link2-23]: started with pid [20969]
process[move_group/camera_base_link3-24]: started with pid [20980]
process[move_group/camera_link_broadcaster-25]: started with pid [20984]
process[move_group-26]: started with pid [20986]
process[rviz_sawyer_HP_Compaq_Pro_6300_SFF_20862_7403073962417655334-27]: started with pid [20993]
[ INFO] [1495016462.693509168]: Number devices connected: 1
[ INFO] [1495016462.693602953]: 1. device on bus 000:00 is a Xbox NUI Camera (2ae) from Microsoft (45e) with serial id 'A00366907653103A'
[ INFO] [1495016462.694673734]: Searching for device with index = 1

(rviz:20993): Gtk-WARNING **: Unable to locate theme engine in module_path: "adwaita",
[ INFO] [1495016462.750378327]: rviz version 1.11.15
[ INFO] [1495016462.750424452]: compiled against Qt version 4.8.6
[ INFO] [1495016462.750436423]: compiled against OGRE version 1.8.1 (Byatis)
[ INFO] [1495016462.778280205]: Starting a 3s RGB and Depth stream flush.
[ INFO] [1495016462.778359537]: Opened 'Xbox NUI Camera' on bus 0:0 with serial number 'A00366907653103A'
[ INFO] [1495016462.779418408]: Loading robot model 'sawyer'...
[ INFO] [1495016462.859296395]: Stereo is NOT SUPPORTED
[ INFO] [1495016462.859370647]: OpenGl version: 3 (GLSL 1.3).
[ WARN] [1495016462.895187551]: Could not find any compatible depth output mode for 1. Falling back to default depth output mode 1.
[ INFO] [1495016462.938451761]: Loading robot model 'sawyer'...
[ INFO] [1495016462.996939730]: Publishing maintained planning scene on 'monitored_planning_scene'
[ INFO] [1495016463.005977717]: MoveGroup debug mode is ON
Starting context monitors...
[ INFO] [1495016463.006036440]: Starting scene monitor
[ INFO] [1495016463.016688848]: Listening to '/planning_scene'
[ INFO] [1495016463.016734632]: Starting world geometry monitor
[ INFO] [1495016463.025046719]: Listening to '/collision_object' using message notifier with target frame '/base '
[ INFO] [1495016463.033351917]: Listening to '/planning_scene_world' for planning scene world geometry
[ INFO] [1495016463.062230866]: Listening to '/attached_collision_object' for attached collision objects
Context monitors started.
[ INFO] [1495016463.110704157]: Initializing OMPL interface using ROS parameters
[ INFO] [1495016463.159454318]: Using planning interface 'OMPL'
[ INFO] [1495016463.189799983]: Param 'default_workspace_bounds' was not set. Using default value: 10
[ INFO] [1495016463.192609516]: Param 'start_state_max_bounds_error' was set to 0.1
[ INFO] [1495016463.195020096]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1495016463.197465266]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1495016463.201089427]: Param 'jiggle_fraction' was set to 0.05
[ INFO] [1495016463.203597709]: Param 'max_sampling_attempts' was not set. Using default value: 100
[ INFO] [1495016463.203654817]: Using planning request adapter 'Add Time Parameterization'
[ INFO] [1495016463.203682529]: Using planning request adapter 'Fix Workspace Bounds'
[ INFO] [1495016463.203711417]: Using planning request adapter 'Fix Start State Bounds'
[ INFO] [1495016463.203723132]: Using planning request adapter 'Fix Start State In Collision'
[ INFO] [1495016463.203761613]: Using planning request adapter 'Fix Start State Path Constraints'
[ WARN] [1495016463.215644770]: 
Deprecation warning: parameter 'allowed_execution_duration_scaling' moved into namespace 'trajectory_execution'.
Please, adjust file trajectory_execution.launch.xml!
[ WARN] [1495016463.217830741]: 
Deprecation warning: parameter 'allowed_goal_duration_margin' moved into namespace 'trajectory_execution'.
Please, adjust file trajectory_execution.launch.xml!
[ INFO] [1495016463.481330910]: Added FollowJointTrajectory controller for /robot/limb/right
[ INFO] [1495016463.481450028]: Returned 1 controllers in list
[ INFO] [1495016463.516304840]: Trajectory execution is managing controllers
Loading 'move_group/ApplyPlanningSceneService'...
Loading 'move_group/ClearOctomapService'...
Loading 'move_group/MoveGroupCartesianPathService'...
Loading 'move_group/MoveGroupExecuteService'...
Loading 'move_group/MoveGroupExecuteTrajectoryAction'...
Loading 'move_group/MoveGroupGetPlanningSceneService'...
Loading 'move_group/MoveGroupKinematicsService'...
Loading 'move_group/MoveGroupMoveAction'...
Loading 'move_group/MoveGroupPickPlaceAction'...
Loading 'move_group/MoveGroupPlanService'...
Loading 'move_group/MoveGroupQueryPlannersService'...
Loading 'move_group/MoveGroupStateValidationService'...
[ INFO] [1495016463.767432287]: 

********************************************************
* MoveGroup using: 
*     - ApplyPlanningSceneService
*     - ClearOctomapService
*     - CartesianPathService
*     - ExecuteTrajectoryService
*     - ExecuteTrajectoryAction
*     - GetPlanningSceneService
*     - KinematicsService
*     - MoveAction
*     - PickPlaceAction
*     - MotionPlanService
*     - QueryPlannersService
*     - StateValidationService
********************************************************

[ INFO] [1495016463.767507058]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[ INFO] [1495016463.767531704]: MoveGroup context initialization complete

All is well! Everyone is happy! You can start planning now!

[ INFO] [1495016463.848688873]: rgb_frame_id = 'camera_rgb_optical_frame' 
[ INFO] [1495016463.848798099]: depth_frame_id = 'camera_depth_optical_frame' 
[ WARN] [1495016463.876687681]: Camera calibration file /home/sawyer/.ros/camera_info/rgb_A00366907653103A.yaml not found.
[ WARN] [1495016463.876777590]: Using default parameters for RGB camera calibration.
[ WARN] [1495016463.876814517]: Camera calibration file /home/sawyer/.ros/camera_info/depth_A00366907653103A.yaml not found.
[ WARN] [1495016463.876862956]: Using default parameters for IR camera calibration.
[ INFO] [1495016465.779364068]: Stopping device RGB and Depth stream flush.
[ INFO] [1495016466.428636329]: Loading robot model 'sawyer'...
[ INFO] [1495016466.532779604]: Loading robot model 'sawyer'...
[ INFO] [1495016466.585212741]: Starting scene monitor
[ INFO] [1495016466.592453780]: Listening to '/move_group/monitored_planning_scene'
[ INFO] [1495016467.461837577]: No active joints or end effectors found for group ''. Make sure you have defined an end effector in your SRDF file and that kinematics.yaml is loaded in this node's namespace.
[ INFO] [1495016467.469390902]: Constructing new MoveGroup connection for group 'right_arm' in namespace ''
[ INFO] [1495016468.301998541]: TrajectoryExecution will use old service capability.
[ INFO] [1495016468.302066666]: Ready to take MoveGroup commands for group right_arm.
[ INFO] [1495016468.302105415]: Looking around: no
[ INFO] [1495016468.302133111]: Replanning: no

I think I have tried almost everything to make it work. Here is some documentation I've been through:

MoveIt tutorials: http:// docs.ros.org/kinetic/api/moveit_tutorials/html/doc/pr2_tutorials/planning/src/doc/perception_configuration.html

MoveIt google group: https:// groups.google.com/forum/#!forum/moveit-users

Baxter Kinect integration for MoveIt: http:// sdk.rethinkrobotics.com/wiki/Kinect_basics

MoveIt installation and tuto for Sawyer http:// sdk.rethinkrobotics.com/intera/MoveIt_Tutorial

It seems that I am not the only one not being able to see the octomap in rviz. Maybe someone here had already been confronted to this problem and know how to solve it ?

Thanks in advance !

Clément

[Clément BAZERQUE]

Also, the .yaml sensor file I am using contains:

sensors:

- sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater

  point_cloud_topic: /move_group/camera/depth_registered/points

  max_range: 5.0

  point_subsample: 1

  padding_offset: 0

  padding_scale: 1.0

  point_subsample: 1

  filtered_cloud_topic: filtered_cloud

Given that the octomap wasn't displayed, I supposed it was either an issue with octomap_server or with my point cloud.

Therefore, I made an octomap server based on the same point cloud topic:

<launch>

<node pkg="octomap_server" type="octomap_server_node" name="octomap_server">

<param name="resolution" value="0.05" />

<!-- fixed map frame (set to 'map' if SLAM or localization running!) -->

<param name="frame_id" type="string" value="camera_link" />

<!-- maximum range to integrate (speedup!) -->

<param name="sensor_model/max_range" value="5.0" />

<!-- data source to integrate (PointCloud2) -->

<remap from="cloud_in" to="move_group/camera/depth_registered/points" />

</node>

</launch>

If I add an occupancyGrip on rviz and subscribe to /octomap_full I can see the generated octomap.

So maybe the initial issue isn't coming from my pointcloud nor octomap_server.

Any ideas ?

[yangta...@gmail.com]

Hello, I have comed to the same prolems, just like yours. In my baxter robot, it shows warn messages, it seems to be the tf problems, just like 
[ WARN] [1495607702.313989795]: Unable to transform object from frame 'camera_rgb_frame' to planning frame '/base', 

I have no idea about this.

在 2017年5月23日星期二 UTC+8下午6:40:57，Clément BAZERQUE写道：

[Clément BAZERQUE]

Thanks for the info. I just checked, the transform exist:

~/ros_ws$ rosrun tf tf_echo base camera_rgb_frame

At time 1495609768.655

- Translation: [1.605, 0.223, 0.841]

- Rotation: in Quaternion [-0.143, -0.090, 0.927, -0.336]

            in RPY (radian) [-0.075, 0.332, -2.459]

            in RPY (degree) [-4.297, 19.007, -140.867]

:~/ros_ws$ rosrun tf tf_echo camera_rgb_frame base

At time 1495609868.436

- Translation: [1.584, -0.812, -0.406]

- Rotation: in Quaternion [-0.143, -0.090, 0.927, 0.336]

            in RPY (radian) [-0.272, 0.206, 2.418]

            in RPY (degree) [-15.591, 11.807, 138.525]

Also, I check my frames using "rosrun tf view_frames" and using the tf plugin in rviz, everything is working.

( If you have set your base to camera transform well, maybe your problem comes from a bad clock synchronisation between robot and computer ? )

[yangta...@gmail.com]

Thank you for your response. I will try it. By the way, how to mody synchronisation between robot and computer?
在 2017年5月24日星期三 UTC+8下午3:20:00，Clément BAZERQUE写道：

[Clément BAZERQUE]

It depends of your robot. I am using Sawyer from Rethink Robotics. I had to set an NTP server on my computer (easier than it sounds) and configure Sawyer to connect to this NTP server (giving it the static IP of my computer).

You can run "ntpdate -q ip_of_your_robot" in a terminal first to see if there is a big offset between robot and computer clocks (mine is 0.02s in average...).

(you'll have to install ntpdate first of course)

[yangta...@gmail.com]

Sincerely thank you. As you said, I connect the robot to Internet and it automatically updates its time. My problem has been solved. Thank you again.

在 2017年5月25日星期四 UTC+8下午4:00:00，Clément BAZERQUE写道：

[Clément BAZERQUE]

You're welcome :)

Is the octomap displayed now from your side ? (I still haven't found any solution)

If your problem was related to tf, maybe mine is too. Especially, I suspect this line:

[ INFO] [1495016467.461837577]: No active joints or end effectors found for group ''. Make sure you have defined an end effector in your SRDF file and that kinematics.yaml is loaded in this node's namespace.

But I don't know how to correct this... 

[yangta...@gmail.com]

Yes, moveit works well now. I load the moveit_robots by the step in http://sdk.rethinkrobotics.com/wiki/MoveIt_Tutorial. I guess the your robot srdf defined in moveit may not be proper. Your robot moveit .srdf file was created by rethink official?

在 2017年5月25日星期四 UTC+8下午9:17:06，Clément BAZERQUE写道：

[Clément BAZERQUE]

Ok. Yes, the .srdf file was created by Rethink official.

They also provide .launch and .yaml files.

I contacted them this morning to see if they are aware of this issue.

I am waiting for their answer. Maybe you have an idea on how to solve it ?

[yangta...@gmail.com]

I have also ask rethink for help, but their response is not pleasure. Your problem seems to be the end effectors, so you can try to compare the real robot urdf file with moveit **.srdf files. the way you get the real robot urdf may be use:

rosparam get -p /robot_description | tail -n +2 > my.baxter.urdf(https://github.com/davetcoleman/baxter_cpp). 

If this can not solve your problem, I am sorry that I have no idea about your problem.

在 2017年5月25日星期四 UTC+8下午10:37:36，Clément BAZERQUE写道：

[Clément BAZERQUE]

Thanks for the hint. At first glance, the gripper is present in the real .urdf but is missing in the robot .urdf given by Rethink Robotics.

I replaced the default .urdf with the real one (nothing ventured, nothing gained), it didn't change anything.

I also checked line by line the .launch files. 

electric_gripper=true is used in order in sawyer_moveit.launch > planning_context.launch > sawyer.srdf.xacro which finally includes rethink_electric_gripper.srdf.xacro

I don't understand everything from these files, but their logic seems to be good. So I would say I am quite sure that the end effector is defined in the SRDF file.

No active joints or end effectors found for group ''. Make sure you have defined an end effector in your SRDF file and that kinematics.yaml is loaded in this node's namespace.

The last thing is to check if "kinematics.yaml is loaded in this node's namespace". I don't know how to check that.

"kinematics.yaml" only appears at the end of planning_context.launch file:

<?xml version="1.0"?>
<launch>
  <!-- Right electric gripper param. Set to true to check for collisions for their links -->
  <arg name="electric_gripper" default="false"/>
  <!-- Set the kinematic tips for the left_arm and right_arm move_groups -->
  <arg name="tip_name" default="right_hand"/>
  <!-- Add controller box collision shape to check for link collisions if set to true-->
  <arg name="controller_box" default="true"/>




  <!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
  <arg name="load_robot_description" default="false"/>


  <!-- The name of the parameter under which the URDF is loaded -->
  <arg name="robot_description" default="robot_description"/>


  <!-- Load universal robot description format (URDF) FIXME when composable URDF -->
  <param if="$(arg load_robot_description)" name="$(arg robot_description)" textfile="$(find sawyer_description)/urdf/sawyer.urdf"/>


  <!-- The semantic description that corresponds to the URDF -->
  <param name="robot_description_semantic"
      command="$(find xacro)/xacro --inorder $(find sawyer_moveit_config)/srdf/sawyer.srdf.xacro
          electric_gripper:=$(arg electric_gripper)
          tip_name:=$(arg tip_name)
          controller_box:=$(arg controller_box)"/>
  
  <!-- Load updated joint limits (override information from URDF) -->
  <group ns="$(arg robot_description)_planning">
    <rosparam command="load" file="$(find sawyer_moveit_config)/config/joint_limits.yaml"/>
  </group>


  <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
  <group ns="$(arg robot_description)_kinematics">
    <rosparam command="load" file="$(find sawyer_moveit_config)/config/kinematics.yaml"/>
  </group>
  
</launch>

If someone has an idea... 

Otherwise, I'll wait for Rethink Robotics answer and post the solution if there is one.

[Clément BAZERQUE]

I think I have spotted another problem. In sawyer_moveit_sensor_manager.launch.xml, I load my kinect_sensor.yaml which contains:

sensors:
 - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
   point_cloud_topic: /camera/depth_registered/points
   max_range: 2.0
   padding_offset: 0
   padding_scale: 3.0
   frame_subsample: 1
   point_subsample: 1

(I voluntary set max_range to 2.0m)

Now if I check these parameters with rosparam, they are not updated (max_range=5.0 instead of 2.0):

[Clément BAZERQUE]

Help ?

Le mercredi 17 mai 2017 17:27:25 UTC+2, Clément BAZERQUE a écrit :

[Clément BAZERQUE]

I'VE FOUND THE SOLUTION ! Finally!!! (by pure chance)

Ok, so here is the solution for us Sawyer users.

Rethink Robotics provides every launch files for interfacing Sawyer with MoveIt.

In the "sawyer_moveit_sensor_manager.launch.xml" they load the sensor configuration file "kinect_sensor.yaml" in a "move_group" namespace.

I removed this namespace by commenting it:

<!-- octomap parameters for moveit -->
         <!-- <group ns="move_group" > -->
             <param name="octomap_frame" type="string" value="camera_link" />
             <param name="octomap_resolution" type="double" value="0.02" />
             <param name="max_range" type="double" value="5.0" />


             <rosparam command="load" file="$(find sawyer_moveit_config)/config/kinect_sensor.yaml" />

         <!-- </group> -->

and in "kinect_sensor.yaml" I subscribed to "/move_group/camera/depth_registered/points"

That's it :) I hope this will help someone.

Note: it is interesting to see that the following lines still appear in the terminal:

[ INFO] [1497519534.574234701]: No active joints or end effectors found for group ''. Make sure you have defined an end effector in your SRDF file and that kinematics.yaml is loaded in this node's namespace.

Le mercredi 17 mai 2017 17:27:25 UTC+2, Clément BAZERQUE a écrit :