3D Perception: Move Group not subscribing to 3D sensor data

MatthewVerbryke commented 6 years ago

Description

Hello,

I have been trying to integrate my robot model with MoveIt!, but for the past month or so, I have been stuck on configuring the 3D perception to work with my robot's Kinect sensor, and I currently have no idea what to do next.

I started out by following the 3D perception tutorial, but when I launch MoveIt! with Rviz, no 3D data appears in Rviz. I checked rqt_graph, which shows that my Kinect sensor is publishing point cloud data. However, nothing is subscribed to this data, and the Octomap server isn't running either. When I explicitly run the Octomap server, move_group still doesn't seem to subscribe to any of the data. (the tutorials make it sound like I don't have to run the server on my own)

What else is needed to get the move group to subscribe and use this data? It seems like something is missing from this tutorial.

Note: I have opened a thread at ROS answers, but I haven't gotten any responses yet.

Relevant Code:

config/sensors_kinect.yaml

sensors:
  - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
    point_cloud_topic: /kinect/depth/points
    max_range: 5.0
    point_subsample: 1
    padding_offset: 0.1
    padding_scale: 1.0
    filtered_cloud_topic: filtered_cloud

launch/sensor_manager.launch.xml

<launch>

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

  <param name="octomap_frame" type="string" value="base_link" />
  <param name="octomap_resolution" type="double" value="0.025" />
  <param name="max_range" type="double" value="5.0" />

</launch>

Your environment

ROS Distro: Kinetic
OS Version: Ubuntu 16.04

Expected behaviour

Octomap data appearing in Rviz when running my MoveIt! configuration, and this data being used during trajectory planning.

Actual behaviour

No sensor data appears in the Rviz window when running MoveIt! configuration

v4hn commented 6 years ago

The tutorial should work (we tested it recently) and you don't have to start anything yourself. Just add the parameters and move_group automatically configures itself on startup.

If that does not happen, look through the command line output of move_group. There should be an info or error message that explains what went wrong.

MatthewVerbryke commented 6 years ago

Thank you for your response. After looking through the output feed for MoveIt! when launching, I'm seeing this error buried in there:

[ INFO] [1511789623.795560774, 126.117000000]: Listening to '/move_group/monitored_planning_scene'
[ INFO] [1511789623.796756204, 126.117000000]: waitForService: Service [/get_planning_scene] has not been advertised, waiting...
[ INFO] [1511789631.141291266, 131.128000000]: Failed to call service get_planning_scene, have you launched move_group? at /tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.9/planning_scene_monitor/src/planning_scene_monitor.cpp:486

If the tutorials are working properly, what could be the cause of this?

v4hn commented 6 years ago

Hm, that error message is not directly related, although it implies that your move_group node either does not start or does not respond.

MatthewVerbryke commented 6 years ago

Okay, I put my current Moveit configuration aside, and started from scratch from the setup assistant. After completing that, I launched the demo.launch without touching any of the auto-generated files, and I'm still getting the same output, with the same failure to call 'get_planning_scene' the only thing really sticking out to me. However it does seem that move_group is launching and at least somewhat functional, as I am able to plan and execute trajectory commands.

(partial) terminal output:

auto-starting new master
process[master]: started with pid [6927]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 361c9d8c-daa1-11e7-a25f-107b444ff29c
process[rosout-1]: started with pid [6940]
started core service [/rosout]
process[joint_state_publisher-2]: started with pid [6957]
process[robot_state_publisher-3]: started with pid [6958]
process[move_group-4]: started with pid [6959]
process[rviz_catalyst2_6913_7206489652998519380-5]: started with pid [6966]
[ INFO] [1512577160.917454223]: rviz version 1.12.13
[ INFO] [1512577160.917489186]: compiled against Qt version 5.5.1
[ INFO] [1512577160.917498810]: compiled against OGRE version 1.9.0 (Ghadamon)
[ INFO] [1512577160.928758629]: Loading robot model 'inmoov'...
[ INFO] [1512577160.928878092]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1512577161.142139127]: Stereo is NOT SUPPORTED
[ INFO] [1512577161.142388207]: OpenGl version: 4.5 (GLSL 4.5).
[ INFO] [1512577161.782969602]: Loading robot model 'inmoov'...
[ INFO] [1512577161.783020318]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1512577162.409817705]: Loading robot model 'inmoov'...
[ INFO] [1512577162.409882718]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1512577164.784319180]: Loading robot model 'inmoov'...
[ INFO] [1512577164.784546792]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1512577165.700038925]: Loading robot model 'inmoov'...
[ INFO] [1512577165.700088056]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1512577166.080850718]: Publishing maintained planning scene on 'monitored_planning_scene'
[ INFO] [1512577166.084253938]: MoveGroup debug mode is ON
Starting context monitors...
[ INFO] [1512577166.084297087]: Starting scene monitor
[ INFO] [1512577166.086892281]: Listening to '/planning_scene'
[ INFO] [1512577166.086935538]: Starting world geometry monitor
[ INFO] [1512577166.090047873]: Listening to '/collision_object' using message notifier with target frame '/world '
[ INFO] [1512577166.093703312]: Listening to '/planning_scene_world' for planning scene world geometry
[ INFO] [1512577166.407541945]: Loading robot model 'inmoov'...
[ INFO] [1512577166.407585454]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1512577170.321595514]: Starting scene monitor
[ INFO] [1512577170.324586451]: Listening to '/move_group/monitored_planning_scene'
[ INFO] [1512577170.325385640]: waitForService: Service [/get_planning_scene] has not been advertised, waiting...
[ INFO] [1512577175.334159461]: Failed to call service get_planning_scene, have you launched move_group? at /tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.9/planning_scene_monitor/src/planning_scene_monitor.cpp:486
[ INFO] [1512577176.496992895]: Listening to '/attached_collision_object' for attached collision objects
Context monitors started.
[ INFO] [1512577176.565201840]: Initializing OMPL interface using ROS parameters
[ INFO] [1512577176.650501968]: Using planning interface 'OMPL'
[ INFO] [1512577176.684624337]: Param 'default_workspace_bounds' was not set. Using default value: 10
[ INFO] [1512577176.685182688]: Param 'start_state_max_bounds_error' was set to 0.1
[ INFO] [1512577176.685807013]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1512577176.686290995]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1512577176.686749912]: Param 'jiggle_fraction' was set to 0.05
[ INFO] [1512577176.687217096]: Param 'max_sampling_attempts' was not set. Using default value: 100
[ INFO] [1512577176.687304434]: Using planning request adapter 'Add Time Parameterization'
[ INFO] [1512577176.687398750]: Using planning request adapter 'Fix Workspace Bounds'
[ INFO] [1512577176.687440061]: Using planning request adapter 'Fix Start State Bounds'
[ INFO] [1512577176.687479256]: Using planning request adapter 'Fix Start State In Collision'
[ INFO] [1512577176.687518634]: Using planning request adapter 'Fix Start State Path Constraints'
[ INFO] [1512577176.725615993]: Fake controller 'fake_right_arm_controller' with joints [ right_shoulder_side right_shoulder_up right_bicep_rotate right_elbow right_wrist ]
[ INFO] [1512577176.726077134]: Fake controller 'fake_left_arm_controller' with joints [ left_shoulder_side left_shoulder_up left_bicep_rotate left_elbow left_wrist ]
[ INFO] [1512577176.726499334]: Fake controller 'fake_head_controller' with joints [ head_tilt head_updown head_leftright ]
[ INFO] [1512577176.727026717]: Fake controller 'fake_hip_controller' with joints [ waist_rotate waist_lean ]
[ INFO] [1512577176.727501289]: Fake controller 'fake_r_hand_controller' with joints [ right_indexPIP right_middlePIP right_pinkyMCP right_ringMCP right_thumbMCP ]
[ INFO] [1512577176.727916915]: Fake controller 'fake_l_hand_controller' with joints [ left_indexPIP left_middlePIP left_pinkyMCP left_ringMCP left_thumbMCP ]
[ INFO] [1512577176.728376062]: Returned 6 controllers in list
[ INFO] [1512577176.739292773]: Trajectory execution is managing controllers
Loading 'move_group/ApplyPlanningSceneService'...
Loading 'move_group/ClearOctomapService'...
Loading 'move_group/MoveGroupCartesianPathService'...
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] [1512577176.870658588]: 

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

