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:
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
↧