This repository has been archived by the owner. It is now read-only.

rviz plugin: stop execution + update of start state to current #713

Merged
merged 6 commits into from Jul 18, 2016

Conversation

Projects
None yet
4 participants
@rhaschke
Contributor

rhaschke commented Jul 15, 2016

This PR combines ideas of #709 and #710 (and would replaces those PRs) to allow

  • stopping of an executed trajectory (not working for execute-only button, see comment in #709)
  • updating the start state after execution, if current state is selected in gui (fixes #646)

I consider this work-in-progress. @davetcoleman Should we address this comment like suggested?

@rhaschke rhaschke referenced this pull request Jul 15, 2016

Closed

safer rviz display #710

Show outdated Hide outdated ...lugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h
@@ -88,7 +88,7 @@ class PlanningSceneDisplay : public rviz::Display
void queueRenderSceneGeometry();
// pass the execution of this function call to a separate thread that runs in the background
void addBackgroundJob(const boost::function<void()> &job, const std::string &name);
void addBackgroundJob(const boost::function<void()> &job, const std::string &name, bool ownThread=false);

This comment has been minimized.

@rhaschke

rhaschke Jul 15, 2016

Contributor

Due to this change, the PR is not ABI compatible. However, this is only related to the rviz plugin, which shouldn't be linked elsewhere. However, if in doubt, we can also target jade-devel.

@rhaschke

rhaschke Jul 15, 2016

Contributor

Due to this change, the PR is not ABI compatible. However, this is only related to the rviz plugin, which shouldn't be linked elsewhere. However, if in doubt, we can also target jade-devel.

This comment has been minimized.

@davetcoleman

davetcoleman Jul 15, 2016

Member

The only thing that bothers me about this is that its not using ROS's underscore variable convention ;-)

@davetcoleman

davetcoleman Jul 15, 2016

Member

The only thing that bothers me about this is that its not using ROS's underscore variable convention ;-)

This comment has been minimized.

@rhaschke

rhaschke Jul 15, 2016

Contributor

Where do you want to have the underscores? As far as I understand it, only member variables should be suffixed by an underscore. Is there anything else?

@rhaschke

rhaschke Jul 15, 2016

Contributor

Where do you want to have the underscores? As far as I understand it, only member variables should be suffixed by an underscore. Is there anything else?

This comment has been minimized.

@v4hn

v4hn Jul 15, 2016

Member

all other argument names in this file use underscore notation, so please change ownThread to own_thread.

@v4hn

v4hn Jul 15, 2016

Member

all other argument names in this file use underscore notation, so please change ownThread to own_thread.

This comment has been minimized.

@v4hn

v4hn Jul 15, 2016

Member

I agree that breaking ABI in this code is well-justified by safety concerns.
You are not using addBackgroundJob anywhere though. :-)
Also, it might be a good idea to have a separate function name for that, instead of extending the existing one with orthogonal code. spawnBackgroundJob?
Alternatively, we could simply stick with the boost::thread one-liner you used in this request.
But then it might still be a good idea to have explicit comments above each of these lines to explain why this should not be addBackgroundJob.

@v4hn

v4hn Jul 15, 2016

Member

I agree that breaking ABI in this code is well-justified by safety concerns.
You are not using addBackgroundJob anywhere though. :-)
Also, it might be a good idea to have a separate function name for that, instead of extending the existing one with orthogonal code. spawnBackgroundJob?
Alternatively, we could simply stick with the boost::thread one-liner you used in this request.
But then it might still be a good idea to have explicit comments above each of these lines to explain why this should not be addBackgroundJob.

This comment has been minimized.

@rhaschke

rhaschke Jul 15, 2016

Contributor

@v4hn @davetcoleman Whatever you prefer. Please vote ;-)

@rhaschke

rhaschke Jul 15, 2016

Contributor

@v4hn @davetcoleman Whatever you prefer. Please vote ;-)

This comment has been minimized.

@v4hn

v4hn Jul 15, 2016

Member

+1 on a one-line spawnBackgroundJob function and a comment above that explains why this sometimes makes sense to use instead of addBackgroundJob.
I would be interested in a real explanation myself :-)

@v4hn

v4hn Jul 15, 2016

Member

+1 on a one-line spawnBackgroundJob function and a comment above that explains why this sometimes makes sense to use instead of addBackgroundJob.
I would be interested in a real explanation myself :-)

This comment has been minimized.

@davetcoleman

