Moveit Octomap Not Allowing Command Planning or Execution
Hi all,
I've been trying to hook up Moveit so that my robot can avoid obstacles using openni_tracker and openni_launch. I've added octomap capabilities to my demo.launch file as:
<!-- Octomap Setup -->
<rosparam command="load" file="$(find robot_moveit_config)/config/sensors_kinect.yaml" />
<param name="octomap_frame" type="string" value="world" />
<param name="octomap_resolution" type="double" value="0.05" />
<param name="max_range" type="double" value="5.0" />
My process for getting point cloud data to appear and for the robot to avoid obstacles encountered in the point cloud is:
~$ roslaunch openni_launch openni.launch
~$ rosrun openni_tracker openni_tracker
Unfortunately at this point the point cloud data does not appear until I issue the manual brute-force command:
~$ rosrun tf static_transform_publisher 0 0 0 0 0 0 /world /camera_link 100
This causes point cloud data to appear and update at the appropriate rate, but I can't execute or plan commands until I have killed the process where I create a transform link, but then the point cloud doesn't update as I would have hoped. Can anyone let me know if I'm making any incorrect assumptions or doing anything here that won't work and could be causing this issue? I can supply any other files that might be necessary to diagnose the issue as well, just let me know!
EDIT The exact error message that I'm getting when I attempt to plan and execute motions while octomap is generated is the following:
[ INFO] [1532961249.635940765]: Didn't received robot state (joint angles) with recent timestamp within 1 seconds.
Check clock synchronization if your are running ROS across multiple machines!
[ WARN] [1532961249.636078212]: Failed to validate trajectory: couldn't receive full current joint state within 1s
[ INFO] [1532961249.636700450]: ABORTED: Solution found but controller failed during execution
A note that might be useful is that I'm actually executing commands for the robot via a Flask application. The application works just fine without an octomap in the planning scene, and when there is an octomap in the planning scene using the flask app or directly moving the robot within the Moveit window causes the warning and abort to occur.
EDIT2 Here is an imgur album showing the robot running correctly and then with the OpenNI commands running: https://imgur.com/a/OCcNRCd
Try reducing the framerate of your sensor (config, a
throttlenode (topic_tools) or settingmax_update_rate). Updating the planning scene locks everything, so it could be that other parts of moveit are being cpu starved.Or your machine just can't keep up (computationally).
This is not a 'brute force command', but a necessity (or at least, the existence of the transform): how is MoveIt going to correctly process your pointcloud without the proper TF frames?
Ah, ok this makes sense. I only referred to it as a 'brute force command' because I felt as though it might be accounting for something I could've done more professionally in a launch or urdf file if possible. As for max_update_rate, would this be set in the demo.launch file or elsewhere?
yes, if your sensor is part of your 'work cell', then adding it to your urdf would be a good thing to do.
max_update_rateis a parameter of the octomap updater plugin you're using.See http://docs.ros.org/kinetic/api/movei... .
This is very helpful, thank you. Unfortunately, even after changing the max_update_rate to 5 and decreasing the resolution to 0.05 from 0.025 I'm still seeing the execution fail with the message that the planning and execution takes longer than 1s.
Is it possible that redundant code between demo.launch, sensor_manager.launch.xml, and sensor_kinect.yaml could be causing the dropped messages? I'm seeing some of the same parameters written in each of them. I can supply any other files that might be useful to see for debugging.
It seems that the config flow is:
But I'm not sure where moveit_sensor_manager.launch.xml or sensor_manager.launch.xml get called in this process.