Ask Your Question
0

Controllers fail during execution when using octomap with Moveit

asked 2018-04-21 11:56:41 -0500

Shreeyak gravatar image

updated 2018-04-21 12:50:08 -0500

I'm using MoveIt with the default RRTConnectkConfigDefault motion planning library. I have a 6 DoF arm to which I pass target poses using roscpp's MoveGroupInterface. I'm using ros_control and have created my own Fake Controllers of the type FollowJointTrajectory. The target poses are acquired from the readings of a depth camera in Gazebo.

By default, I do not use the octomap using the depth cam. In this cases, MoveIt is able to generate the plans and also execute them successfully. I can see the arm moving in Rviz. I do not have an arm in Gazebo, it's only loaded in Rviz.

When I use the octomap, MoveIt can generate the plans, but fails during execution. All the joint states are being published on topic /joint_states. The same code works when I remove the sensors.yaml file and don't use the octomap. The controllers are up. I don't see any other errors in the terminal. Please help me identify the cause.

EDIT: When I increase the resolution of the octomap from 0.01m to 0.05m, then suddenly things start to work? I changed the value in moveit_config/sensor_manager.launch file as so:

<param name="octomap_resolution" type="double" value="0.05" />

I'd like to make things work at 0.01m resolution, it looks granular enough.


Here are the error messages: Error from MoveGroupInterface terminal where I pass in commands:

[ INFO] [1524323290.317619120, 261.964000000]: Ready to take commands for planning group arm.
[ INFO] [1524323293.819272021, 265.299000000]: 3D Co-ords of next target: X: 0.487010, Y: -0.132180, Z:-0.215447
[ INFO] [1524323303.789838914, 275.190000000]: ABORTED: Solution found but controller failed during execution

Error from the MoveIt terminal:

  [ INFO] [1524323029.558940862, 3.456000000]: Starting scene monitor
  [ INFO] [1524323029.562571287, 3.460000000]: Listening to '/move_group/monitored_planning_scene'
  [ INFO] [1524323033.952942898, 7.842000000]: Constructing new MoveGroup connection for group 'arm' in namespace ''
  [ INFO] [1524323035.143276258, 9.027000000]: Ready to take commands for planning group arm.
  [ INFO] [1524323035.143337694, 9.027000000]: Looking around: no
  [ INFO] [1524323035.143357118, 9.027000000]: Replanning: no
  [New Thread 0x7fffbef7e700 (LWP 20461)]
  [Wrn] [Publisher.cc:141] Queue limit reached for topic /gazebo/empty/pose/local/info, deleting message. This warning is printed only once.
  [ WARN] [1524323072.113526614, 45.706000000]: Failed to fetch current robot state.
  [ INFO] [1524323072.113676595, 45.706000000]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
  Debug:   Starting goal sampling thread
  [New Thread 0x7fffb3fff700 (LWP 21008)]
  Debug:   Waiting for space information to be set up before the sampling thread can begin computation...
  [ INFO] [1524323072.116232595, 45.709000000]: Planner configuration 'arm[RRTConnectkConfigDefault]' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
  Debug:   The value of parameter 'longest_valid_segment_fraction' is now: '0.0050000000000000001'
  Debug:   The value of parameter 'range' is now: '0'
  Debug:   arm[RRTConnectkConfigDefault]: Planner range detected to be 4.017020
  Info:    arm[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
  Debug:   arm[RRTConnectkConfigDefault]: Waiting for goal region samples ...
  Debug:   Beginning sampling thread ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-04-21 14:14:26 -0500

gvdhoorn gravatar image

The octomap pointcloud updater locks the planning scene during updates. I'm going to take a guess and say that you have a depth sensors publishing pointclouds at 20 to 30 Hz. If that is the case, the 'rest of MoveIt' doesn't get a chance to do some useful work before the updater locks the scene again.

Try decreasing the framerate and see if that works any better.

edit flag offensive delete link more

Comments

I'm publishing the point clouds at 6Hz and am running on a fairly powerful PC. Here's a snippet from my camera.gazebo.xacro file where I configure the depth cam plugin:

<update_rate>6.0</update_rate>

Shreeyak gravatar imageShreeyak ( 2018-04-21 14:33:18 -0500 )edit

Same value also in the libgazebo_ros_openni_kinect.so plugin config, 6Hz.

Shreeyak gravatar imageShreeyak ( 2018-04-21 14:34:15 -0500 )edit

Also, if I were to use a real depth camera, like the realsense, how would I configure it? Would I need to limit the framerate on the real camera as well?

Shreeyak gravatar imageShreeyak ( 2018-04-21 14:35:30 -0500 )edit

Just to test, set the framerate to 1.0 Hz. See what happens. The fact that lowering the resolution of the octomap helps suggests this has something to do with processing capacity.

gvdhoorn gravatar imagegvdhoorn ( 2018-04-22 04:08:49 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2018-04-21 11:56:41 -0500

Seen: 353 times

Last updated: Apr 21 '18