def __add_free_space(name, x, y, operation=CollisionObject.ADD):
sp = SolidPrimitive()
sp.type = SolidPrimitive.BOX
sp.dimensions = [0.5, 0.5, 0.1]
spp = PoseStamped()
spp.header.frame_id = 'map'
spp.pose.position.x = x
spp.pose.position.y = y
spp.pose.position.z = 0.05
spp.pose.orientation.w = 1
co = CollisionObject()
co.operation = operation
co.id = name
co.type.db = json.dumps({
'table': 'NONE',
'type': 'free_space',
'name': co.id
})
co.header.frame_id = 'map'
co.header.stamp = rospy.get_rostime()
co.primitives.append(sp)
co.primitive_poses.append(TF2().transform_pose(spp, spp.header.frame_id,
co.header.frame_id).pose)
psw = PlanningSceneWorld()
psw.collision_objects.append(co)
obstacle_pub.publish(psw)
rospy.loginfo("Added a fake obstacle named '%s'", name)
def __remove_virtual_obstacle(name):
co = CollisionObject()
co.operation = CollisionObject.REMOVE
co.id = name
co.type.db = json.dumps({'table': 'NONE', 'type': 'obstacle', 'name': co.id})
psw = PlanningSceneWorld()
psw.collision_objects.append(co)
obstacle_pub.publish(psw)
rospy.loginfo("Removed a fake obstacle named '%s'", name)
def cb(self, msg):
psw = PlanningSceneWorld()
psw.octomap.header.stamp = rospy.Time.now()
psw.octomap.header.frame_id = 'world'
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
def update_scene(self, points):
""" for debugging purposes, sends object, table and octomap clearing in same message"""
ps = PlanningSceneWorld()
# clear the octomap
ps.octomap.header.frame_id = 'odom_combined'
ps.octomap.octomap.id = 'OcTree'
# add object bounding box
(box_pose, box_dims) = self.get_bounding_box(points)
bbox_primitive = SolidPrimitive()
bbox_primitive.type = bbox_primitive.BOX
bbox_primitive.dimensions = [box_dims.x, box_dims.y, box_dims.z]
bbox = CollisionObject()
bbox.id = 'obj1'
bbox.header.frame_id = box_pose.header.frame_id
bbox.operation = bbox.ADD
bbox.primitives.append(bbox_primitive)
bbox.primitive_poses.append(box_pose.pose)
ps.collision_objects.append(bbox)
# and, table
table_pose = PoseStamped()
table_pose.header.frame_id = "odom_combined"
table_pose.pose.position.x = 0.55
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = 0.75
table_pose.pose.orientation.w = 1.0
table_dims = Point()
table_dims.x = 0.7
table_dims.y = 1.0
table_dims.z = 0.05
table_primitive = SolidPrimitive()
table_primitive.type = table_primitive.BOX
# TODO: this is a hack to make the buffer bigger.
# By default, it appears to be 10cm x 10cm x 0cm, but I'm not sure which dimension is which
table_primitive.dimensions = [table_dims.x, table_dims.y, table_dims.z]
table = CollisionObject()
table.id = 'table'
table.header.frame_id = table_pose.header.frame_id
table.operation = table.ADD
table.primitives.append(table_primitive)
table.primitive_poses.append(table_pose.pose)
ps.collision_objects.append(table)
self.scene_diff_pub.publish(ps)
def set_scene(self, scene):
self.current_scene = scene
psw = PlanningSceneWorld()
for co_json in scene['objects']:
# TODO: Fix orientation by using proper quaternions on the client
pose = self._make_pose(co_json['pose'])
# TODO: Decide what to do with STL vs. Collada. The client has a Collada
# loader but the PlanningSceneInterface can only deal with STL.
# TODO: Proper mapping between filenames and URLs
# filename = '/home/julian/aaad/moveit/src/moveit_web/django%s' % co_json['meshUrl']
filename = '/home/julian/aaad/moveit/src/moveit_web/django/static/meshes/table_4legs.stl'
co = self.ps.make_mesh(co_json['name'], pose, filename)
psw.collision_objects.append(co)
self.psw_pub.publish(psw)
def clear_scene(self):
ps = PlanningSceneWorld()
ps.octomap.header.frame_id = 'odom_combined'
#ps.octomap.octomap.data.append(0) #TOTAL HACK. I have no clue what this actually does ...
ps.octomap.octomap.id = 'OcTree'
self.scene_diff_pub.publish(ps)