Add "set to goal" button for start state in rviz

[buckybots]

When following the directions :

Press the "Set Start To Current" button in the "Planning" tab at the bottom of the Motion Planning plugin. This will set the start configuration that you want to plan from to the current configuration of the robot. You will need to do this every time when planning with a real or simulated robot.

It appears this button sets the start position to the initial position of the robot, not its current position. So that after the first plan is executed, the initial position no longer matches the current position, and the button does not operate as expected.

[isucan]

If you do just "Plan", you only get a motion plan, which you can visualize. The current state of the robot is not affected. If you click Plan & Execute (and you have a robot or a simulator behind), the robot will move, and the current state will be that which you expect.

[buckybots]

Yes, planning and executing the plan works as expected. The issue is that after this is done once, the robot in Gazebo is now in a new pose. To create a new plan, we want to plan from the current (changed) pose, to a new position. At this point I would expect that "Set Start State" , "" would put the starting position (of the arm) in the planner to its current pose in Gazebo, not the pose before I executed the first plan (I am calling that the original pose of the PR2). I can set the starting position to the current pose by using "select start state" , "same as goal", since having executed one plan, the arm's pose now matches the "goal" pose of the previous plan. While this works, I do not think how the "Set Start State" " is working how you intended. It might be that I just don't understand what the interface means by the "" selection. I would think that should be the pose of the Robot as is currently is in Gazebo.

On Tue, May 21, 2013 at 4:33 AM, isucan notifications@github.com wrote:

If you do just "Plan", you only get a motion plan, which you can visualize. The current state of the robot is not affected. If you click Plan & Execute (and you have a robot or a simulator behind), the robot will move, and the current state will be that which you expect.

— Reply to this email directly or view it on GitHubhttps://github.com/ros-planning/moveit_planners/issues/13#issuecomment-18195644 .

[isucan]

I see. So basically you would like a mode where the start state stays pegged to the current state of the robot. This is done internally when you do Plan & Execute, but not when you just do Plan (even if followed by Execute). Is this correct?

[buckybots]

Yes, correct, except, I thought I was reporting a bug. I am not yet familiar enough with the software to determine if the interface should act differently.

I just tried same thing using Plan and Execute (but I did not reinitialize the setup). When I select "Update" on Query: Select Start State:

, the pose is set to the pose the PR2 had when I started up RVIZ, not the current pose as created with the most recent "Plan and Execute". It appears to me that RVIZ is not updating the state. Since I do not have anything else running that changes the Pose of the robot, the current state does match the state from my prior run of "Plan and Execute". On Tue, May 21, 2013 at 10:51 AM, isucan notifications@github.com wrote: > I see. So basically you would like a mode where the start state stays > pegged to the current state of the robot. This is done internally when you > do Plan & Execute, but not when you just do Plan (even if followed by > Execute). Is this correct? > > — > Reply to this email directly or view it on GitHubhttps://github.com/ros-planning/moveit_planners/issues/13#issuecomment-18218116 > .

[mdrx-io]

I, too, was using state expecting that to be state resulting from the last trajectory execution, not the initial position of the robot. Now that there is a button, I can achieve what I want - but it was confusing at first.

[mikeferguson]

I think the issue here is that the planning scene topic is probably not set to the "monitored_planning_scene" in the RVIZ plugin -- which means that the "current state" does not update as the robot moves.

[davetcoleman]

+1 to @mikeferguson's explanation

[mdrx-io]

I did already have the planning scene topic set to "monitored_planning_scene". It turns out my real issue was forgetting to hit the Update button in the Query: Select Start State panel for . For some reason I didn't remember having to do that before when working with a fresh moveit_config/demo.launch file and that the normal behavior was that it automatically would update the current position after having already executed a plan.