Moveit! RViz does not replan.

[Suh-Shin hwang]

Hi all,

I'm trying to use the "Allow Replanning" within the RViz plugin. (I'm essentially running a modified demo.launch)

Currently, my system is setup as so: Moveit RViz plugin <=> a Joint Trajectory Action <=> Intermediate conversion <=> Existing position controller.

The occupancy map is being continuously updated as the robot is moving around, but the path is not being replanned. Is there something I need to change or add into any of those components to allow the replanning feature? 

[Suh-Shin hwang]

No one has used the replan feature?

[Suh-Shin hwang]

I noticed something when executing the path. This is the output I have:

[ INFO] [1407941419.293362482, 1176.935000000]: Received new trajectory execution service request...

[ INFO] [1407941419.293862619, 1176.935000000]: JTAS: Goal recieved

[ INFO] [1407941419.294821606, 1176.940000000]: JTAS: Goal Accepted

[ INFO] [1407941419.294892393, 1176.940000000]: JTAS: Goal passed to waypoint server

[ INFO] [1407941419.295194399, 1176.940000000]: Waypoints server: Recieved waypoints. Executing

[ INFO] [1407941419.295300923, 1176.940000000]: MultiDOFFollowJointTrajectoryController: quadrotor_controller started execution

[ INFO] [1407941419.373221494, 1177.015000000]: Waypoints Server: Executing trajectory 1 of 9

[ INFO] [1407941419.495824006, 1177.140000000]: Waypoints Server: Executing trajectory 2 of 9

[ INFO] [1407941419.613873033, 1177.260000000]: Waypoints Server: Executing trajectory 3 of 9

[ INFO] [1407941419.733081540, 1177.380000000]: Waypoints Server: Executing trajectory 4 of 9

[ WARN] [1407941419.789193085, 1177.435000000]: Controller handle reports status RUNNING

[ INFO] [1407941419.789367873, 1177.435000000]: Execution completed: RUNNING

[ INFO] [1407941419.851666049, 1177.495000000]: Waypoints Server: Executing trajectory 5 of 9

[ INFO] [1407941419.975052014, 1177.620000000]: Waypoints Server: Executing trajectory 6 of 9

[ INFO] [1407941422.480743053, 1180.115000000]: Waypoints Server: Executing trajectory 7 of 9

[ INFO] [1407941422.599383536, 1180.235000000]: Waypoints Server: Executing trajectory 8 of 9

[ INFO] [1407941422.717795919, 1180.355000000]: Waypoints Server: Executing trajectory 9 of 9

[ INFO] [1407941422.836288740, 1180.475000000]: Waypoints server: All waypoints successfully executed, returning to JTAS

[ INFO] [1407941422.837188001, 1180.475000000]: JTAS: Trajectory execution success

In the middle of the execution, I see that there is a line saying that " Controller handle reports status RUNNING" and immediately it states that the execution is complete.

Does the execution of the controller handle have any impact on the replan feature?

On Tuesday, August 5, 2014 10:01:54 AM UTC-4, Suh-Shin hwang wrote:

Hi all,

(chop)

[Suh-Shin hwang]

I think I also should ask: when does replanning occur? As in which specific part of the program loop?

I am unclear when MoveIt computes a motion path and sends the path to the joint trajectory action server, how does MoveIt monitor whether or not replan needs to happen.

[Zdeněk Materna]

I 'm also interested... Can anyone explain it or point to appropriate code? Thanks.

Dne středa, 13. srpna 2014 17:20:31 UTC+2 Suh-Shin hwang napsal(a):

...how does MoveIt monitor whether or not replan needs to happen.

[Acorn Pooley]

Replanning occurs if a plan is found to have a problem while the plan is actually being executed.  For example if a new obstacle appears in the world (e.g. in the octomap) while the plan is being executed, and that obstacle is in the path of the plan such that the plan would cause the robot to collide with this new obstacle, then the path execution is halted.  MoveIt waits for a certain amount of time to see if the obstacle goes away.  If the old plan is still blocked after the wait then MoveIt generates a new plan to the same goal and executes that plan instead.

Possible problems that could occur:
- if no new plan can be found (e.g. because the obstacle blocks all paths to the goal)
- if the octomap does not update fast enough and the previous plan completes even though a new obstacle appears (in this case the arm may hit the new obstacle)
- Sometimes voxels will be added to the octomap and then not be cleared.  If these prevent a new plan from succeeding then replanning will fail.  It will also fail if these voxels overlap with the robot's current position.

-Acorn

---

From: moveit...@googlegroups.com [moveit...@googlegroups.com] on behalf of Zdeněk Materna [zdenek....@gmail.com]
Sent: Tuesday, September 30, 2014 5:57 AM
To: moveit...@googlegroups.com
Subject: Re: Moveit! RViz does not replan.

[Zdeněk Materna]

Thanks for clarification, but how exactly moveit reasons about new obstacle? Is it somehow monitoring that something new appeared in workspace / octomap or is it just testing next robot state for collision?

Zdenek

Dne úterý, 30. září 2014 22:21:43 UTC+2 Acorn Pooley napsal(a):

[Acorn Pooley]

The octomap monitor is continuously monitoring the sensor (depth image or pointcloud) and adding obstacles to the octomap as they appear.

Meanwhile MoveIt execution manager is testing the remaining portion of the trajectory that is being executed to see if it is still collision free.  If it is not collision free it stops the plan (and optionally replans if the old trajectory is still not collision free after a given period of time).

-Acorn

---

From: moveit...@googlegroups.com [moveit...@googlegroups.com] on behalf of Zdeněk Materna [zdenek....@gmail.com]

Sent: Wednesday, October 01, 2014 1:53 AM
To: moveit...@googlegroups.com
Cc: zdenek....@gmail.com

Subject: Re: Moveit! RViz does not replan.

[Suh-Shin hwang]

Hi Zdenek,

I'm not sure if the later versions have fixed it as I am on Groovy build, but I noticed the move() function in the move_group.cpp inside moveit_ros/planning_interface/move_group_interface/src/ does not contain any lines that contain the message for replanning.

If you check this link on the message of planning_options that gets used in the move() function, there's a field for replan and replan attempts. However, in the code of the move() function, there is no mention to these fields before the goal is sent. So I basically added these lines to fix it. There might also be lines in the constructGoal() function that is relevant but please check for yourself (it's been a while since I fixed it and I didn't document it... bad habit)

-Suh-Shin

[Dina Youakim]

Hi all,

I am not able to produce this behavior in MoveIt!, I have an octomap updater and it shows well in rviz. I tried with plan/execute and with move but in both cases the path becomes invalid in the middle of the execution, but the execution continues anyway. Any suggestion what I would be missing ??

Cheers,

Dina.