[ INFO] [1512577176.871759704]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[ INFO] [1512577176.871780871]: MoveGroup context initialization complete

You can start planning now!

MatthewVerbryke commented 6 years ago

Okay, Taking a break from a day or two and delving into it today, I found my issue. It appears that I had a few things that were screwed up in my MoveIt configuration and launch files that were causing this issue.

I'm not sure what caused the failure to call get_planning_scene, but when I trimmed my launch file down to just launching move_group and Rviz, MoveIt launched properly and found the planning scene.
After fixing that, it gave me the new error that the Fixed Frame [map] does not exist, so I added a static transform publisher and a joint state publisher.
Last but not least, I had the wrong camera name for my point cloud topic. In following the tutorial, I found I put down the point_cloud topic as /kinect/depth/points(similar to how its set up in the tutorial), but my gazebo model publishes it under the topic name inmoov/kinect/depth/points.

After fixing all of these issues, I managed to get sensor data through to the move_group/monitored_planning_scene and display in the rviz window.

Thank you for your help on this issue, and sorry that its was mostly my inexperience with moveit that caused it.

v4hn commented 6 years ago

Thank you for continue trying, that's equally important. Congrats on finding your problems. :-)

afdaniele commented 6 years ago

I have the same issue, I checked all the points above but I still don't see the 3D data in rviz. I went through all the launch files that I have and it seems that none of them are including the launch file robot_moveit_sensor_manager.launch. Is that the intended behavior? What is it loading? Thanks.

nis-ur commented 6 years ago

Hi Matthew. This is my problem exactly so it's good to hear that you got there in the end. When you say you trimmed your launch file down, which launch file was it that you trimmed? One of the files inside the UR_Moveit_config folder? @MatthewVerbryke

MatthewVerbryke commented 6 years ago

@afdaniele Sorry for the very late delay in responding, but yes as far as I can tell for a very basic implementation, $ROBOT$_moveit_sensor_manager.launch isn't used by anything.

@nis-ur Yes, it was moveit_planning_execution.launch, which is the main launch file for my configuration. Originally, I had a much more complicated file, based on someone else's configuration, but I found a working example in one of my lab-mates configs. To be honest, I knew next to nothing about roslaunch back then, so I really didn't quite understand what was going on in the launch file.

MatthewVerbryke commented 6 years ago

@joaocabogon Yeah you can find my launch file here

moveit / moveit