davetcoleman Jul 16, 2016

Member

+1 to @v4hn's comment

@davetcoleman

davetcoleman Jul 16, 2016

Member

+1 to @v4hn's comment

Show outdated Hide outdated move_group/src/default_capabilities/execute_service_capability.cpp
@@ -45,6 +45,7 @@ move_group::MoveGroupExecuteService::MoveGroupExecuteService():
void move_group::MoveGroupExecuteService::initialize()
{
// TODO: Use own callback queue and spinner thread to not block on execution?

This comment has been minimized.

@davetcoleman

davetcoleman Jul 15, 2016

Member

+1

@@ -162,63 +200,66 @@ void MotionPlanningFrame::updateQueryStateHelper(robot_state::RobotState &state,
configureWorkspace();
if (const robot_model::JointModelGroup *jmg = state.getJointModelGroup(planning_display_->getCurrentPlanningGroup()))
state.setToRandomPositions(jmg);
return;

This comment has been minimized.

@davetcoleman

davetcoleman Jul 15, 2016

Member

So in this function updateQueryStateHelper() you don't actually change any logic but rather just wanted to cleanup the program flow?

@davetcoleman

davetcoleman Jul 15, 2016

Member

So in this function updateQueryStateHelper() you don't actually change any logic but rather just wanted to cleanup the program flow?

This comment has been minimized.

@rhaschke

rhaschke Jul 15, 2016

Contributor

Yep. I found it difficult to follow the nested if structure.

@rhaschke

rhaschke Jul 15, 2016

Contributor

Yep. I found it difficult to follow the nested if structure.

This comment has been minimized.

@v4hn

v4hn Jul 15, 2016

Member

+1, the nested structures throughout MoveIt! are totally confusing and I would like to see most of them replaced by "Check and early return", as you did here.

@v4hn

v4hn Jul 15, 2016

Member

+1, the nested structures throughout MoveIt! are totally confusing and I would like to see most of them replaced by "Check and early return", as you did here.

This comment has been minimized.

@davetcoleman

davetcoleman Jul 15, 2016

Member

+1 I'm a big fan of check and early return

@davetcoleman

davetcoleman Jul 15, 2016

Member

+1 I'm a big fan of check and early return

This comment has been minimized.

@rhaschke

rhaschke Jul 15, 2016

Contributor

Nice, that we agree on that style ;-)

@rhaschke

rhaschke Jul 15, 2016

Contributor

Nice, that we agree on that style ;-)

Show outdated Hide outdated visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp
@@ -227,9 +227,12 @@ void PlanningSceneDisplay::reset()
}
}
void PlanningSceneDisplay::addBackgroundJob(const boost::function<void()> &job, const std::string &name)
void PlanningSceneDisplay::addBackgroundJob(const boost::function<void()> &job, const std::string &name, bool ownThread)

This comment has been minimized.

@davetcoleman

davetcoleman Jul 15, 2016

Member

again please use underscores

@davetcoleman

davetcoleman Jul 15, 2016

Member

again please use underscores

This comment has been minimized.

@rhaschke

rhaschke Jul 15, 2016

Contributor

Got it.

@rhaschke

rhaschke Jul 15, 2016

Contributor

Got it.

@davetcoleman

This comment has been minimized.

Show comment
Hide comment
@davetcoleman

davetcoleman Jul 15, 2016

Member

I think its a good idea to address #709 (comment) but I understand if you don't have the cycles for it right now

Member

davetcoleman commented Jul 15, 2016

I think its a good idea to address #709 (comment) but I understand if you don't have the cycles for it right now

Show outdated Hide outdated ...ation/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp
@@ -55,14 +55,20 @@ void MotionPlanningFrame::planButtonClicked()
void MotionPlanningFrame::executeButtonClicked()
{
ui_->execute_button->setEnabled(false);
planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computeExecuteButtonClicked, this), "execute");
boost::thread(boost::bind(&MotionPlanningFrame::computeExecuteButtonClicked, this));

This comment has been minimized.

@davetcoleman

davetcoleman Jul 15, 2016

Member

Why use boost threads when this class already has its own background job framework setup? Seems like a departure of the functional style of the rest of the class.

@davetcoleman

davetcoleman Jul 15, 2016

Member

Why use boost threads when this class already has its own background job framework setup? Seems like a departure of the functional style of the rest of the class.

This comment has been minimized.

@rhaschke

rhaschke Jul 15, 2016

Contributor

