An error occurs in sphereMap

[zxx01]

Thank you for your open-source code. I encountered these errors while running the example, how can I resolve them?

[ChanJoon]

I met the same problem. How did you resolve it?

[zxx01]

I met the same problem. How did you resolve it?

Hmm... I just wanted to see how the algorithm performed at that time, so I didn’t try to fix it.

[ChanJoon]

Thank you for sharing your experience and quick response. Like you, I’m interested in looking into the algorithm, but it’s sad to see how it actually works.

Anyway, if I find a solution, I’ll post it on this thread. Thanks again!

[MrTomzor]

Hi, sorry for the late reply. Thanks for bringing this up. I compiled it now on my machine with that using 'catkin build' and ran the example.launch now and did not encounter the error you sent, and I also don't remember seeing the error ever during the development, so I hope we can figure it out.

From the image you uploaded, a message above the red node crash error shows some failed assertion 'children == Null' in OcTreeDataNode, which is called in the code of OctoMap. I can't see a stack trace in the image you sent, so I'm unsure of what triggered that to be called in the Spheremap code. You could compile the C++ code with debug flags and run it with gcc to show the stack trace, that could help in figuring it out. However, I think that most likely you have an incompatible version of OctoMap. What version are you using? Our octomap package version is 1.9.8 and the version of the octomap_ros package is 0.4.1, so I suggest you try those and let us know if it helps.

I also don't see the TF_REPEATED_BUFFER warning when I run it, that one seems very weird and could be the problem. How exactly did you compile and run the code? Do you maybe run the example.launch and also play the rosbag separately? Also the map you sent in the image looks very disconnected. Did you change any parameters? Without them, the map should be fully connected like in this video: https://www.youtube.com/watch?v=NEgUuRps9Tk

[ChanJoon]

@MrTomzor Thanks. As you said, it was indeed an issue with Octomap.
I found a reason and here's how anyone facing this issue can resolve it:

1. Modify OcTreeBaseImpl.hxx in your Octomap. Refer to Fix children[] array not deleted in deleteNodeRecurs function. OctoMap/octomap#288 for details.

2. This requires manually install and build these packages octomap, octomap-ros, octomap-msgs.
   (After that, build spheremap again.)

Then you can successfully run example.launch.

I don't know why octomap_ros installed via binaries isn't working, as @MrTomzor said.
(I checked both octomap_ros and liboctomap-dev version but there was no problem.)

By the way, could you check if this is the correct visualization of darpa_subt_finals_uav_green.bag?

I don't see any meshes in LTV-map example or the purple spheres shown in the README.md.

[AngelRodriguez8008]

Hi All,

@MrTomzor got the same error.

nodelet: /opt/ros/noetic/include/octomap/OcTreeDataNode.hxx:69: octomap::OcTreeDataNode<T>::~OcTreeDataNode() [with T = octomap::SphereMapNodeData]: Assertion `children == NULL' failed.
  [spot/spheremap_server_mapper-3] process has died
  [pid xxx, exit code -6, cmd /opt/ros/noetic/lib/nodelet/nodelet standalone spheremap_server/MappingNodelet 
  ~spheremap_spheres:=spheremap_server_vis/spheremap/spheres 
  ~spheremap_graph:=spheremap_server_vis/spheremap/graph 
  ~spheremap_topology:=spheremap_server_vis/spheremap/topology 
  ~spheremap_navigation:=spheremap_server_vis/spheremap/navigation 
  ~spheremap_debug:=spheremap_server_vis/spheremap/debug 
  ~odom_uav_in:=/state_estimation 
  ~octomap_binary_in:=/octomap_binary 
  ~get_spheremap_path:=~get_spheremap_path 
  ~set_safety_planning_params:=~set_safety_planning_params 
  __name:=spheremap_server_mapper

My versions are:

ros-noetic-octomap               1.9.8
ros-noetic-octomap-mapping       0.6.8
ros-noetic-octomap-msgs          0.3.5
ros-noetic-octomap-ros           0.4.1
ros-noetic-octomap-rviz-plugins  0.2.4
ros-noetic-octomap-server        0.6.8

I will try to fix & recompile Octomap as @ChanJoon suggest.

Thanks for sharing.

[AngelRodriguez8008]

Hi All,

the @ChanJoon approach works for me.
But I needed to rebuild all these:

octomap
octomap_mapping
octomap_msgs
octomap_ros
octomap_rviz_plugins

hier are my changes:

Final code:

\octomap\octomap\include\octomap\OcTreeBaseImpl.hxx - Line 708 -714:
if (!nodeHasChildren(node)) {
    if (node->children != NULL) {
        delete[] node->children;
        node->children = NULL;
    }
    return true;
}

Result on my own environment:
Point Cloud:

Octomap (it was a bit difficult to find the proper input clouds & frame parameters configuration):

Spheremap:

I didn't find these magenta Spheres, image from video:

It also works for multi-level environments.

Questions (I need to go deeper into it.):
Why they are disconnected nodes on the topological graph?
Can I constrain the Sphere's height to the Robot sensor height?

I am looking to use the Ongoing Generated Map for Autonomous Exploration, for ground robots in indoors multi-level environments. Any suggestion is welcome.

Thanks a lot at @MrTomzor & @penickar

Best regards.

Angel A.

[MrTomzor]

Thank you @ChanJoon and @AngelRodriguez8008 for solving the problems related to octomap!

@ChanJoon the spheremap looks correct in your image. There should not be any mesh, the LTV-Map only contains a graph where the shape and position of each segment in the spheremap is abstracted as a bounding box at the segment's position (which you have in the image) and the connectivity between abstracted segments is kept the same as in the original segments.

The purple spheres are frontier clusters that we used to send attached to the extracted LTV-Map clusters during SubT. In the image in the readme and in the video, it was meant to be an illustration of how spatial information related to your specific mission can be attached to the LTV-Map for sharing among robots, I'm sorry if it was misleading. The spheremap version in this repo is meant primarily for the rapid path-finding and generation of the LVT-Map for sharing lightweight topology information, to which you can then attach spatial data specific for your mission. If you want to know how we used LTV-Map for map sharing and merging frontier goals among multiple UAVs, please check out our other paper UAVs Beneath the Surface: Cooperative Autonomy for Subterranean Search and Rescue in DARPA SubT

@AngelRodriguez8008 the disconnected nodes and segments are ok in the map. This can happen for example because due to sensor noise or in dynamic environments, some free space can seem connected at first, but in another update, when a cell becomes occupied, the spheres will shrink and can become disconnected. I see in your image that small segments along the walls are disconnected, I suspect this is a few lone spheres that occured in this way because in the first few octomap updates at that area the free space was connected. However, the pathfinding service in spheremap checks multiple nearby segments at both the start and end positions, so it should not be a problem even if you start planning near these unconnected few spheres.

You can also of course constrain spheres to be created only at some height, but you would have to change the code. I would maybe not recommend using spheremap for path planning on a ground robot (if that is what you want to do), because the spheres often do not cover all the free space near the ground. For a ground robot, you could maybe change parts of the code to use cylinders instead of spheres. Also the path planning in the spheremap is for flying robots, it does not try to stay close to the ground. But you could also just use spheremap for extracting the topology and connectivity and handle path planning separately.

I'd be happy to talk about spheremap and your use-cases in more detail if you want to, feel free to email me at musilto8 at fel.cvut.cz.