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 graspThis(self, object_name): target = CollisionObject() target.id = str(self.ez_objects[object_name][0]) target.primitive_poses = [self.ez_objects[object_name][1].pose] response = self.planning_srv(group_name=self.gripper_name, target=target) return response.grasps
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 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 pub_obj(self, obj_type, pos, dim): m = Marker() m.id = self.obj_types.index(obj_type) m.ns = "simple_tabletop" m.type = Marker.CUBE m.pose = Pose(Point(*pos), Quaternion(0, 0, 0, 1)) m.header = Header(0, rospy.Time(0), "base_link") m.scale = Vector3(*dim) m.color = self.colors[obj_type] mtext = deepcopy(m) mtext.type = Marker.TEXT_VIEW_FACING mtext.id += 100 mtext.text = obj_type M = MarkerArray([m, mtext]) self.pub.publish(M) #moveit collision objects co = CollisionObject() co.header = deepcopy(m.header) co.id = obj_type obj_shape = SolidPrimitive() obj_shape.type = SolidPrimitive.BOX obj_shape.dimensions = list(dim) co.primitives.append(obj_shape) obj_pose = Pose(Point(*pos), Quaternion(0, 0, 0, 1)) co.primitive_poses = [obj_pose] self.pub_co.publish(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 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 __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 create_sphere(name, pose, radius, frame_id='/world_frame'): co = CollisionObject() co.operation = CollisionObject.ADD co.id = name co.header.frame_id = frame_id sphere = SolidPrimitive() sphere.type = SolidPrimitive.SPHERE sphere.dimensions = [radius] co.primitives = [sphere] co.primitive_poses = [pose] return co
def create_box(name, pose, size, frame_id='/world_frame'): co = CollisionObject() co.operation = CollisionObject.ADD co.id = name co.header.frame_id = frame_id box = SolidPrimitive() box.type = SolidPrimitive.BOX box.dimensions = list(size) co.primitives = [box] co.primitive_poses = [pose] return co
def _move_command(self, name, prim_poses=[], mesh_poses=[], frame_id=None): co = CollisionObject() co.header.stamp = rospy.Time.now() if frame_id is not None: co.header.frame_id = frame_id else: co.header.frame_id = self._planning_frame co.id = name co.primitive_poses = prim_poses co.mesh_poses = mesh_poses co.operation = CollisionObject.MOVE return co
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 update_collision_box(id, position, orientation=(1, 0, 0, 0), dimensions=(1, 1, 1), operation=CollisionObject.ADD): pose = make_pose(position, orientation, robot.get_planning_frame()) co = CollisionObject() co.operation = operation co.id = id co.header = pose.header box = SolidPrimitive() box.type = SolidPrimitive.BOX box.dimensions = dimensions co.primitives = [box] co.primitive_poses = [pose.pose] planning_scene_publisher.publish(co)
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 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 add_cylinder(self, object_id, frame, size, pose=None, position=None, orientation=None, rpy=None, color=None): try: stamped_pose = self.make_stamped_pose(frame=frame, pose=pose, position=position, orientation=orientation, rpy=rpy) # Cylinders are special - they are not supported directly by # moveit_commander. It must be published manually to collision scene cyl = CollisionObject() cyl.operation = CollisionObject.ADD cyl.id = object_id cyl.header = stamped_pose.header prim = SolidPrimitive() prim.type = SolidPrimitive.CYLINDER prim.dimensions = [size[0], size[1]] cyl.primitives = [prim] cyl.primitive_poses = [stamped_pose.pose] # self.scene_pub.publish(cyl) marker = self.make_marker_stub(stamped_pose, [size[1] * 2, size[2] * 2, size[0]], color=color) marker.type = Marker.CYLINDER self.publish_marker(object_id, marker) sleep(0.5) logdebug("Loaded " + object_id + " as cylindrical collision object.") except ROSException as e: loginfo("Problem loading " + object_id + " collision object: " + str(e))
def add_obstacle(position, operation=CollisionObject.ADD): planning_scene_publisher = rospy.Publisher('/collision_object', CollisionObject, queue_size=10) scene = moveit_commander.PlanningSceneInterface() robot = moveit_commander.RobotCommander() rospy.sleep(2) pose = make_pose(position, [1, 0, 0, 0], robot.get_planning_frame()) co = CollisionObject() co.operation = operation co.id = "obstacle" co.header = pose.header box = SolidPrimitive() box.type = SolidPrimitive.BOX box.dimensions = (0.5, .7, 0.1) co.primitives = [box] co.primitive_poses = [pose.pose] planning_scene_publisher.publish(co)
def attach_cylinder(link, name, pose, height, radius, touch_links=[]): aco = AttachedCollisionObject() 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] aco.object = co aco.link_name = link if len(touch_links) > 0: aco.touch_links = touch_links else: aco.touch_links = [link] scene._pub_aco.publish(aco)
def attach_sphere(link, name, pose, radius, touch_links=[]): aco = AttachedCollisionObject() 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] aco.object = co aco.link_name = link if len(touch_links) > 0: aco.touch_links = touch_links else: aco.touch_links = [link] scene._pub_aco.publish(aco)
def add_arbitrary_obstacle(size, id, pose, planning_scene_publisher, scene, robot, operation): """ Adds an arbitrary rectangular prism obstacle to the planning scene. This is currently only for the ground plane size: numpy array of size 3 (x,y,z dimensions) id: string (object id/name) pose: PoseStamped objecct (objects pose with respect to world frame) planning_scene_publisher: ros Publisher('/collision_object', CollisionObject) scene: PlanningSceneInterface robot: RobotCommander operation: currently just use CollisionObject.ADD """ co = CollisionObject() co.operation = operation co.id = id co.header = pose.header box = SolidPrimitive() box.type = SolidPrimitive.BOX box.dimensions = size co.primitives = [box] co.primitive_poses = [pose.pose] planning_scene_publisher.publish(co)
def add_bin(bin, prefix="/shelf"): # Necessary parameters _, max_x, min_y, max_y, min_z, max_z = rospy.get_param(prefix + "/bins/" + bin) shelf_max_x = DEPTH / 2 shelf_min_y, shelf_max_y = -WIDTH / 2, WIDTH / 2 shelf_min_z, shelf_max_z = 0, HEIGHT objects = [] poses = [] # Create left side objects.append( SolidPrimitive( type=SolidPrimitive.BOX, dimensions=[DEPTH, abs(shelf_min_y - min_y), HEIGHT], )) poses.append( Pose( position=Point(x=0, y=(shelf_min_y + min_y) / 2, z=HEIGHT / 2), orientation=Quaternion(x=0, y=0, z=0, w=1), )) # Create right side objects.append( SolidPrimitive( type=SolidPrimitive.BOX, dimensions=[DEPTH, abs(shelf_max_y - max_y), HEIGHT], )) poses.append( Pose( position=Point(x=0, y=(shelf_max_y + max_y) / 2, z=HEIGHT / 2), orientation=Quaternion(x=0, y=0, z=0, w=1), )) # Create top objects.append( SolidPrimitive( type=SolidPrimitive.BOX, dimensions=[DEPTH, abs(max_y - min_y), abs(shelf_max_z - max_z)], )) poses.append( Pose( position=Point(x=0, y=(max_y + min_y) / 2, z=(shelf_max_z + max_z) / 2), orientation=Quaternion(x=0, y=0, z=0, w=1), )) # Create bottom objects.append( SolidPrimitive( type=SolidPrimitive.BOX, dimensions=[DEPTH, abs(max_y - min_y), abs(shelf_min_z - min_z)], )) poses.append( Pose( position=Point(x=0, y=(max_y + min_y) / 2, z=(shelf_min_z + min_z) / 2), orientation=Quaternion(x=0, y=0, z=0, w=1), )) # Create back objects.append( SolidPrimitive( type=SolidPrimitive.BOX, dimensions=[DEPTH / 2, abs(max_y - min_y), abs(max_z - min_z)], )) poses.append( Pose( position=Point(x=shelf_max_x / 2, y=(max_y + min_y) / 2, z=(min_z + max_z) / 2), orientation=Quaternion(x=0, y=0, z=0, w=1), )) co = CollisionObject() co.operation = CollisionObject.ADD co.id = "shelf" co.header.frame_id = "/shelf" co.header.stamp = rospy.Time.now() co.primitives = objects co.primitive_poses = poses scene._pub_co.publish(co)
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)
handle_pose.position.z = 1.22 handle_pose.orientation.x = 0 handle_pose.orientation.y = 0 handle_pose.orientation.z = 0 handle_pose.orientation.w = 1 surface_pose = deepcopy(handle_pose) surface_pose.position.x += 0.25 surface_pose.position.z += 0.11 handle_size = [0.02, 0.155, 0.16] surface_size = [0.48, 0.23, 0.08] co = CollisionObject() co.operation = CollisionObject.ADD co.id = "Scoop" co.header.frame_id = "/base_link" co.header.stamp = rospy.Time.now() handle = SolidPrimitive() handle.type = SolidPrimitive.BOX handle.dimensions = handle_size surface = SolidPrimitive() surface.type = SolidPrimitive.BOX surface.dimensions = surface_size co.primitives = [handle, surface] co.primitive_poses = [handle_pose, surface_pose] scene._pub_co.publish(co) print "Published scoop" # rospy.spin() moveit_commander.roscpp_shutdown()
TABLE_CENTER.pose = Pose(position=Point(0, -1.37, .76)) TABLE_CENTER.pose.orientation.w = 1 TABLE_SIZE = [1.525, 2.74, 0.0201] # 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] print(co) count = 0 while not rospy.is_shutdown(): pub.publish(co) co.primitive_poses[0].position.x += 1 count += 1 print(count) rospy.sleep(0.5)
def add_order_bin(x, y): # Necessary parameters bin_width, bin_depth, bin_height = 0.65, 0.4, 0.43 interior_width, interior_depth, interior_height = 0.45, 0.29, 0.18 wall_width, wall_depth = (bin_width - interior_width) / 2, ( bin_depth - interior_depth) / 2 base_height = bin_height - interior_height objects = [] poses = [] # Create Bottom objects.append( SolidPrimitive( type=SolidPrimitive.BOX, dimensions=[bin_depth, bin_width, base_height], )) poses.append( Pose( position=Point(x=x, y=y, z=base_height / 2), orientation=Quaternion(x=0, y=0, z=0, w=1), )) # Create left side objects.append( SolidPrimitive( type=SolidPrimitive.BOX, dimensions=[bin_depth, wall_width, interior_height], )) poses.append( Pose( position=Point(x=x, y=y - interior_width / 2 - wall_width / 2, z=base_height + interior_height / 2), orientation=Quaternion(x=0, y=0, z=0, w=1), )) # Create right side objects.append( SolidPrimitive( type=SolidPrimitive.BOX, dimensions=[bin_depth, wall_width, interior_height], )) poses.append( Pose( position=Point(x=x, y=y + interior_width / 2 + wall_width / 2, z=base_height + interior_height / 2), orientation=Quaternion(x=0, y=0, z=0, w=1), )) # Create near side objects.append( SolidPrimitive( type=SolidPrimitive.BOX, dimensions=[wall_depth, interior_width, interior_height], )) poses.append( Pose( position=Point(x=x - interior_depth / 2 - wall_depth / 2, y=y, z=base_height + interior_height / 2), orientation=Quaternion(x=0, y=0, z=0, w=1), )) # Create far side objects.append( SolidPrimitive( type=SolidPrimitive.BOX, dimensions=[wall_depth, interior_width, interior_height], )) poses.append( Pose( position=Point(x=x + interior_depth / 2 + wall_depth / 2, y=y, z=base_height + interior_height / 2), orientation=Quaternion(x=0, y=0, z=0, w=1), )) co = CollisionObject() co.operation = CollisionObject.ADD co.id = "order_bin" 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 __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