Executing the motion in the (single) background thread blocks visual updates of the scene robot.

Unfortunately, this is only mentioned in the commit log. I will add a source-code comment.

@rhaschke

rhaschke Jul 15, 2016

Contributor

Executing the motion in the (single) background thread blocks visual updates of the scene robot.

Unfortunately, this is only mentioned in the commit log. I will add a source-code comment.

Show outdated Hide outdated ...ation/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp
@@ -55,14 +55,22 @@ void MotionPlanningFrame::planButtonClicked()
void MotionPlanningFrame::executeButtonClicked()
{
ui_->execute_button->setEnabled(false);
planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computeExecuteButtonClicked, this), "execute");
// executing the motion in the (single) background thread, blocks visual updates of the scene robot
boost::thread(boost::bind(&MotionPlanningFrame::computeExecuteButtonClicked, this));

This comment has been minimized.

@v4hn

v4hn Jul 15, 2016

Member

Why are visual updates blocked? I suppose planning scene updates don't get through?
Maybe we could have two callback queues, one for callback related functions and one for handling of user interaction?
I suppose it is a good idea to have a uniform way to run background computation on button pressed throughout rviz-moveit.

@v4hn

v4hn Jul 15, 2016

Member

Why are visual updates blocked? I suppose planning scene updates don't get through?
Maybe we could have two callback queues, one for callback related functions and one for handling of user interaction?
I suppose it is a good idea to have a uniform way to run background computation on button pressed throughout rviz-moveit.

This comment has been minimized.

@rhaschke

rhaschke Jul 15, 2016

Contributor

Without separate threads for this, the background-job-thread was mainly blocked with waiting for the trajectory execution to finish. Everything else going to the background job simply needs to wait as well - which doesn't make sense.
To have a more uniform "look&feel", I augmented addBackgroundJob() with an extra option ownThread defaulting to false. For some reason the use of this code got lost when cleaning up my messy local repo. However, you suggested to go with boost::thread only. I have a slight preference for that too. Please vote for an option!

@rhaschke

rhaschke Jul 15, 2016

Contributor

Without separate threads for this, the background-job-thread was mainly blocked with waiting for the trajectory execution to finish. Everything else going to the background job simply needs to wait as well - which doesn't make sense.
To have a more uniform "look&feel", I augmented addBackgroundJob() with an extra option ownThread defaulting to false. For some reason the use of this code got lost when cleaning up my messy local repo. However, you suggested to go with boost::thread only. I have a slight preference for that too. Please vote for an option!

Show outdated Hide outdated ...ation/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp
@@ -129,16 +137,48 @@ void MotionPlanningFrame::computePlanButtonClicked()
void MotionPlanningFrame::computeExecuteButtonClicked()
{
if (move_group_ && current_plan_)
move_group_->execute(*current_plan_);
{
// ui_->stop_button->setEnabled(true); // enable stopping

This comment has been minimized.

@v4hn

v4hn Jul 15, 2016

Member

Why is this commented?

@v4hn

v4hn Jul 15, 2016

Member

Why is this commented?

This comment has been minimized.

@rhaschke

rhaschke Jul 15, 2016

Contributor

Because it's currently not working. A service cannot be interrupted. @wkentaro suggested an option which I will try later.

@rhaschke

rhaschke Jul 15, 2016

Contributor

Because it's currently not working. A service cannot be interrupted. @wkentaro suggested an option which I will try later.

Show outdated Hide outdated ...ation/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp
// Hm. The current state is only updated after a while (only when using /execute/ service)
// TODO: Wait for update to planning scene? Related to #671
sleep(1);
updateStartStateToCurrent();

This comment has been minimized.

@v4hn

v4hn Jul 15, 2016

Member

If this does not explicitly ask move_group for the current state via a service request, 👎.
Waiting for the current state via sleep is a workaround that might fail on slow systems/heavy load.
This is the exact problem I was trying to address by only sending empty start states in #710 .

In my opinion, a proper solution would add a new <always-current> option and keep updating the maintained start state in this case. This has some non-trivial edge-cases, but it would allow for other nodes to move the arm without rendering the display's start-state obsolete.

@v4hn

v4hn Jul 15, 2016

Member

If this does not explicitly ask move_group for the current state via a service request, 👎.
Waiting for the current state via sleep is a workaround that might fail on slow systems/heavy load.
This is the exact problem I was trying to address by only sending empty start states in #710 .

In my opinion, a proper solution would add a new <always-current> option and keep updating the maintained start state in this case. This has some non-trivial edge-cases, but it would allow for other nodes to move the arm without rendering the display's start-state obsolete.

This comment has been minimized.

@rhaschke

rhaschke Jul 15, 2016

Contributor

I know, that this is a work-around for now. This should addressed with #716.
I can only address one thing at a time ;-)

@rhaschke

rhaschke Jul 15, 2016

Contributor

I know, that this is a work-around for now. This should addressed with #716.
I can only address one thing at a time ;-)

