Skip to content

use waitForCurrentState(ros::Time::now(), instead of waitForCurrentSt…#717

Merged
davetcoleman merged 3 commits intomoveit:indigo-develfrom
k-okada:add_350
Jan 15, 2018
Merged

use waitForCurrentState(ros::Time::now(), instead of waitForCurrentSt…#717
davetcoleman merged 3 commits intomoveit:indigo-develfrom
k-okada:add_350

Conversation

@k-okada
Copy link
Copy Markdown
Contributor

@k-okada k-okada commented Dec 10, 2017 •
edited by rhaschke
Loading

…ate(1.0) in TrajectoryExecutionManager::validate, cherry pick part of #350

Description

Current indigo moveit has regression compare to hydro, which you could find very easily at answers.ros.org, such as

and interestingly this problem was fixed in Kinetic

that is because indigo moveit requires update of entire joints from /joint_states by using haveCompleteState() function, whereas kinetic moveit uses state_update_condition_.wait_for(lock, boost::chrono::nanoseconds((timeout - elapsed).toNSec())); in #63 which uses https://github.com/ros-planning/moveit/blob/0.9.9/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp#L265-L280 with basicall wait for

This change has been introduced in #350

To summarize, if /joint_states is not complite,

  • Hydro : The robot can move trajectory
  • Indigo (before this PR), It shows Failed to validate trajectory: couldn't receive full current joint state within 1s and do not move
  • Indigo (after this PR), it shows The complete state of the robot is not yet known. Missing RHAND_JOINT0, JOINT1, JOINT2, ... and robot can move
  • Kinetic, as same as Indigo (after this PR)

Checklist

  • Required: Code is auto formatted using clang-format
  • Extended the tutorials / documentation, if necessary reference
  • Include a screenshot if changing a GUI
  • Optional: Created tests, which fail without this PR reference
  • Optional: Decide if this should be cherry-picked to other current ROS branches (Indigo, Jade, Kinetic)

@k-okada
use waitForCurrentState(ros::Time::now(), instead of waitForCurrentSt…
97d0414 
…ate(1.0) in TrajectoryExecutionManager::validate, cherry pick part of moveit#350
@v4hn v4hn requested a review from rhaschke December 10, 2017 14:58
@rhaschke
Copy link
Copy Markdown
Contributor

rhaschke commented Dec 11, 2017

#350 fixed race conditions with planning scene updates. To this end, #350 also renamed waitForCurrentState to waitForCompleteState (to correctly capture it's semantics) and correctly implemented waitForCurrentState as a new function (with different interface). However #350 was never back-ported to Indigo, probably due to ABI-breaking changes.

#63 introduced trajectory validation before execution, which calls the new waitForCurrentState() introduced in #350 to ensure that the latest state is available.

The current PR introduces the new semantics of waitForCurrentState(), introducing all ABI-breaking variables etc., but skipping many other parts of it. That's not really nice.

Hence, I suggest to either go for a full back-port of #350, accepting ABI breakage or to solve the issue by some other means (e.g. hoping that we already received the latest robot state). @v4hn: Can you give me a call to discuss this?

rhaschke
rhaschke requested changes Dec 11, 2017
View reviewed changes
Copy link
Copy Markdown
Contributor

@rhaschke rhaschke left a comment •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is not a clean solution to the problem. We need to find a better one. Thanks @k-okada for your detailed analysis!

@k-okada k-okada mentioned this pull request Dec 12, 2017
Closed
@k-okada
Copy link
Copy Markdown
Contributor Author

k-okada commented Dec 12, 2017 •
edited
Loading

thanks for comments, see #721 for full back port of #350, but there are several conflicts which I had to fix manually, so I'm not confident with this.

@k-okada k-okada mentioned this pull request Dec 12, 2017
Closed
5 tasks
rhaschke
rhaschke requested changes Dec 12, 2017
View reviewed changes
Copy link
Copy Markdown
Contributor

@rhaschke rhaschke left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Dear @k-okada,
I had a phone call with @v4hn and we agreed to accept this PR with some minor changes to stay ABI-compatible. Basic idea is to replace the condition variable with busy waiting (see inline comments for details).
Subsequently, I will change other usages of waitForCurrentState() requiring the new semantics as well.

moveit_ros/planning/CMakeLists.txt Outdated
endif()

find_package(Boost REQUIRED system filesystem date_time program_options signals thread)
find_package(Boost REQUIRED system filesystem date_time program_options signals thread chrono)
Copy link
Copy Markdown
Contributor

@rhaschke rhaschke Dec 12, 2017

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

revert this

...lanning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h Outdated
#include <boost/function.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/thread/condition_variable.hpp>
Copy link
Copy Markdown
Contributor

@rhaschke rhaschke Dec 12, 2017

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

revert this

...lanning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h Outdated
ros::Time last_tf_update_;

mutable boost::mutex state_update_lock_;
mutable boost::condition_variable state_update_condition_;
Copy link
Copy Markdown
Contributor

@rhaschke rhaschke Dec 12, 2017

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

revert this

moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp Outdated
boost::mutex::scoped_lock lock(state_update_lock_);
while (current_state_time_ < t)
{
state_update_condition_.wait_for(lock, boost::chrono::nanoseconds((timeout - elapsed).toNSec()));
Copy link
Copy Markdown
Contributor

@rhaschke rhaschke Dec 12, 2017

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Implement this using busy waiting, inserting here a sleep(0.1) for example
This allows us to get rid of the ABI-breaking insertion of state_update_condition_.

moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp Outdated


// notify waitForCurrentState() *after* potential update callbacks
state_update_condition_.notify_all();
Copy link
Copy Markdown
Contributor

@rhaschke rhaschke Dec 12, 2017

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

revert this

@rhaschke rhaschke mentioned this pull request Jan 4, 2018
Closed
@rhaschke rhaschke self-assigned this Jan 4, 2018
@rhaschke
restore ABI-compatibility
ff12ce1 
replace conditional waiting with busy waiting
@rhaschke
fixed comparison
275ec2a
@rhaschke
Copy link
Copy Markdown
Contributor

rhaschke commented Jan 5, 2018

@k-okada @saurabhbansal90 Please could you verify that this latest change fixes your issues, e.g. #501?.
@v4hn @davetcoleman Could you please provide another review.

@davetcoleman davetcoleman merged commit 4c9a748 into moveit:indigo-devel Jan 15, 2018
@k-okada k-okada deleted the add_350 branch January 16, 2018 04:45
@rhaschke rhaschke mentioned this pull request Jan 22, 2018
Open
@rkeatin3 rkeatin3 mentioned this pull request Jan 23, 2018
Closed
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Reviewers

@davetcoleman davetcoleman davetcoleman approved these changes

@rhaschke rhaschke rhaschke approved these changes

Assignees

@rhaschke rhaschke

Labels

None yet

Projects

None yet

Milestone

No milestone

Development

Successfully merging this pull request may close these issues.

3 participants

@k-okada @rhaschke @davetcoleman