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)