Show outdated Hide outdated ...ation/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp
ui_->plan_and_execute_button->setEnabled(true);
ui_->stop_button->setEnabled(false);
updateStartStateToCurrent();

This comment has been minimized.

@v4hn

v4hn Jul 15, 2016

Member

Here, you're not sleeping and the start state might be obsolete, right?

@v4hn

v4hn Jul 15, 2016

Member

Here, you're not sleeping and the start state might be obsolete, right?

This comment has been minimized.

@rhaschke

rhaschke Jul 15, 2016

Contributor

Using the move_group action server, the issue wasn't that dramatic, because intermediate updates come through.
However, you are right, in principle the delayed update issue is observable here too. Should be fixed in #716 - hope a lot of people will help on this ;-)

@rhaschke

rhaschke Jul 15, 2016

Contributor

Using the move_group action server, the issue wasn't that dramatic, because intermediate updates come through.
However, you are right, in principle the delayed update issue is observable here too. Should be fixed in #716 - hope a lot of people will help on this ;-)

@rhaschke

This comment has been minimized.

Show comment
Hide comment
@rhaschke

rhaschke Jul 16, 2016

Contributor

I addressed all comments. Remaining issue is to turn ExecuteService in ExecuteAction and of course deal with #716.

Contributor

rhaschke commented Jul 16, 2016

I addressed all comments. Remaining issue is to turn ExecuteService in ExecuteAction and of course deal with #716.

@rhaschke

This comment has been minimized.

Show comment
Hide comment
@rhaschke

rhaschke Jul 16, 2016

Contributor

Further cleaned up and rebased.
Disabling QueryStartState by default as suggested in #710.
Relies on #717.
For correct working of start-state updates, we need to fix #716 as well.
Could be merged now.

Contributor

rhaschke commented Jul 16, 2016

Further cleaned up and rebased.
Disabling QueryStartState by default as suggested in #710.
Relies on #717.
For correct working of start-state updates, we need to fix #716 as well.
Could be merged now.

