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
