Create OcTree object from octomap_msgs/Octomap message type in ROS

[ewan-pugh]

Hi,

I am trying to create an OcTree object from an octomap ROS message but cannot figure out how to do it. I can successfully construct the stream header from the message but do not know how to serialize the data into the stream. Does anyone know how to do this?

This is our current attempt which does not work:

x = np.array(msg.data).astype(np.int8)
stream = b"# Octomap OcTree binary file id OcTree\nsize {}\nres {}\ndata\n".format(len(x), msg.resolution)
stream += x.tobytes()
octree = octomap.OcTree(msg.resolution)
octree.readBinary(stream)

[boazMgm]

Hi, I'm facing the same issue. Did you find any solution? I'm using rospy.ServiceProxy to get the map but I don't understand how to convert it to Octree. Thanks

[apurv-andromeda]

For those still looking for a solution,

stream = bytes(
            bytearray(
                f"# Octomap OcTree binary file\n# (feel free to add / change comments, but leave the first line as it is!)\n#\nid {[msg.id](http://msg.id/)}\nsize 0\nres {msg.resolution}\ndata\n{msg.data}",
                "utf-8",
            )
        )
        octree = octomap.OcTree(msg.resolution)
        octree.readBinary(stream)

[Cypre55]

If you are looking to read a regular OcTree (not binary), you can do the following:

def get_octree_from_msg (msg):        
        map = msg.map # OctomapMsg
        fileHeader = "# Octomap OcTree file"
        s = ""
        s += fileHeader + "\n# (feel free to add / change comments, but leave the first line as it is!)\n#\n"
        s += "id " + map.id + "\n"
        s += "size " + str(10) + "\n" # It does not matter what size you put here
        s += "res " + str(map.resolution) + "\n"
        s += "data\n"

        # convert string to bytes
        s = s.encode('utf-8')

        length = len(map.data)
        pattern = '<%sb'%length
        buff = io.BytesIO()
        buff.write(struct.Struct(pattern).pack(*map.data))

        # bytes to string
        s += buff.getvalue()

        # Random Resolution just to initialize the object
        # The correct resolution will be set on reading
        tree = octomap.OcTree(0.1).read(s) 
        return tree