Show outdated Hide outdated ...ation/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp
void MotionPlanningFrame::onFinishedExecution(bool success)
{
// visualize result of execution
if (success) ui_->result_label->setText("Executed");

This comment has been minimized.

@davetcoleman

davetcoleman Jul 17, 2016

Member

you should line break after if() statement per ROS style

@davetcoleman

davetcoleman Jul 17, 2016

Member

you should line break after if() statement per ROS style

This comment has been minimized.

@rhaschke

rhaschke Jul 17, 2016

Contributor

Fixed.

@rhaschke

rhaschke Jul 17, 2016

Contributor

Fixed.

@davetcoleman

This comment has been minimized.

Show comment
Hide comment
Member

davetcoleman commented Jul 17, 2016

+1

v4hn and others added some commits Jul 10, 2016

rviz display: disable Query Start State by default
This property is "only" used by people playing around with planning internals,
e.g. the people maintaining the code..

The "average" user does not expect to be able to manipulate the start state of the motion-request
te makes from rviz - it doesn't make sense if you want to actually move the robot.
Therefore, disable the start-state by default to reduce the initial complexity
the average user is confronted with.
rviz plugin: execute motion in separate thread
executing the motion in the (single) background thread blocks visual updates of the scene robot
suppress warning "Execution of motions should always start at the rob…
…ot's current state."

when planning+executing

@rhaschke rhaschke merged commit 6a18450 into indigo-devel Jul 18, 2016

2 checks passed

continuous-integration/travis-ci/pr The Travis CI build passed
Details
continuous-integration/travis-ci/push The Travis CI build passed
Details

@rhaschke rhaschke deleted the wip-stop-execution branch Jul 18, 2016

rhaschke added a commit that referenced this pull request Jul 18, 2016

cherry-pick #713 from indigo-devel
- cleanup structure of updateQueryStateHelper
- updateStartStateToCurrent after execution
- visualization: stop execution button
- suppress warning "Execution of motions should always start at the robot's current state." when planning+executing
- rviz display: disable Query Start State by default
@davetcoleman

This comment has been minimized.

Show comment
Hide comment
@davetcoleman

davetcoleman Jul 18, 2016

Member

since this is a relatively major change (new GUI button) to a stable (indigo) release I think you should announce this on the mailing list

also, please be sure to cherry-pick to jade and kinetic

thanks!

Member

davetcoleman commented Jul 18, 2016

since this is a relatively major change (new GUI button) to a stable (indigo) release I think you should announce this on the mailing list

also, please be sure to cherry-pick to jade and kinetic

thanks!

davetcoleman added a commit that referenced this pull request Jul 18, 2016

@wkentaro

This comment has been minimized.

Show comment
Hide comment
@wkentaro

wkentaro Jul 18, 2016

Contributor

@davetcoleman @rhaschke Thanks for working for this!

Contributor

wkentaro commented Jul 18, 2016

@davetcoleman @rhaschke Thanks for working for this!

@davetcoleman

This comment has been minimized.

Show comment
Hide comment
@davetcoleman

davetcoleman Jul 18, 2016

Member

the thanks goes to @rhaschke !

Member

davetcoleman commented Jul 18, 2016

the thanks goes to @rhaschke !

@rhaschke

This comment has been minimized.

Show comment
Hide comment
@rhaschke

rhaschke Jul 18, 2016

Contributor

please be sure to cherry-pick to jade and kinetic

Dave, you already merge-committed #718 into jade-devel. I would have loved to use a squashing commit only, i.e. forwarding jade-devel to the cherry-pick branch.
Anybody minds to rebase jade-devel there?

The same I would do for kinetic. For me this eases reading the git graph:
Merge commits are unique contributions, while cherry-picks could be committed directly.
I just had to use a PR to let Travis do the compile checking.

Contributor

rhaschke commented Jul 18, 2016

please be sure to cherry-pick to jade and kinetic

Dave, you already merge-committed #718 into jade-devel. I would have loved to use a squashing commit only, i.e. forwarding jade-devel to the cherry-pick branch.
Anybody minds to rebase jade-devel there?

The same I would do for kinetic. For me this eases reading the git graph:
Merge commits are unique contributions, while cherry-picks could be committed directly.
I just had to use a PR to let Travis do the compile checking.

@rhaschke

This comment has been minimized.

Show comment
Hide comment
@rhaschke

rhaschke Jul 18, 2016

Contributor

since this is a relatively major change (new GUI button) to a stable
(indigo) release I think you should announce this on the mailing list

Dear Dave,
before announcing on the mailing list, I would like to finish #716,
which is a major pre-requisite for #713 to work correctly.
Hopefully, I find some time this evening to continue on that.
In the same vein, I'm planning to merge those changes to Kinetic only as
a bundle.

Contributor

rhaschke commented Jul 18, 2016

since this is a relatively major change (new GUI button) to a stable
(indigo) release I think you should announce this on the mailing list

Dear Dave,
before announcing on the mailing list, I would like to finish #716,
which is a major pre-requisite for #713 to work correctly.
Hopefully, I find some time this evening to continue on that.
In the same vein, I'm planning to merge those changes to Kinetic only as
a bundle.

@davetcoleman

This comment has been minimized.

Show comment
Hide comment
@davetcoleman

davetcoleman Jul 18, 2016

Member

I just had to use a PR to let Travis do the compile checking.

Please make note of that in your PR if you don't want it actually merged yet.

Dave, you already merge-committed #718 into jade-devel.

Yes, I hadn't seen the PR until after I made that comment

You're announcement timeline sounds good, thanks!

Member

davetcoleman commented Jul 18, 2016

I just had to use a PR to let Travis do the compile checking.

Please make note of that in your PR if you don't want it actually merged yet.

Dave, you already merge-committed #718 into jade-devel.

Yes, I hadn't seen the PR until after I made that comment

You're announcement timeline sounds good, thanks!

otamachan pushed a commit to otamachan/moveit_ros that referenced this pull request Oct 22, 2017

cherry-pick #713 from indigo-devel
- cleanup structure of updateQueryStateHelper
- updateStartStateToCurrent after execution
- visualization: stop execution button
- suppress warning "Execution of motions should always start at the robot's current state." when planning+executing
- rviz display: disable Query Start State by default
Sign up for free to subscribe to this conversation on GitHub. Already have an account? Sign in.