Revision history [back]

Hey,

to get the obstacles in MoveIt (octomap) from rtabmap, you have to write a script that subscribes to the rtabmap-octomap

rospy.Subscriber('octomap_binary', Octomap, your_callback, queue_size=1)

and publish this into move_group/monitored_planning_scene

rospy.Publisher('move_group/monitored_planning_scene', PlanningScene, queue_size=1)

Full script:

#! /usr/bin/env python

import rospy
from octomap_msgs.msg import Octomap
from moveit_msgs.msg import PlanningScene, PlanningSceneWorld, CollisionObject

frame_id = 'map'
rospy.init_node('moveit_octomap_handler')
pub_octo = rospy.Publisher('move_group/monitored_planning_scene', PlanningScene, queue_size=10)
pub2 = rospy.Publisher('/planning_scene', PlanningScene, queue_size=10)
psw = PlanningSceneWorld()
psw.octomap.header.stamp = rospy.Time.now()
psw.octomap.header.frame_id = frame_id
psw.octomap.origin.position.x = 0
psw.octomap.origin.orientation.w = 1
ps = PlanningScene()


def cb(msg):
    col_obj = CollisionObject()
    col_obj.header.frame_id = frame_id

    psw.collision_objects.append(col_obj)
    psw.octomap.octomap = msg
    ps.world.collision_objects.append(col_obj)
    ps.world = psw
    ps.is_diff = True
    pub_octo.publish(ps)
    pub2.publish(ps)


if __name__ == '__main__':
    print("Starte die Uebertragung von Rtabmap-Octomap in die Pfadplanung")
    rospy.Subscriber('octomap_binary', Octomap, cb, queue_size=1)  # octomap_full
    rospy.spin()

This works for me but there is probably a more elegant way.

Robert