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 add_invisible_collision_object(self): co_box = CollisionObject() co_box.header.frame_id = 'base_footprint' co_box.id = 'invisible_box' box = SolidPrimitive() box.type = SolidPrimitive.BOX box_height = 0.76 box.dimensions.append(0.80) box.dimensions.append(1.60) box.dimensions.append(box_height) co_box.primitives.append(box) box_pose = Pose() box_pose.position.x = 0.80 box_pose.position.y = 0.0 box_pose.position.z = box_height / 2.0 box_pose.orientation.w = 1.0 co_box.primitive_poses.append(box_pose) co_box.operation = CollisionObject.ADD color = ObjectColor() color.id = 'invisible_box' color.color.g = 1.0 color.color.a = 0.15 ps = PlanningScene() ps.world.collision_objects.append(co_box) ps.object_colors.append(color) ps.is_diff = True try: self.planning_scene_service(ps) except rospy.ServiceException, e: print("Service call failed: %s" % e)
def add_padded_lab(): rospy.loginfo("Adding padded order bin") poses = [] objects = [] o_b_pose = Pose() o_b_pose.orientation = Quaternion(x=0, y=0, z=0, w=1) o_b_pose.position = Point(x=0.5, y=0.2, z=0.22) objects.append( SolidPrimitive(type=SolidPrimitive.BOX, dimensions=[0.43, 0.68, 0.46])) poses.append(o_b_pose) wiring_pose = Pose() wiring_pose.orientation = Quaternion(x=0, y=0, z=0, w=1) wiring_pose.position = Point(x=-.55, y=0, z=0.15) objects.append( SolidPrimitive(type=SolidPrimitive.BOX, dimensions=[0.6, 0.7, 0.4])) poses.append(wiring_pose) co = CollisionObject() co.operation = CollisionObject.ADD co.id = "padded_lab" co.header.frame_id = "/base_link" co.header.stamp = rospy.Time.now() co.primitives = objects co.primitive_poses = poses scene._pub_co.publish(co)
def remove_object(self, name): remove_object = CollisionObject() remove_object.header.frame_id = "odom_combined" remove_object.id = name remove_object.operation = remove_object.REMOVE self.collision_pub.publish(remove_object)
def make_mesh(name, pose, filename, scale=(1, 1, 1)): ''' returns a CollisionObject with the mesh defined in the path. mostly copied from https://github.com/ros-planning/moveit_commander/blob/kinetic-devel/src/moveit_commander/planning_scene_interface.py ''' print("Creating mesh with file from", filename) co = CollisionObject() scene = pyassimp.load(filename) if not scene.meshes: raise MoveItCommanderException("There are no meshes in the file") co.operation = CollisionObject.ADD co.id = name co.header = pose.header mesh = Mesh() for face in scene.meshes[0].faces: triangle = MeshTriangle() if len(face) == 3: triangle.vertex_indices = [face[0], face[1], face[2]] mesh.triangles.append(triangle) for vertex in scene.meshes[0].vertices: point = Point() point.x = vertex[0] * scale[0] point.y = vertex[1] * scale[1] point.z = vertex[2] * scale[2] mesh.vertices.append(point) co.meshes = [mesh] co.mesh_poses = [pose.pose] pyassimp.release(scene) return co
def _remove_command(self, name): co = CollisionObject() co.header.stamp = rospy.Time.now() co.header.frame_id = self._planning_frame co.id = name co.operation = CollisionObject.REMOVE return co
def create_collision_object(shape_type,pos,size,frame_id,op,object_id): col = CollisionObject() col.id = object_id col.operation = op col.header.frame_id=frame_id # create primitive primitive = SolidPrimitive() primitive.type = shape_type primitive.dimensions = size # create pose pose = Pose() pose.position.x = pos[0] pose.position.y = pos[1] pose.position.z = pos[2] pose.orientation.x = pose.orientation.y = pose.orientation.z = 0 pose.orientation.w = 1 col.primitives = [primitive] col.primitive_poses = [pose] return col
def form_attach_collision_object_msg(self, link_name, object_name, size, pose): obj = AttachedCollisionObject() # The CollisionObject will be attached with a fixed joint to this link obj.link_name = link_name # This contains the actual shapes and poses for the CollisionObject # to be attached to the link col_obj = CollisionObject() col_obj.id = object_name col_obj.header.frame_id = link_name col_obj.header.stamp = rospy.Time.now() sp = SolidPrimitive() sp.type = sp.BOX sp.dimensions = [0.0] * 3 sp.dimensions[0] = size.x sp.dimensions[1] = size.y sp.dimensions[2] = size.z col_obj.primitives = [sp] col_obj.primitive_poses = [pose] # Adds the object to the planning scene. If the object previously existed, it is replaced. col_obj.operation = col_obj.ADD obj.object = col_obj # The weight of the attached object, if known obj.weight = 0.0 return obj
def remove_object(name="Object"): co = CollisionObject() co.operation = CollisionObject.REMOVE co.id = name co.header.frame_id = "/base_link" co.header.stamp = rospy.Time.now() while scene._pub_co.get_num_connections() == 0: rospy.sleep(0.01) scene._pub_co.publish(co) while True: rospy.sleep(0.1) result, success = get_planning_scene( PlanningSceneComponents( PlanningSceneComponents.WORLD_OBJECT_NAMES + PlanningSceneComponents.WORLD_OBJECT_GEOMETRY ) ) if not success: continue found = False for object in result.scene.world.collision_objects: if object.id == name: found = True if not found: return
def add_box_obstacle(self, size, name, pose): """ Adds a rectangular prism obstacle to the planning scene Inputs: size: 3x' ndarray; (x, y, z) size of the box (in the box's body frame) name: unique name of the obstacle (used for adding and removing) pose: geometry_msgs/PoseStamped object for the CoM of the box in relation to some frame """ # Create a CollisionObject, which will be added to the planning scene co = CollisionObject() co.operation = CollisionObject.ADD co.id = name co.header = pose.header # Create a box primitive, which will be inside the CollisionObject box = SolidPrimitive() box.type = SolidPrimitive.BOX box.dimensions = size # Fill the collision object with primitive(s) co.primitives = [box] co.primitive_poses = [pose.pose] # Publish the object self._planning_scene_publisher.publish(co)
def remove_object(self, name, type, costmap='both'): """ Remove an object from one or both costmaps. :param name: object name :param type: object type :param costmap: 'local', 'global' or 'both' """ co = CollisionObject() co.operation = CollisionObject.REMOVE co.id = name co.type.db = json.dumps({'table': 'NONE', 'type': type, 'name': name}) try: if costmap in ['local', 'both']: resp = self._lcm_sl_srv([co]) if resp.error.code != ThorpError.SUCCESS: rospy.logerr( "Unable to remove object %s from %s costmap: %d", name, costmap, resp.error.code) return False if costmap in ['global', 'both']: resp = self._gcm_sl_srv([co]) if resp.error.code != ThorpError.SUCCESS: rospy.logerr( "Unable to remove object %s from %s costmap: %d", name, costmap, resp.error.code) return False rospy.loginfo("Removed object %s of type %s from %s costmap", name, type, costmap) return True except rospy.ServiceException as err: rospy.logerr("Unable to remove object %s from %s costmap: %s", name, costmap, str(err)) return False
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 makeMesh(self, name, pose, filename): if not use_pyassimp: rospy.logerr( 'pyassimp is broken on your platform, cannot load meshes') return scene = pyassimp.load(filename) if not scene.meshes: rospy.logerr('Unable to load mesh') return mesh = Mesh() for face in scene.meshes[0].faces: triangle = MeshTriangle() if len(face.indices) == 3: triangle.vertex_indices = [ face.indices[0], face.indices[1], face.indices[2] ] mesh.triangles.append(triangle) for vertex in scene.meshes[0].vertices: point = Point() point.x = vertex[0] point.y = vertex[1] point.z = vertex[2] mesh.vertices.append(point) pyassimp.release(scene) o = CollisionObject() o.header.stamp = rospy.Time.now() o.header.frame_id = self._fixed_frame o.id = name o.meshes.append(mesh) o.mesh_poses.append(pose) o.operation = o.ADD return o
def publishCO(self, points, id, is_table=False): pts = np.asarray(points) dx, dy, dz = np.max(pts, axis=0) - np.min(pts, axis=0) max_x, max_y, max_z = np.max(pts, axis=0) min_x, min_y, min_z = np.min(pts, axis=0) collision_object = CollisionObject() collision_object.id = id collision_object.header = self.lastHeader object_shape = SolidPrimitive() object_shape.type = SolidPrimitive.BOX object_shape.dimensions.append(dx) object_shape.dimensions.append(dy) shape_pose = Pose() shape_pose.position.x = min_x + dx / 2 shape_pose.position.y = min_y + dy / 2 if is_table: object_shape.dimensions.append(max_z) shape_pose.position.z = max_z / 2 else: object_shape.dimensions.append(dz) shape_pose.position.z = min_z + dz / 2 shape_pose.orientation.w = 1.0 collision_object.primitives.append(object_shape) collision_object.primitive_poses.append(shape_pose) collision_object.operation = CollisionObject.ADD self.pub_collision.publish(collision_object)
def makeMesh(self, name, pose, filename): scene = pyassimp.load(filename) if not scene.meshes: rospy.logerr('Unable to load mesh') return mesh = Mesh() for face in scene.meshes[0].faces: triangle = MeshTriangle() if len(face.indices) == 3: triangle.vertex_indices = [face.indices[0], face.indices[1], face.indices[2]] mesh.triangles.append(triangle) for vertex in scene.meshes[0].vertices: point = Point() point.x = vertex[0] point.y = vertex[1] point.z = vertex[2] mesh.vertices.append(point) pyassimp.release(scene) o = CollisionObject() o.header.stamp = rospy.Time.now() o.header.frame_id = self._fixed_frame o.id = name o.meshes.append(mesh) o.mesh_poses.append(pose) o.operation = o.ADD return o
def _add_box(self, name, px, py, pz, sl, sw, sh): """ Performs the simple adding of a single box with given dimensions. """ obj = CollisionObject() obj.id = name obj.operation = obj.ADD obj.header.frame_id = "odom_combined" obstacle = SolidPrimitive() obstacle.type = obstacle.BOX obstacle.dimensions = (sl, sw, sh) pos = Pose() pos.position.x = px pos.position.y = py pos.position.z = pz pos.orientation.x = 0 pos.orientation.y = 0 pos.orientation.z = 0 pos.orientation.w = 1 obj.primitives.append(obstacle) obj.primitive_poses.append(pos) self.diff.world.collision_objects.append(obj)
def __make_mesh(self, name, pose, filename): co = CollisionObject() scene = pyassimp.load(filename) if not scene.meshes: raise MoveItCommanderException("There are no meshes in the file") co.operation = CollisionObject.ADD co.id = name co.header = pose.header mesh = Mesh() for face in scene.meshes[0].faces: triangle = MeshTriangle() if len(face.indices) == 3: triangle.vertex_indices = [face.indices[0], face.indices[1], face.indices[2]] mesh.triangles.append(triangle) for vertex in scene.meshes[0].vertices: point = Point() point.x = vertex[0] point.y = vertex[1] point.z = vertex[2] mesh.vertices.append(point) co.meshes = [mesh] co.mesh_poses = [pose.pose] pyassimp.release(scene) return co
def __make_mesh(self, name, pose, filename, scale = (1, 1, 1)): co = CollisionObject() scene = pyassimp.load(filename) if not scene.meshes: raise MoveItCommanderException("There are no meshes in the file") co.operation = CollisionObject.ADD co.id = name co.header = pose.header mesh = Mesh() for face in scene.meshes[0].faces: triangle = MeshTriangle() if len(face.indices) == 3: triangle.vertex_indices = [face.indices[0], face.indices[1], face.indices[2]] mesh.triangles.append(triangle) for vertex in scene.meshes[0].vertices: point = Point() point.x = vertex[0]*scale[0] point.y = vertex[1]*scale[1] point.z = vertex[2]*scale[2] mesh.vertices.append(point) co.meshes = [mesh] co.mesh_poses = [pose.pose] pyassimp.release(scene) return co
def remove_world_object(self, name): """ Remove object from planning scene """ co = CollisionObject() co.operation = CollisionObject.REMOVE co.id = name self._pub_co.publish(co)
def _moveObject(self, objectName, objectPose): co = CollisionObject() co.operation = co.MOVE co.id = objectName co.mesh_poses = [objectPose.pose] co.header.stamp = objectPose.header.stamp co.header.frame_id = objectPose.header.frame_id self._co_publisher.publish(co)
def move_object(self, name, pose): co = CollisionObject() co.operation = CollisionObject.MOVE co.id = name co.header = pose.header co.primitive_poses = [pose.pose] #sphere.dimensions = [radius] # future self.__submit(co, attach=False)
def remove_world_object(self, name = None): """ Remove an object from planning scene, or all if no name is provided """ co = CollisionObject() co.operation = CollisionObject.REMOVE if name != None: co.id = name self._pub_co.publish(co)
def remove_world_object(self, name=None): """ Remove an object from planning scene, or all if no name is provided """ co = CollisionObject() co.operation = CollisionObject.REMOVE if name is not None: co.id = name self.__submit(co, attach=False)
def makeObject(self, shape, pose, solid): obj = CollisionObject() obj.header.stamp = rospy.Time.now() obj.id = shape obj.primitives.append(solid) obj.primitive_poses.append(pose) obj.operation = obj.ADD return obj
def remove_world_object(self, name=None): """ Remove an object from planning scene, or all if no name is provided """ co = CollisionObject() co.operation = CollisionObject.REMOVE if name != None: co.id = name self._pub_co.publish(co)
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 makeSolidPrimitive(self, name, solid, pose): o = CollisionObject() o.header.stamp = rospy.Time.now() o.header.frame_id = self._fixed_frame o.id = name o.primitives.append(solid) o.primitive_poses.append(pose) o.operation = o.ADD return o
def _getCollisionObject(name, urdf, pose, operation): ''' This function takes an urdf and a TF pose and updates the collision geometry associated with the current planning scene. ''' co = CollisionObject() co.id = name co.operation = operation if len(urdf.links) > 1: rospy.logwarn( 'collison object parser does not currently support kinematic chains' ) for l in urdf.links: # Compute the link pose based on the origin if l.origin is None: link_pose = pose else: link_pose = pose * kdl.Frame(kdl.Rotation.RPY(*l.origin.rpy), kdl.Vector(*l.origin.xyz)) for c in l.collisions: # Only update the geometry if we actually need to add the # object to the collision scene. # check type of each collision tag. if isinstance(c.geometry, urdf_parser_py.urdf.Box): primitive = True if co.operation == CollisionObject.ADD: size = c.geometry.size element = SolidPrimitive() element.type = SolidPrimitive.BOX element.dimensions = list(c.geometry.size) co.primitives.append(element) elif isinstance(c.geometry, urdf_parser_py.urdf.Mesh): primitive = False if co.operation == CollisionObject.ADD: scale = (1, 1, 1) if c.geometry.scale is not None: scale = c.scale element = _loadMesh(c.geometry, scale) co.meshes.append(element) else: raise NotImplementedError( "we do not currently support geometry of type %s" % (str(type(c.geometry)))) pose = kdl.Frame(kdl.Rotation.RPY(*c.origin.rpy), kdl.Vector(*c.origin.xyz)) pose = link_pose * pose if primitive: co.primitive_poses.append(pm.toMsg(pose)) else: # was a mesh co.mesh_poses.append(pm.toMsg(pose)) return co
def __make_mesh_custom(self, name, pose, mesh): co = CollisionObject() co.operation = CollisionObject.ADD co.id = name co.header = pose.header co.meshes = [mesh] co.mesh_poses = [pose.pose] return co
def __init__(self, speed): self.speed = speed # Initialize moveit_commander moveit_commander.roscpp_initialize(sys.argv) # Initialize the robot self.robot = moveit_commander.RobotCommander() # Initialize the planning scene self.scene = moveit_commander.PlanningSceneInterface() self.scene_publisher = rospy.Publisher('/collision_object', CollisionObject, queue_size=10) # Instantiate a move group self.group = moveit_commander.MoveGroupCommander(GROUP_NAME) # Set the maximum time MoveIt will try to plan before giving up self.group.set_planning_time(.25) # Set maximum velocity scaling self.group.set_max_velocity_scaling_factor(1.0) self.group.set_max_acceleration_scaling_factor(1.0) # For displaying plans to RVIZ self.display_pub = rospy.Publisher('/move_group/display_planned_path', DisplayTrajectory, queue_size=10) print(self.group.get_current_pose()) print(self.group.get_end_effector_link()) print(self.group.get_pose_reference_frame()) print(self.group.get_goal_tolerance()) # Create a CollisionObject, which will be added to the planning scene co = CollisionObject() co.operation = CollisionObject.ADD co.id = 'table' co.header = TABLE_CENTER.header # Create a box primitive, which will be inside the CollisionObject box = SolidPrimitive() box.type = SolidPrimitive.BOX box.dimensions = TABLE_SIZE # Fill the collision object with primitive(s) co.primitives = [box] co.primitive_poses = [TABLE_CENTER.pose] # Publish the object (don't know why but need to publish twice) self.scene_publisher.publish(co) rospy.sleep(0.5) self.scene_publisher.publish(co) rospy.sleep(0.5)
def __make_cylinder(name, pose, height, radius): co = CollisionObject() co.operation = CollisionObject.ADD co.id = name co.header = pose.header cylinder = SolidPrimitive() cylinder.type = SolidPrimitive.CYLINDER cylinder.dimensions = [height, radius] co.primitives = [cylinder] co.primitive_poses = [pose.pose] return co
def __make_box(self, name, pose, size): co = CollisionObject() co.operation = CollisionObject.ADD co.id = name co.header = pose.header box = SolidPrimitive() box.type = SolidPrimitive.BOX box.dimensions = list(size) co.primitives = [box] co.primitive_poses = [pose.pose] return co
def __make_sphere(self, name, pose, radius): co = CollisionObject() co.operation = CollisionObject.ADD co.id = name co.header = pose.header sphere = SolidPrimitive() sphere.type = SolidPrimitive.SPHERE sphere.dimensions = [radius] co.primitives = [sphere] co.primitive_poses = [pose.pose] return co
def convert_to_collision_object(link, full_linkname): collision_object = link_to_collision_object(link, full_linkname) if not collision_object: return link_root = get_root_collision_model(link).root_link.name if link_root not in collision_objects: collision_objects[link_root] = CollisionObject() collision_objects[link_root].id = link_root collision_objects[link_root].operation = CollisionObject.ADD append_to_collision_object(collision_objects[link_root], collision_object)
def __make_cylinder(self, name, pose, size): co = CollisionObject() co.operation = CollisionObject.ADD co.id = name co.header = pose.header cyl = SolidPrimitive() cyl.type = SolidPrimitive.CYLINDER cyl.dimensions = list(size) co.primitives = [cyl] co.primitive_poses = [pose.pose] return co
def find_block(self): """ Find block and add it to the scene """ # Get necessary values while not self.object_location: print "Waiting for object location..." rospy.sleep(1.0) object_location = self.object_location image_size = self.left_camera_size while not self.current_pose: print "Waiting for current pose values..." rospy.sleep(1.0) current_pose = self.current_pose.position endeffector_height = current_pose.z - self.table_height # Store values for calculation Pp = [object_location.x, object_location.y, object_location.z] Cp = [image_size[0] / 2, image_size[1] / 2, 0] Bp = [current_pose.x, current_pose.y, current_pose.z] Go = [-0.0230, 0.0110, 0.1100] cc = [0.0021, 0.0021, 0.0000] d = [endeffector_height, endeffector_height, 0.0000] # Calculate block's position in workspace # workspace = (Pp - Cp) * cc * d + Bp + Go pixel_difference = map(operator.sub, Pp, Cp) camera_constant = map(operator.mul, cc, d) pixel_2_real = map(operator.mul, pixel_difference, camera_constant) pixel_2_real[2] = endeffector_height * -1 workspace_without_gripper = map(operator.add, pixel_2_real, Bp) workspace = map(operator.add, workspace_without_gripper, Go) # Add block to scene p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.pose.position.x = workspace_without_gripper[0] p.pose.position.y = workspace_without_gripper[1] p.pose.position.z = workspace_without_gripper[2] + 0.06 co = CollisionObject() co.operation = CollisionObject.ADD co.id = "block" co.header = p.header box = SolidPrimitive() box.type = SolidPrimitive.BOX box.dimensions = list((0.1, 0.1, 0.1)) co.primitives = [box] co.primitive_poses = [p.pose] pub_co = rospy.Publisher("collision_object", CollisionObject, latch=True) pub_co.publish(co) return
def add_plane(self, name, pose, normal = (0, 0, 1), offset = 0): """ Add a plane to the planning scene """ co = CollisionObject() co.operation = CollisionObject.ADD co.id = name co.header = pose.header p = Plane() p.coef = list(normal) p.coef.append(offset) co.planes = [p] co.plane_poses = [pose.pose] self._pub_co.publish(co)
def removeCollisionObject(self, name, use_service=True): """ Remove an object. """ o = CollisionObject() o.header.stamp = rospy.Time.now() o.header.frame_id = self._fixed_frame o.id = name o.operation = o.REMOVE try: del self._objects[name] self._removed[name] = o except KeyError: pass self.sendUpdate(o, None, use_service)
def attach_object(self, group_name): rospy.loginfo('attaching object to ' + group_name) # select toolframe depending on group name tool_frame = TOOL_FRAME_RIGHT if 'right' in group_name else TOOL_FRAME_LEFT # pose for attached object in tool frame coordiantes pose = Pose() pose.position.z = 0.05 pose.orientation.w = 1.0 primitive = SolidPrimitive() primitive.type = primitive.BOX primitive.dimensions = [0.07, 0.07, 0.07] o = CollisionObject() o.header.frame_id = tool_frame o.id = "handed_object" o.primitives = [primitive] o.primitive_poses = [pose] o.operation = o.ADD a = AttachedCollisionObject() a.object = o # allow collisions with hand links a.touch_links = ALLOWED_TOUCH_LINKS # attach object to tool frame a.link_name = tool_frame # don't delete old planning scene, if we didn't get one so far, create a new one scene = self.current_planning_scene if self.current_planning_scene is not None else PlanningScene() # add attached object to scene scene.robot_state.attached_collision_objects.append(a) # mark scene as changed scene.is_diff = True self.pub_ps.publish(scene)
def removeCollisionObject(self, name, wait=True): """ Remove an object. """ o = CollisionObject() o.header.stamp = rospy.Time.now() o.header.frame_id = self._fixed_frame o.id = name o.operation = o.REMOVE try: del self._objects[name] self._removed[name] = o except KeyError: pass self._pub.publish(o) if wait: self.waitForSync()
def insert_object(self, pose, dims, name): """ Expects a PoseStamped to be input, along with a Point and string """ primitive = SolidPrimitive() primitive.type = 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 primitive.dimensions = [dims.x, dims.y, dims.z] add_object = CollisionObject() add_object.id = name add_object.header.frame_id = pose.header.frame_id add_object.operation = add_object.ADD add_object.primitives.append(primitive) add_object.primitive_poses.append(pose.pose) self.collision_pub.publish(add_object)
def __make_mesh(name, pose, filename, scale=(1, 1, 1)): co = CollisionObject() if pyassimp is False: raise MoveItCommanderException( "Pyassimp needs patch https://launchpadlibrarian.net/319496602/patchPyassim.txt") scene = pyassimp.load(filename) if not scene.meshes or len(scene.meshes) == 0: raise MoveItCommanderException("There are no meshes in the file") if len(scene.meshes[0].faces) == 0: raise MoveItCommanderException("There are no faces in the mesh") co.operation = CollisionObject.ADD co.id = name co.header = pose.header mesh = Mesh() first_face = scene.meshes[0].faces[0] if hasattr(first_face, '__len__'): for face in scene.meshes[0].faces: if len(face) == 3: triangle = MeshTriangle() triangle.vertex_indices = [face[0], face[1], face[2]] mesh.triangles.append(triangle) elif hasattr(first_face, 'indices'): for face in scene.meshes[0].faces: if len(face.indices) == 3: triangle = MeshTriangle() triangle.vertex_indices = [face.indices[0], face.indices[1], face.indices[2]] mesh.triangles.append(triangle) else: raise MoveItCommanderException("Unable to build triangles from mesh due to mesh object structure") for vertex in scene.meshes[0].vertices: point = Point() point.x = vertex[0] * scale[0] point.y = vertex[1] * scale[1] point.z = vertex[2] * scale[2] mesh.vertices.append(point) co.meshes = [mesh] co.mesh_poses = [pose.pose] pyassimp.release(scene) return co
def execute(self, ud): rospy.logdebug("Entered 'GRASP_ARM_PLAN' state.") collision_object = CollisionObject() collision_object.header.frame_id = 'odom' collision_object.id = 'coffee' primitive = SolidPrimitive() primitive.type = primitive.CYLINDER primitive.dimensions.append(0.15) primitive.dimensions.append(0.06) collision_object.primitives.append(primitive) collision_object.operation = collision_object.ADD ta = PoseStamped() ta.header.frame_id = 'odom' ta.header.stamp = rospy.Time.now() ta.pose.position.x = 0.843708 ta.pose.position.y = -0.0977909 ta.pose.position.z = 1.10764 ta.pose.orientation.x = -0.126284 ta.pose.orientation.y = -0.368197 ta.pose.orientation.z = -0.525026 ta.pose.orientation.w = 0.756856 collision_object.primitive_poses.append(ud.grasp_target_pose.pose) try: res = self.make_plan_srv(targetPose=ud.grasp_target_pose, jointPos=[], planningSpace=computePlanRequest.CARTESIAN_SPACE, collisionObject=collision_object) except rospy.ServiceException: rospy.logerr("Failed to connect to the planning service.") return 'grasp_arm_error' if res.planningResult == computePlanResponse.PLANNING_FAILURE: rospy.logwarn("Arm planning failed. Moving the base is probably required to reach target pose.") return 'grasp_arm_plan_failed' else: ud.grasp_arm_plan = res.trajectory rospy.sleep(rospy.Duration(10)) return 'grasp_arm_plan_succeeded'
def __init__(self): """ Initialize class """ # Overall MoveIt initialization moveit_commander.roscpp_initialize(sys.argv) self.robot = moveit_commander.RobotCommander() # MoveIt scene initialization self.scene = moveit_commander.PlanningSceneInterface() self.table_height = -0.320 p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.pose.position.x = 0.8 p.pose.position.y = 0.025 p.pose.position.z = -0.6 co = CollisionObject() co.operation = CollisionObject.ADD co.id = "table" co.header = p.header box = SolidPrimitive() box.type = SolidPrimitive.BOX box.dimensions = list((0.75, 1.25, 0.55)) co.primitives = [box] co.primitive_poses = [p.pose] pub_co = rospy.Publisher("collision_object", CollisionObject, latch=True) pub_co.publish(co) # Moveit group initialization self.group = moveit_commander.MoveGroupCommander("left_arm") self.group.set_goal_position_tolerance(0.12) self.group.set_goal_orientation_tolerance(0.10) # Gripper initialization self.left_gripper = baxter_interface.Gripper("left", CHECK_VERSION) self.left_gripper.reboot() self.left_gripper.calibrate() # Camera initialization self.left_camera = baxter_interface.CameraController("left_hand_camera") self.right_camera = baxter_interface.CameraController("right_hand_camera") self.head_camera = baxter_interface.CameraController("head_camera") self.right_camera.close() self.head_camera.close() self.left_camera.open() self.left_camera.resolution = [1280, 800] self.left_camera_size = [800, 1280] _camera_sub = rospy.Subscriber("/cameras/left_hand_camera/image", Image, self._on_camera) # Open CV initialization cv2.namedWindow("Original", cv2.WINDOW_NORMAL) cv2.moveWindow("Original", 1990, 30) cv2.namedWindow("Thresholded", cv2.WINDOW_NORMAL) cv2.moveWindow("Thresholded", 3270, 30) self.low_h = 165 self.high_h = 179 self.low_s = 70 self.high_s = 255 self.low_v = 60 self.high_v = 255 self.bridge = CvBridge() # Object location initialization self.object_location = None # Subscribe to pose at all times self.current_pose = None _pose_sub = rospy.Subscriber("/robot/limb/left/endpoint_state", EndpointState, self.set_current_pose) return
def hand_over(): rospy.init_node('moveit_node') listener = tf.TransformListener() #listener.waitForTransform('left_hand_camera', 'base') #Initialize moveit_commander moveit_commander.roscpp_initialize(sys.argv) #Start a node #rospy.init_node('moveit_node') #Initialize both arms robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() left_arm = moveit_commander.MoveGroupCommander('left_arm') #right_arm = moveit_commander.MoveGroupCommander('right_arm') left_arm.set_planner_id('RRTConnectkConfigDefault') left_arm.set_planning_time(10) #right_arm.set_planner_id('RRTConnectkConfigDefault') #right_arm.set_planning_time(10) # Add a collison object # Build the attached object shape = SolidPrimitive() shape.type = SolidPrimitive.BOX #shape.dimensions.resize(3) # print(shape.dimensions) shape.dimensions.append(0.6) shape.dimensions.append(1.5) shape.dimensions.append(0.6) # print(shape) # Create pose obj_pose = Pose() obj_pose.position.x = 0.665 obj_pose.position.y = 0.062 obj_pose.position.z = -0.7 obj_pose.orientation.x = 0 obj_pose.orientation.y = 0 obj_pose.orientation.z = 0 obj_pose.orientation.w = 1 # create collision object cobj = CollisionObject() cobj.primitives = shape cobj.primitive_poses=obj_pose cobj.header.frame_id='base' cobj.id = 'Table' # create collision object message # ATTACHED_COLLISION_OBJECT.link_name = TCP_LINK_NAME # ATTACHED_COLLISION_OBJECT.object = cobj # ATTACHED_COLLISION_OBJECT.object.header.frame_id = TCP_LINK_NAME # ATTACHED_COLLISION_OBJECT.touch_links.push_back("gripper_body") # Attache it to the scene cobj.operation = CollisionObject.ADD attach_object_publisher = rospy.Publisher('collision_object',CollisionObject,queue_size = 10) attach_object_publisher.publish(cobj) rospy.sleep(1) #Set up the left gripper left_gripper = baxter_gripper.Gripper('left') input=1 pile_1=[] pile_2=[] pile_3=[] while input: #Original pose ------------------------------------------------------ original = PoseStamped() original.header.frame_id = "base" #the pose the baxter will first turn to original.pose.position.x = 0.505 original.pose.position.y = 0.487 original.pose.position.z = 0.515 original.pose.orientation.x = 0.505 original.pose.orientation.y = 0.487 original.pose.orientation.z = 0.515 original.pose.orientation.w = -0.493 left_arm.set_pose_target(original) #Set the start state for the left arm left_arm.set_start_state_to_current_state() left_plan = left_arm.plan() raw_input('Press <Enter> to move the left arm to the original pose: ') left_arm.execute(left_plan) rospy.sleep(2) # First goal pose to pick up ---------------------------------------------- goal_1 = PoseStamped() goal_1.header.frame_id = "base" while not rospy.is_shutdown(): try: marker = rospy.wait_for_message('ar_pose_marker',AlvarMarkers) #rospy.sleep(1) #print(marker) ar_name = marker.markers[0].id arTagName = 'ar_marker_' + str(ar_name) print(arTagName) break except: continue #arTagName = 'ar_marker_' + str(ar_name) # Set up your subscriber and define its callback image_topic = "/cameras/left_hand_camera/image" my_image_msg = rospy.wait_for_message(image_topic, Image) print("Received an image!") # Convert your ROS Image message to OpenCV2 cv2_img = bridge.imgmsg_to_cv2(my_image_msg, "bgr8") # Save your OpenCV2 image as a jpeg cv2.imwrite('camera_image'+str(ar_name)+'.png', cv2_img) # Carry out the digit recognition alg # zipcode = digitRecog('camera_image'+str(ar_name)+'.png') # Display the original image on Baxter's screen display_pub = rospy.Publisher('/robot/xdisplay',Image, latch=True) display_pub.publish(my_image_msg) rospy.sleep(1) while not rospy.is_shutdown(): try: (trans_marker_cam, rot_marker_cam) = listener.lookupTransform(arTagName,'left_hand_camera', rospy.Time()) (trans_cam_hand, rot_cam_hand) = listener.lookupTransform('left_hand_camera', 'left_hand', rospy.Time()) (trans_hand_base, rot_hand_base) = listener.lookupTransform('left_hand', 'base', rospy.Time()) break except: continue # Find the transform between the AR tag and the base g_marker_cam = quat_to_rbt(trans_marker_cam, rot_marker_cam) g_cam_hand = quat_to_rbt(trans_cam_hand, rot_cam_hand) g_hand_base = quat_to_rbt(trans_hand_base, rot_hand_base) g_final = linalg.inv(np.dot(np.dot(g_marker_cam, g_cam_hand), g_hand_base)) # print('final') # print(g_final) #x, y, and z position x_pos = "%.3f" % (g_final[0,3]) y_pos = "%.3f" % (g_final[1,3]) z_pos = "%.3f" % (g_final[2,3]) # print(x_pos) # print(y_pos) # print(z_pos) goal_1.pose.position.x = float(x_pos) goal_1.pose.position.y = float(y_pos) - 0.2 goal_1.pose.position.z = float(z_pos) #Orientation as a quaternion goal_1.pose.orientation.x = -0.689 goal_1.pose.orientation.y = -0.013 goal_1.pose.orientation.z = -0.035 goal_1.pose.orientation.w = 0.723 #Set the goal state to the pose you just defined left_arm.set_pose_target(goal_1) #Set the start state for the left arm left_arm.set_start_state_to_current_state() #Plan a path left_plan = left_arm.plan() #Execute the plan raw_input('Press <Enter> to move the left arm to goal pose 1 (path constraints are never enforced during this motion): ') left_arm.execute(left_plan) rospy.Subscriber('/robot/range/left_hand_range/state',Range, callback) rospy.sleep(1) # print(mail_range) #Check using IR to see of the mail is close enough incre = 0.01 while mail_range > 0.06 and incre < 0.05: print('Too far from the mail. Trying again...') rospy.Subscriber('/robot/range/left_hand_range/state',Range, callback) rospy.sleep(1) print(mail_range) goal_1.pose.position.y = float(y_pos) - 0.1 + incre incre += 0.01 #Set the goal state to the pose you just defined left_arm.set_pose_target(goal_1) #Set the start state for the left arm left_arm.set_start_state_to_current_state() #Plan a path left_plan = left_arm.plan() #Execute the plan raw_input('Press <Enter> to move the left arm to get the mail: ') left_arm.execute(left_plan) # print(mail_range) #Close the Gripper raw_input('Press <Enter> to close the gripper') left_gripper.close(timeout=60) print('Gripper closed') #TODO- generate the recognized image and publish it # Check http://sdk.rethinkrobotics.com/wiki/Display_Image_-_Code_Walkthrough # display_pub.sleep() img = cv2.imread('withRecog'+str(ar_name)+'.png') msg = CvBridge().cv2_to_imgmsg(img, encoding="bgr8") rec_pub = rospy.Publisher('/robot/xdisplay', Image, latch=True) rec_pub.publish(msg) # Sleep to allow for image to be published. rospy.sleep(1) #Second goal pose ----------------------------------------------------- rospy.sleep(2.0) goal_2 = PoseStamped() goal_2.header.frame_id = "base" if ar_name in (1, 3, 12): #x, y, and z position goal_2.pose.position.x = 0.7 goal_2.pose.position.y = 0.6 goal_2.pose.position.z = -0.18 pile_1.insert(0,ar_name) elif ar_name in (0,9,15,16): #x, y, and z position goal_2.pose.position.x = 0.7 goal_2.pose.position.y = 0.3 goal_2.pose.position.z = -0.18 pile_2.insert(0,ar_name) else: #x, y, and z position goal_2.pose.position.x = 0.7 goal_2.pose.position.y = 0 goal_2.pose.position.z = -0.18 pile_3.insert(0,ar_name) print('pile_1') print(pile_1) print('pile_2') print(pile_2) print('pile_3') print(pile_3) #Orientation as a quaternion goal_2.pose.orientation.x = 0.0 goal_2.pose.orientation.y = -1.0 goal_2.pose.orientation.z = 0.0 goal_2.pose.orientation.w = 0.0 #Execute the plan #raw_input('Press <Enter> to move the left arm to the zipcode region: ') #Set the goal state to the pose you just defined left_arm.set_pose_target(goal_2) #Set the start state for the left arm left_arm.set_start_state_to_current_state() #Plan a path left_plan = left_arm.plan() #Execute the plan raw_input('Press <Enter> to move the left arm to the zipcode region: ') left_arm.execute(left_plan) #Release the Gripper raw_input('Press <Enter> to open the gripper') left_gripper.open(block=True) print('Gripper opened') #reg_pub.sleep() #Continue or stop input=int(raw_input('Press 1 to start/continue moving, or 0 to stop moving ')) return (pile_1,pile_2,pile_3)
pub = rospy.Publisher("obstacle_distance/registerObstacle", CollisionObject, queue_size = 1) while pub.get_num_connections() < 1: if rospy.is_shutdown(): exit(0) rospy.logwarn("Please create a subscriber to 'obstacle_distance/registerObstacle' topic (Type: moveit_msgs/CollisionObject)") time.sleep(1.0) rospy.loginfo("Continue ...") # Publish a simple sphere x = CollisionObject() x.id = "Funny Sphere" x.header.frame_id = root_frame x.operation = CollisionObject.ADD #x.operation = CollisionObject.REMOVE sphere = SolidPrimitive() sphere.type = SolidPrimitive.SPHERE sphere.dimensions.append(0.1) # radius x.primitives.append(sphere) pose = Pose() pose.position.x = 0.35 pose.position.y = -0.45 pose.position.z = 0.8 pose.orientation.x = 0.0; pose.orientation.y = 0.0; pose.orientation.z = 0.0; pose.orientation.w = 1.0; x.primitive_poses.append(pose)
def init_cluster_grasper(self, cluster, probabilities=[], use_probability=True): self.cluster_frame = cluster.header.frame_id #use PCA to find the object frame and bounding box dims, and to get the cluster points in the object frame (z=0 at bottom of cluster) if use_probability: if len(probabilities) == 0: (self.object_points, self.object_bounding_box_dims, self.object_bounding_box, \ self.object_to_base_frame, self.object_to_cluster_frame) = \ self.cbbf.find_object_frame_and_bounding_box(cluster, []) #empty probabilities if self.draw_box: self.draw_bounding_box(self.object_bounding_box_dims, name='probabilistic_object_box', color=[0.2,0.2,1]) print 'probabilistic_object_box' else: (self.object_points, self.object_bounding_box_dims, self.object_bounding_box, \ self.object_to_base_frame, self.object_to_cluster_frame) = \ self.cbbf.find_object_frame_and_bounding_box(cluster, probabilities) #draw the bounding box if self.draw_box: self.draw_bounding_box(self.object_bounding_box_dims, name='probabilistic_weighted_object_box', color=[0,0.9,0.9]) print 'probabilistic_weighted_object_box' else: (self.object_points, self.object_bounding_box_dims, self.object_bounding_box, \ self.object_to_base_frame, self.object_to_cluster_frame) = \ self.cbbf.find_object_frame_and_bounding_box(cluster, probabilities) if self.draw_box: self.draw_bounding_box(self.object_bounding_box_dims, name='deterministic_object_box', color=[1,0,0]) print 'deterministic_object_box' # MoveIt Stuff: CollisionObject ''' print "self.object_bounding_box_dims=", self.object_bounding_box_dims print "self.object_bounding_box=", self.object_bounding_box print "self.object_to_base_frame.shape=", self.object_to_base_frame.shape print "self.object_to_base_frame=", self.object_to_base_frame print "self.object_to_cluster_frame=", self.object_to_cluster_frame ''' # Add the bounding box as a CollisionObject co = CollisionObject() co.header.stamp = rospy.get_rostime() #co.header.frame_id = "base_footprint" co.header.frame_id = "base_link" co.primitives.append(SolidPrimitive()) co.primitives[0].type = SolidPrimitive.BOX # Clear the previous CollisionObject co.id = "part" co.operation = co.REMOVE self.pub_co.publish(co) # Clear the previously attached object aco = AttachedCollisionObject() aco.object = co self.pub_aco.publish(aco) # Add the new CollisionObject box_height = self.object_bounding_box_dims[2] co.operation = co.ADD co.primitives[0].dimensions.append(self.object_bounding_box_dims[0]) co.primitives[0].dimensions.append(self.object_bounding_box_dims[1]) co.primitives[0].dimensions.append(self.object_bounding_box_dims[2]) co.primitive_poses.append(Pose()) co.primitive_poses[0].position.x = self.object_to_base_frame[0,3] co.primitive_poses[0].position.y = self.object_to_base_frame[1,3] co.primitive_poses[0].position.z = self.object_to_base_frame[2,3] + box_height/2 quat = tf.transformations.quaternion_about_axis(math.atan2(self.object_to_base_frame[1,0], self.object_to_base_frame[0,0]), (0,0,1)) #quat = tf.transformations.quaternion_from_matrix(self.object_to_base_frame) co.primitive_poses[0].orientation.x = quat[0] co.primitive_poses[0].orientation.y = quat[1] co.primitive_poses[0].orientation.z = quat[2] co.primitive_poses[0].orientation.w = quat[3] self.pub_co.publish(co) # END MoveIt! stuff #for which directions does the bounding box fit within the hand? gripper_space = [self.gripper_opening - self.object_bounding_box_dims[i] for i in range(3)] self._box_fits_in_hand = [gripper_space[i] > 0 for i in range(3)] #only half benefit for the longer dimension, if one is significantly longer than the other if self._box_fits_in_hand[0] and self._box_fits_in_hand[1]: if gripper_space[0] > gripper_space[1] and self.object_bounding_box_dims[0]/self.object_bounding_box_dims[1] < .8: self._box_fits_in_hand[1] *= .5 elif gripper_space[1] > gripper_space[0] and self.object_bounding_box_dims[1]/self.object_bounding_box_dims[0] < .8: self._box_fits_in_hand[0] *= .5 #rospy.loginfo("bounding box dims: "+pplist(self.object_bounding_box_dims)) #rospy.loginfo("self._box_fits_in_hand: "+str(self._box_fits_in_hand)) #compute the average number of points per sq cm of bounding box surface (half the surface only) bounding_box_surf = (self.object_bounding_box_dims[0]*100 * self.object_bounding_box_dims[1]*100) + \ (self.object_bounding_box_dims[0]*100 * self.object_bounding_box_dims[2]*100) + \ (self.object_bounding_box_dims[1]*100 * self.object_bounding_box_dims[2]*100) self._points_per_sq_cm = self.object_points.shape[1] / bounding_box_surf