safer rviz display

[v4hn]

In #646 we discussed that the display should not allow execution of arbitrary generated plans for safety reasons. This request proposes to make it the default use-case to have the interactive start-state marker disabled and disable execution if it is enabled.

While this disallows executing trajectories even if the selected start-state equals the current state, it is the safest way to implement this that came to my mind.

Improvements and comments are welcome!

Fixes #646 .

[v4hn]

Hey @rhaschke,

I believe you misunderstood the idea behind this request.

MoveIt supports the use-case "use the currently monitored state as start state for the request I'm sending you", by leaving the start state of a MotionPlanRequest empty. The request makes use of this mechanism and sends an empty start state in the request if the "Query Start State" property is disabled. setStartStateToCurrentState() is just a fancy name for "clear the start state". That way, the current state of the robot is used, even if the rviz-plugin does not even know about the robot's current state, e.g. because the wrong planning scene topic was selected.

To forbid execution of trajectories that do not start from the actual start state of the robot, I disabled execution of all trajectories where this mechanism is not used, i.e. "Query Start State" is enabled.

I'm not entirely sure we want to go that way, but it resolves all safety issues and afaics it's a reasonable solution. The only problem I see is that it changes the behavior of the display in that it is not possible to execute trajectories when "Query Start State" is enabled. But that's not as easy to include, so I decided to leave it as is. Also I believe the issue of a robot arm moving at high speed to its "start state", justifies such a UI change.

[rhaschke]

Hi Michael,

I understand that you essentially add the possibility to always plan from the current state by explicitly specifying an empty start state. This behaviour is enabled when QueryStartState is disabled.

I still don't like this interface for several reasons:

1. The above described workflow (select current start state, manually modify it, disable QueryStartState to hide the marker, plan) doesn't work anymore. rviz will always plan from the current start state.
2. I don't like the idea of associating the visibility of the StartState marker with "planning from current state". We could have an internal flag indicating that the StartState didn't changed after selecting "current". If so, we can fallback to an empty start state. If it has changed somewhere in between, we use the explicit state. Alternatively, there should be an extra entry in the dropdown, e.g. <current> vs. <current snapshot>.
3. It doesn't protect you from the wrong start state issue: If you have several clients interfacing the move_group, e.g. two rviz instances. In both instances you generate a plan from "current" state (but don't yet execute it). If you then execute your plans one after the other, the robot will warp to the start of the second plan.

I think, Dave pointed out the only valid solution: There should be a validity check in MoveGroup server before executing a trajectory: If the current state (just before starting the trajectory) doesn't match the first via point, execution should be rejected.

[v4hn]

I agree that we should add a test to the MoveGroup server. Anyone is welcome to add it. This does not improve the user interface though, and the user might not even see the error message.

On Mon, Jul 11, 2016 at 08:27:21AM -0700, Robert Haschke wrote:

1. The above described workflow (select current start state, manually modify it, disable QueryStartState to hide the marker, plan) doesn't work anymore. rviz will always plan from the current start state.

Oh, I got you wrong there. You want that behavior. Up to now I thought of it as undefined (and undesirable). If we want to preserve the behavior, then the approach in this request does not make much sense. I'll have a look at the alternatives you propose. Will probably take some time though.

[rhaschke]

Interestingly, a combined "plan + execute" enforces the empty start state with a call to clearRequestStartState.Hence, using this button, the current state is enforced automatically. However, #442 remains. Consider #713 as a replacement for this PR.

[v4hn]

Discussion of this continues in #713