0
$\begingroup$

Rosanswers logo

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 computation
  Debug:   Stopped goal sampling thread after 10 sampling attempts
  Debug:   arm[RRTConnectkConfigDefault]: Waited 0.040278 seconds for the first goal sample.
  [Thread 0x7fffb3fff700 (LWP 21008) exited]
  Info:    arm[RRTConnectkConfigDefault]: Created 4 states (2 start + 2 goal)
  Info:    Solution found in 0.043073 seconds
  Info:    SimpleSetup: Path simplification took 0.005992 seconds and changed from 3 to 17 states
  [ INFO] [1524323075.985180447, 49.557000000]: Execution request received for ExecuteTrajectory action.
  [ WARN] [1524323076.985406204, 50.550000000]: Failed to validate trajectory: couldn't receive full current joint state within 1s
  [ INFO] [1524323076.985470962, 50.550000000]: Execution completed: ABORTED
  [ WARN] [1524323081.883843029, 55.422000000]: Failed to fetch current robot state.
  [ INFO] [1524323081.883937069, 55.422000000]: Planning request received for MoveGroup action. Forwarding to planning pipeline.

Here's my sensors.yaml file:

# Add a depth camera to Rviz
sensors:
- sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
  point_cloud_topic: /camera/depth_registered/points
  max_range: 5
  padding_offset: 0.01
  padding_scale: 1.0
  point_subsample: 1
  filtered_cloud_topic: output_cloud
# this last topic is just to debug, having it adds processing time

And here's my controllers.yaml file:

controller_list:
    - name: "robot_follow_joint_trajectory_controller"
      action_ns: follow_joint_trajectory
      type: FollowJointTrajectory
      default: true
      joints:
          - robot_link1_joint
          - robot_link2_joint
          - robot_link3_joint
          - robot_link4_joint
          - robot_link5_joint
          - robot_link5_2_joint

Here's my robot_moveit_controller_manager.launch.xml

<launch>
  <!-- <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" /> -->
  <arg name="moveit_controller_manager" default="moveit_ros_control_interface::MoveItControllerManager" />
  
  <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)" />

  <rosparam file="$(find greppy_moveit_6dof)/config/controllers.yaml" />
</launch>

Originally posted by Shreeyak on ROS Answers with karma: 1 on 2018-04-21

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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.


Originally posted by gvdhoorn with karma: 86574 on 2018-04-21

This answer was ACCEPTED on the original site

Post score: 0


Original comments

Comment by Shreeyak on 2018-04-21:
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>

Comment by Shreeyak on 2018-04-21:
Same value also in the libgazebo_ros_openni_kinect.so plugin config, 6Hz.

Comment by Shreeyak on 2018-04-21:
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?

Comment by gvdhoorn on 2018-04-22:
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.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.