MotionPlanning RViz plugin have to be manually refreshed after clear_octomap

[FSund]

Description

After calling the /clear_octomap service, the MotionPlanning RViz plugin have to be reloaded (uncheck and re-check the plugin, or remove/add), for the octomap to disappear from the Planning Scene.

The same happens if an empty octomap is loaded from a file via /move_group/load_map (the old octomap is still shown until the plugin is reloaded). The empty map is generated via /move_group/save_map on a fresh scene.

The same behaviour is present when calling the /apply_planning_scene service with an empty octomap:
(untested code, it is a minimal version of the actual code I use)

get_planning_scene = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene)
components = PlanningSceneComponents()
components.components = sum([
    PlanningSceneComponents.WORLD_OBJECT_NAMES,
    PlanningSceneComponents.WORLD_OBJECT_GEOMETRY,
    PlanningSceneComponents.OCTOMAP
])
ps = get_planning_scene(components)
ps = ps.scene  # moveit_msgs/PlanningScene
ps.is_diff = True
ps.world.octomap = OctomapWithPose()
apply_planning_scene = rospy.ServiceProxy('/apply_planning_scene', ApplyPlanningScene)
apply_planning_scene(ps)

I am not sure if any of these 3 methods of clearing the octomap are related, but I thought I'd just as well report all the similar issues I've run into.

Your environment

• ROS Distro: Noetic
• OS Version: Pop!OS
• Built from source, branch master (moveit repo is on commit e7b388d)
• The behaviour seems to be the same on moveit 1.1.7 from apt on Pop!OS 20.04

Steps to reproduce

• Start RViz and moveit
• Load/scan any octomap
• Call service /clear_octomap

Expected behaviour

I would expect the octomap to disappear from the Planning Scene after the service is called.

Actual behaviour

The octomap is still shown until I reload the MotionPlanning plugin.

[atmagopal]

I've been experiencing the same issue. Have you had any progress towards fixing this @FSund ?

[v4hn]

Quickly skimming the code clearing the octomap should trigger `UPDATE_GEOMETRY` which *should* trigger publishing a new ROS message with the cleared octomap. Can you see the message on the `move_group/monitored_planning_scene` topic when you clear the scene? Either the trigger fails or the new message is interpreted wrongly in the RViz plugin.

[FSund]

Can you see the message on the move_group/monitored_planning_scene topic when you clear the scene?

Yes, the following message appears on the monitoried_planning_scene topic when I press "Clear octomap" in RViz

---
name: "(noname)+"
robot_state: 
[...]
world: 
  collision_objects: []
  octomap: 
    header: 
      seq: 0
      stamp: 
        secs: 0
        nsecs:         0
      frame_id: "ab_base"
    origin: 
      position: 
        x: 0.0
        y: 0.0
        z: 0.0
      orientation: 
        x: 0.0
        y: 0.0
        z: 0.0
        w: 0.0
    octomap: 
      header: 
        seq: 0
        stamp: 
          secs: 0
          nsecs:         0
        frame_id: ''
      binary: False
      id: ''
      resolution: 0.0
      data: []
is_diff: True
---

The Octomap is still visible in RViz though.

[v4hn]

I see. The error is that we check for whether data is empty [here](https://github.com/ros-planning/moveit/blob/master/moveit_core/planning_scene/src/planning_scene.cpp#L1291-L1293) to decide whether or not an octomap is represented in the message or whether it is not filled in. But the empty data array should be permitted to mean the octomap is empty. I would propose to change that conditional to `!scene_msg.world.octomap.id.empty()` to decide whether the map is filled in. However, from your paste it also seems we do not actually set the `id` field when it's cleared, so in order for this change to work you also have to modify the places that can fill in the octomap message in case it is empty and set the `id` field to `OcTree` [which is the only allowed non-empty value anyway](https://github.com/ros-planning/moveit/blob/master/moveit_core/planning_scene/src/planning_scene.cpp#L1371). Could you implement this and provide a PR if it works? I do not have a setup around to test this.

[FSund]

EDIT: I think I understand what you meant now. An empty id field should indicate an empty octomap. So I just have to figure out where the octomap message is created when the Clear octomap button is pressed. I think I've narrowed it down to somewhere in PlanningSceneMonitor::scenePublishingThread.

I am not sure what you are suggesting. Should the id field always be filled? Or should the id field only be empty when the octomap is empty?

I'll have a look at the code and try to figure out where everything is being manipulated.

[FSund]

EDIT: I've now realized that removing that check will clear the octomap every time a diff message is sent, if the octomap field is not filled.

To me the check here seems superflous, since map.data.empty() and map.id != OcTree is checked here. If I remove the check the octomap disappears from RViz when I press "Clear octomap".

But perhaps I'm not understanding how everything fits together properly.

[FSund]

I have made a pull request with a preliminary test, to check how CI responds to my changes.

[FSund]

It seems the octomap message is created in PlanningScene::getOctomapMsg. The issue I'm having is that the id is set in fullMapToMsg, so this is not available if map does not exist (which I'm guessing it doesn't if the octomap has been cleared).

Seeing as "OcTree" is already hardcoded here I could just hardcode it myself, but it seems like a bit of a hack.