CurrentState compute time

[samraise]

When planning a task the CurrentState stage always seems to take a long amount of time for our robot because of this line: void CurrentState::compute() { scene_ = std::make_shared<planning_scene::PlanningScene>(robot_model_); that's run every time compute is called(). Ideally I'd like to create the default scene only when the the CurrentState is initialized or better yet created. This problem become worse when a stage fails and has to create the scene_ more than once. Is there a way to workaround this? If not, any recommendations on creating the PlanningScene object faster?

[rhaschke]

Allocating the planning scene is nothing which takes much time. What takes time is getting the current robot state. If MoveIt is slow on this, your robot probably publishes (some) joint states with a low frequency only.

As task pipeline should use a CurrentState stage only once in the beginning. If you want to continue from some other stage (in your case with placing the bolt), you should use a MonitoringGenerator stage to "resume" from solutions generated by the source stage.

[samraise]

Allocating the planning scene is nothing which takes much time. What takes time is getting the current robot state.

I've tried debugging CurrentState::compute() by inserting some print statements with timestamps and, at least for me, it is allocating the planning scene which takes around 2.5 seconds, specifically when allocateEnv gets called.

you should use a MonitoringGenerator stage to "resume" from solutions generated by the source stage.

I'll try looking at this. I'm currently using the Python wrapper so creating subclasses that work properly has been difficult in the past.

Thanks for the help!

[rhaschke]

As you pointed out, CurrentState uses the RobotModel-based PlanningScene constructor. The code you are referring to now is a constructor receiving a parent scene. So, these are definitely unrelated.

You should use a MonitoringGenerator stage to "resume" from solutions generated by the source stage.

I'm currently using the Python wrapper so creating subclasses that work properly has been difficult in the past.

Have a look at PyMonitoringGenerator for an example.

[samraise]

Sorry I gave you a link to the wrong place. Here's the stack where the issue is occurring: initialize->setActiveCollisionDetector->addCollisionDetector->allocateEnv

Let me know if ros-planning/moveit is a better place to ask this

[samraise]

Also I guess this isn't clear in my original post. I'm not trying to return to the original state. I'm simply running the sequences pick and place (current->connect->move relative->etc) as one shots repeatedly.

[rhaschke]

If allocating the environment is costly for you, you have probably very large object meshes, do you? Moving the allocation of the planning scene to the stage initialization should be easily possible. However, I don't consider this myself as the CurrentState stage should only be executed once by construction. Thus, it doesn't matter if the scene allocation is in init() or in compute(). Both are called as part of Task::plan() and should run exactly once.

Having said that, I'm really puzzled why your CurrentState stage in the place container is run 6 times!?

[rhaschke]

I'm simply running the sequences pick and place (current->connect->move relative->etc) as one shots repeatedly.

Also (and particularly) in this case, you shouldn't start over from CurrentState, but from where you left off picking! Remember that planning and execution phases are separated. If you use CurrentState as the start for placing, you essentially start from where the robot is when calling Task::plan(), i.e. before having picked anything.

[samraise]

I yet again underexplained my set up: For each task I'm planning, executing, and then moving onto the next task. If the plan fails, I retry planing a certain number of times. The first picture was me trying to show that when the place sequence fails (because of the "Pose" stage) I had to create the PlanningScene in CurrentState::compute() every time I ran task.plan(). My original question was more along the lines of if I could create the PlanningScene once for a task and not have to recreate it when calling plan/compute again.

There are some needlessly complex meshes in my URDF which is probably the underlying issue as you pointed out. I'll look at simplifying those and going from there. Thanks again for the help.

[rhaschke]

No, there is no way to create the PlanningScene once already during stage creation. As I explained, Task::plan() will call both Stage::init() and Stage::compute().