Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Closed
avoelz opened this issue Apr 27, 2018 · 3 comments

Comments

Projects
None yet
3 participants
@avoelz
Copy link

commented Apr 27, 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 #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 executed as long as the planning request is processed.

@rhaschke rhaschke self-assigned this Apr 27, 2018

@rhaschke

This comment has been minimized.

Copy link
Collaborator

commented Apr 28, 2018

@avoelz Thank you very much for this detailed analysis, which in short is a follows:

  • move_group currently employs a single spinner thread
  • processing a service call, blocks any other event processing on the global event queue
    (because there is only a single spinner thread)
  • actions utilize their own event queue and spinner thread, and thus are not affected by this issue

I see the following solutions:

  1. Increase the number of spinning threads in move_group. This raises the following questions:
    • What happens if a second service call (for the same or another service) is processed in parallel?
      I guess, this will block the next thread. Thus, it will not finally solve the issue.
    • Are different capabilities thread safe? Can they be called simultaneously? Probably not.
    • I remember that we discussed that issue before and there was a vote against multiple spinning threads. @davetcoleman @v4hn: Do you remember the reasons?
  2. Events related to PlanningScene updates should be handled on their own event queue and a dedicated spinning thread. This has the following implications:
    • The PlanningSceneMonitor could handle this.
    • PlanningScene events are processed independently from all other events, which resolves the issue.
    • However, event processing is not synchronized anymore (because we have different queues).
      Does this create new issues?
    • PlanningScene access should be thread-safe in principle...
    • The problem remains, if there is no PlanningSceneMonitor instantiated.
      The busy looping, we perform in this situation, should maybe call ros::spin() then?
@avoelz

This comment has been minimized.

Copy link
Author

commented May 2, 2018

@rhaschke Thank you for the quick reply. I already guessed that simply increasing the number of spinning threads would introduce other problems. In addition to your ideas, I see two other solutions:

  • Replace all services by action servers. The motion_planning_plugin for RViz uses the MoveGroupMoveAction with the flag plan_only set to true instead of the MoveGroupPlanService. As advantage, this would allow to preempt the planning process.
  • Do not call waitForCurrentRobotState in MoveGroupPlanService. Planning should use the start_state from the MotionPlanRequest so the current state should not be required.

@v4hn v4hn referenced this issue May 15, 2018

Closed

Release 201805 #886

1 of 2 tasks complete
@v4hn

This comment has been minimized.

Copy link
Member

commented May 30, 2018

We went with @avoelz's second proposal for now and #923 removed the blocking function call to remove the regression.

@avoelz You argue that the current state is not required here to begin with.
I don't agree, but I did not test it: If your robot has more joints than you specify in your start_state (start_state.is_diff == true) it should take the rest of the joint values from the current planning scene.
If that's not what happens in practice at the moment, I consider this a bug.

However, if the state is no diff we might skip this check altogether, that seems to be true.

rhaschke added a commit to ubi-agni/moveit that referenced this issue Aug 22, 2018

rhaschke added a commit to ubi-agni/moveit that referenced this issue Aug 23, 2018

rhaschke added a commit to ubi-agni/moveit that referenced this issue Sep 10, 2018

rhaschke added a commit to ubi-agni/moveit that referenced this issue Sep 10, 2018

rhaschke added a commit that referenced this issue Sep 10, 2018

Merge pull request #1033 from ubi-agni/fix-868
Fix #868: calling service capability locks PlanningSceneMonitor

ipa-hsd added a commit to ipa-hsd/moveit that referenced this issue Mar 18, 2019

Cherry-Pick ros-planning#1033: Fix ros-planning#868 (ros-planning#1057)
* Revert "disable waitForCurrentRobotState() for PlanService capability (ros-planning#923)"

This reverts commit cf2217c.

* only waitForCurrentRobotState when start_state.is_diff

as discussed in ros-planning#868 (comment)

* reduce code duplication

* use own EventQueue in PlanningSceneMonitor

... to become independent of any capabilities that might block the global queue

* PlanningSceneMonitor: accept parent NodeHandle

* add MIGRATION note
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.