The robotic platform I use is rail-mounted and has a manipulator 6-DOF robotic arm). I am using MoveIt! to control the arm. The rail the robot is mounted on is added as a collision object. I want to add the rest of the environment (a large steel tank) as an OctoMap, since I want to perform raycasting.
I publish the OctoMap using the octomap_server_node from the octomap_server package. I republish it to the monitored planning scene using the following snippet:
class OctoHandler():
mapMsg = None
def __init__(self):
rospy.init_node('moveit_octomap_handler')
rospy.Subscriber('octomap_full', Octomap, self.cb, queue_size=1)
pub = rospy.Publisher('move_group/monitored_planning_scene', PlanningScene, queue_size=1)
r = rospy.Rate(0.25)
while(not rospy.is_shutdown()):
if(self.mapMsg is not None):
pub.publish(self.mapMsg)
else:
pass
r.sleep()
def cb(self, msg):
psw = PlanningSceneWorld()
psw.octomap.header.stamp = rospy.Time.now()
psw.octomap.header.frame_id = 'map'
psw.octomap.octomap = msg
psw.octomap.origin.position.x = 0
psw.octomap.origin.orientation.w = 1
ps = PlanningScene()
ps.world = psw
ps.is_diff = True
self.mapMsg = ps
The OctoMap is added to the planning scene and visible in Rviz. However, instead of the in the global map frame, the visualization is projected in the start_rail frame, the root of my planning scene:
↧