Publishing to /planning_scene while running depth sensor
I have a robot with a depth sensor where I want to manually add an old Octomap from a scan to the planning scene. I tried publishing a PlanningScene
with the Octomap directly to /planning_scene
, but that doesn't seem to work if I have the depth sensor running at the same time. It briefly shows up in RViz, but seems to be overwritten by the depth sensor or the internal MoveIt Octomap right away. What is the correct way of doing this?
Edit: I've tried with and without PlanningScene.is_diff = True
, but the behaviour seems about the same. I'm currently experimenting some more with having the sensor on vs off while publishing to /planning_scene
.
Edit2: Some more results
- Start the scanner
- Store the current PlanningScene (in a Python module/class I wrote) via the service
get_planning_scene
- Stop the scanner
- Call service
clear_octomap
- Publish previous PlanningScene on
planning_scene
topic withPlanningScene.is_diff = True
At this point everything is the way I want, the old scanned scene is visible in RViz. But when I start the scanner again the old map disappears, and only newly scanned objects are visible.
Edit3: Sometimes when I publish on planning_scene
I get an error Found empty JointState message
. After this motion planning always fails, with warnings Unable to find a valid state nearby the start state
, Skipping invalid start state (invalid state)
, and error Motion planning start tree could not be initialized!
. I'm publising PlanningScene
with all fields empty except PlanningScene.world.octomap
, so the empty JointState message is true. I'm thinking that I'm not using planning_scene
in the intended way here...
Edit4: The Found empty JointState message
error does not appear if I use is_diff=True
.