How to implement a custom MoveGroupCapability for online path replanning

edit

Hi all,

This is a follow up of a previous question

First, I will summarize what I have so far,

The context

My settings

I have been using MoveGroupInterface to plan and execute simple pose goals and joint-space goals for UR5 robotic arm. Static obstacles, added before planning are taken into account. Robotic arm plans trajectory avoiding such obstacles or it informs when no plan can be found given the static obstacle.

What I want to achieve

The motion command has been issued to the robot. Now, an obstacle appears in the environment, the obstacle is in the way of previously calculated path. We want the robotic arm to adapt its trajectory, change its course, avoiding the obstacle.

What I have so far

I have basically, two simple achievements to that end so far,

(a) I have a node, that contains a planning scene monitor; I can monitor the environment and acknowledge new obstacles.

// Initialized with

m_pPlanningSceneMonitor = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description");
m_pPlanningSceneMonitor->startSceneMonitor("/move_group/monitored_planning_scene");
m_pPlanningSceneMonitor->startWorldGeometryMonitor("/collision_object");

// with read-only copy of planning scene, I can use the two handy functions

planning_scene_monitor::LockedPlanningSceneRO ps(m_pPlanningSceneMonitor);
ps->checkCollision(collision_request, collision_result);
ps->distanceToCollision(ps->getCurrentState())

These two functions are good, and I can make a safety stop of the robotic arm. But, I need to promptly (the sooner the better ) plan and execute another path to the goal. The simplest way is to stop, and then use m_MoveGroup->plan method, but it still needs quite some time.

Following suggestions given in the answer to the post,

(b) I use the m_MoveGroup->move() method, which requests to plan and execute. The path will be checked against every geometry update in the scene. The planner will be invoked again in this case to generate a new collision-free trajectory and execute the new one.

This approach lets the robotic arm stop, however, the response to move again can still be rather slow. It waits for a while before executing the new trajectory. And, often times, the trajectory is rather complicated. Using the OMPL planners, some planners show a better response than others, for example, RRTConnect. However, the desirable would be just shifting the part of the trajectory where obstacle appeared, and keeping the rest of the trajectory when possible.

Now,

Another suggestion was to implement a custom MoveGroupCapability.

In this regard, I want to know,

---- Is the following a good starting point link text

---- I am using ubuntu 18.04, ros-melodic. Does a newer ros version is needed to implement custom capabilities.

---- As a starting point, In a callback function, I can receive the computed trajectory, and then using the function isPathValid(my_path_msg.init_state, my_path_msg.traj, GetPlanningGroup())) to see, if the path will become invalid. There is sometime to react until hitting the obstacle. The simplest approach is to stop. I would want to compute a new trajectory and trying to stitch the old and new trajectories around the obstacle. Depending on how far is the moving arm to the dynamic obstacle, the time may not be enough to react (i..e ... (more)