Publishing to /planning_scene while running depth sensor

asked 2021-04-14 02:03:01 -0500

FSund gravatar image

updated 2021-04-14 03:25:03 -0500

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 with PlanningScene.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, 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.

edit retag flag offensive close merge delete