‘Failed to fetch current robot state‘ when using the ‘plan_kinematic_path‘ service #868

https://github.com/ros-planning/moveit/issues/868https://github.com/ros-planning/moveit/issues/868

avoelz commented on 27 Apr 2018

Description The 'plan_kinematic_path' service requires at least one second computation time and outputs the message 'Failed to fetch current robot state'. The issue has been introduced by the pull request #350 and the new line + // before we start planning, ensure that we have the latest robot state received... + context_->planning_scene_monitor_->waitForCurrentRobotState(ros::Time::now()); in moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp While processing the computePlanService, the current state monitor is not able to process the incoming joint states in the callback function planning_scene_monitor::CurrentStateMonitor::jointStateCallback. Therefore waitForCurrentRobotState always waits until the maximum allowed time (by defaul 1 second) has passed and prints the error message 'Failed to fetch current robot state'. The problem can be solved by adding a second spinning thread, i.e. ros::AsyncSpinner spinner(2); in moveit_ros/move_group/src/move_group.cpp. However, I do not know if this would introduce new problems at other places. Note that the problem does not exist with the MoveGroupMoveAction capability because the action server does not block the spinning thread. Your environment ROS Distro: Kinetic OS Version: Ubuntu 16.04 Source or Binary build? Applies to both as long as fix race conditions when updating PlanningScene #350 is included. Steps to reproduce I have added some debug print statements to highlight the issue, see the diff, then run roslaunch *_moveit_config demo.launch for an arbitrary robot model with move_group/MoveGroupPlanService capability enabled rosservice call /plan_kinematic_path "motion_plan_request: ..." for an arbitrary motion plan request Expected behaviour Service plan_kinematic_path returns in few milliseconds with output similar to [ INFO] [1524824641.260897825]: planning_scene_monitor::CurrentStateMonitor::jointStateCallback [ INFO] [1524824641.460614270]: planning_scene_monitor::CurrentStateMonitor::jointStateCallback [ INFO] [1524824641.660500520]: planning_scene_monitor::CurrentStateMonitor::jointStateCallback [ INFO] [1524824641.790221505]: Received new planning service request... [ INFO] [1524824641.860544618]: planning_scene_monitor::CurrentStateMonitor::jointStateCallback [ERROR] [1524824641.861405290]: No group specified to plan for [ INFO] [1524824641.861477559]: Elapsed time: 0.071 s [ INFO] [1524824642.060578465]: planning_scene_monitor::CurrentStateMonitor::jointStateCallback [ INFO] [1524824642.260910956]: planning_scene_monitor::CurrentStateMonitor::jointStateCallback [ INFO] [1524824642.461013625]: planning_scene_monitor::CurrentStateMonitor::jointStateCallback Actual behaviour Service plan_kinematic_path returns after approximately one second with output similar to [ INFO] [1524824543.477195149]: planning_scene_monitor::CurrentStateMonitor::jointStateCallback [ INFO] [1524824543.577266157]: planning_scene_monitor::CurrentStateMonitor::jointStateCallback [ INFO] [1524824543.776860643]: planning_scene_monitor::CurrentStateMonitor::jointStateCallback [ INFO] [1524824543.811930327]: Received new planning service request... [ INFO] [1524824544.812130689]: 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] [1524824544.812195049]: Failed to fetch current robot state. [ERROR] [1524824544.813068385]: No group specified to plan for [ INFO] [1524824544.813148931]: Elapsed time: 1.001 s [ INFO] [1524824544.813359534]: planning_scene_monitor::CurrentStateMonitor::jointStateCallback [ INFO] [1524824544.976705795]: planning_scene_monitor::CurrentStateMonitor::jointStateCallback [ INFO] [1524824545.077333781]: planning_scene_monitor::CurrentStateMonitor::jointStateCallback One can see that in the second case the jointStateCallback is not exe