Python Pose.position方法代码示例

# 需要导入模块: from geometry_msgs.msg import Pose [as 别名]
# 或者: from geometry_msgs.msg.Pose import position [as 别名]
    def calculate_mk2_ik(self):

        #goal_point = Point(0.298, -0.249, -0.890) home position

        goal_pose = Pose()

        # Goto position 1
        goal_point = Point(0.298, -0.249, -0.890)
        #goal_point = Point(0.298, -0.5, -1.0)
        goal_pose.position = goal_point
        quat = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw
        goal_pose.orientation = Quaternion(*quat.tolist())
        moveit_goal = self.create_move_group_pose_goal(goal_pose, group="manipulator", end_link_name="link_7", plan_only=False)
        rospy.loginfo("Sending goal...")
        self.moveit_ac.send_goal(moveit_goal)
        rospy.loginfo("Waiting for result...")
        self.moveit_ac.wait_for_result(rospy.Duration(10.0))
        moveit_result = self.moveit_ac.get_result()

        # Goto position 2
        goal_point = Point(0.305, -0.249, -1.05)
        goal_pose.position = goal_point
        quat = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw
        goal_pose.orientation = Quaternion(*quat.tolist())
        moveit_goal = self.create_move_group_pose_goal(goal_pose, group="manipulator", end_link_name="link_7", plan_only=False)
        rospy.loginfo("Sending goal...")
        self.moveit_ac.send_goal(moveit_goal)
        rospy.loginfo("Waiting for result...")
        self.moveit_ac.wait_for_result(rospy.Duration(10.0))
        moveit_result = self.moveit_ac.get_result()

# 需要导入模块: from geometry_msgs.msg import Pose [as 别名]
# 或者: from geometry_msgs.msg.Pose import position [as 别名]
def move_to_box(objcolorl):
    if objcolorl == 0:
        #move to green box
        pose = Pose()
        pose.orientation = Quaternion(1.00, 0.0, 0.00, 0.00)
        pose.position = Point(0.570, -0.176, 0.283)

        path = '/home/ynazon1/Pictures/green_success.png' 
        img = cv2.imread(path)
        msg = cv_bridge.CvBridge().cv2_to_imgmsg(img, encoding="bgr8")
        pubpic = rospy.Publisher('/robot/xdisplay', Image, latch=True, queue_size=1)
        pubpic.publish(msg)
        # Sleep to allow for image to be published.
        rospy.sleep(1)

    else:
        #move to blue box
        pose = Pose()
        pose.orientation = Quaternion(1.00, 0.0, 0.00, 0.00)
        pose.position = Point(0.708, -0.153, 0.258)

        path = '/home/ynazon1/Pictures/red_success.png' 
        img = cv2.imread(path)
        msg = cv_bridge.CvBridge().cv2_to_imgmsg(img, encoding="bgr8")
        pubpic = rospy.Publisher('/robot/xdisplay', Image, latch=True, queue_size=1)
        pubpic.publish(msg)
        # Sleep to allow for image to be published.
        rospy.sleep(1)
    request_pose(pose, "left", left_group)

# 需要导入模块: from geometry_msgs.msg import Pose [as 别名]
# 或者: from geometry_msgs.msg.Pose import position [as 别名]
def pose_at_distance2(pose1, pose2, distance):
    """
    Returns a pose that has the same orientation as the original
    but the position is at a distance from the original.
    Very usefull when you want to mantain a distance from an object. 
    It takes into account where the robot is
    """

    new_pose2 = Pose()
    new_pose2.position = substract_vector(pose2.position, pose1.position)
    new_pose2 = pose_at_distance(new_pose2, distance)

    new_pose2.position = add_vectors(new_pose2.position, pose1.position)
    return new_pose2

# 需要导入模块: from geometry_msgs.msg import Pose [as 别名]
# 或者: from geometry_msgs.msg.Pose import position [as 别名]
def publisher(paths):
    publisher = rospy.Publisher('pathfinder', Path, queue_size=20)
    rospy.init_node('path_publisher', anonymous=True)
    rate = rospy.Rate(10) # 10 hertz
    msgs=[] #list for all the messages we need to publish
    for path in paths:
        msg = Path() #new path message
        msg.header = create_std_h() #add a header to the path message
        for x,y,z in path:
            #create a pose for each position in the path
            #position is the only field we care about, leave the rest as 0
            ps = PoseStamped()
            ps.header=create_std_h()
            i=Pose()
            i.position=Point(x,y,z)
            ps.pose=i
            msg.poses.append(ps)
        
        msgs.append(msg)
    while not rospy.is_shutdown(): # while we're going
        #publish each message
        for m in msgs:
            #rospy.loginfo(m)
            publisher.publish(m)
        rate.sleep()

# 需要导入模块: from geometry_msgs.msg import Pose [as 别名]
# 或者: from geometry_msgs.msg.Pose import position [as 别名]
def get_grasp_pose(g_decision_making_state, object_grasp_pose, object_grasp_pose_set):
    
	# load pre-computer grasps
	# loaded_file = numpy.load('../grasp_data/' + g_decision_making_state.object_name + '.npz' )
	
	# loaded_grasp = loaded_file['grasp'][0]
	# print len(loaded_grasp[g_decision_making_state.object_name])

	loaded_file = numpy.load('../grasp_data/' + g_decision_making_state.object_name + '.npz' )
	loaded_grasp = loaded_file['grasp'][0]
	num_of_grasp = len(loaded_grasp[g_decision_making_state.object_name])
	temp = object_grasp_pose
	for i in range(0,num_of_grasp):
		print i
		print_angles(loaded_grasp[g_decision_making_state.object_name][i])
		print_angles(temp)
		object_grasp_pose_temp = temp
		print_angles(temp)
		object_grasp_pose_set[i] = Pose()
		object_grasp_pose_set[i].position = temp.position
		object_grasp_pose_set[i].orientation = rotate_orientation(object_grasp_pose_temp.orientation, loaded_grasp[g_decision_making_state.object_name][i].orientation)

		move_ok = plan_and_move_end_effector_to(object_grasp_pose, 0.3, 
                                            g_bin_positions[g_decision_making_state.bin_id]['map_robot_position_2_ik_seed'])
		print_angles(object_grasp_pose_set[i])
		# raw_input("press Enter to continue...")

	return object_grasp_pose_set

# 需要导入模块: from geometry_msgs.msg import Pose [as 别名]
# 或者: from geometry_msgs.msg.Pose import position [as 别名]
 def execute(self, userdata):
     userdata.orders = [DrinkOrder('david', 'coke'), DrinkOrder('michael', 'milk')]
     pose = Pose()
     pose.position = Point(0.059, 6.26, 0.0)
     pose.orientation = Quaternion(0, 0, -0.59, 0.8)
     userdata.guests_location = pose
     return succeeded

# 需要导入模块: from geometry_msgs.msg import Pose [as 别名]
# 或者: from geometry_msgs.msg.Pose import position [as 别名]
            def obj_response_cb(userdata, response):
                if response.exists:
                    pose = Pose()
                    #Wont there be problems mixing float32 with 64? TODO
                    pose.position = Point(response.base_coordinates.x, response.base_coordinates.y, 0)
                    #TODO, this may give problems
                    pose.orientation = Quaternion(*quaternion_from_euler(0, 0, response.base_coordinates.z))
                    if MANTAIN_DISTANCE_FROM_OBJECT:
                        pose = pose_at_distance(pose, DISTANCE_FROM_OBJECT)
                    userdata.object_location = pose

                    release_pose = ObjectPose()
                    release_pose.pose = response.arm_coordinates
                    userdata.object_release_location = release_pose

                    print "\n\n printing pose for move to object"
                    print pose
                    print "\n\n printing pose for releasing the object"
                    print release_pose
                    print "\n that was the pose of the object\n\n"
                    print userdata.object_name
                    print "is the object name"
                    return succeeded
                else:
                    userdata.object_location = None
                    return aborted

# 需要导入模块: from geometry_msgs.msg import Pose [as 别名]
# 或者: from geometry_msgs.msg.Pose import position [as 别名]
def transform_pose(pose_in, transform):
    '''
    Transforms a pose

    Takes a pose defined in the frame defined by transform (i.e. the frame in which transform is the origin) and 
    returns it in the frame in which transform is defined.  Calling this with the origin pose will return transform.
    This returns
        
        pose.point = transform_point(pose_in.point, transform)
        
        pose.orientation = transform_orientation(pose_in.orientation, transform)
    
    **Args:**

        **pose (geometry_msgs.msg.Pose):** Pose to transform

        **transform (geometry_msgs.msg.Pose):** The transform
    
    **Returns:**
        A geometry_msgs.msg.Pose
    '''
    pose = Pose()
    pose.position = transform_point(pose_in.position, transform)
    pose.orientation = transform_quaternion(pose_in.orientation, transform)
    return pose

# 需要导入模块: from geometry_msgs.msg import Pose [as 别名]
# 或者: from geometry_msgs.msg.Pose import position [as 别名]
    def publishSphere2(self, pose, color, scale, lifetime=None):
        """
        Publish a sphere Marker. This renders a smoother, flatter-looking sphere.

        @param pose (numpy matrix, numpy ndarray, ROS Pose)
        @param color name (string) or RGB color value (tuple or list)
        @param scale (ROS Vector3, float)
        @param lifetime (float, None = never expire)
        """

        if (self.muted == True):
            return True

        # Convert input pose to a ROS Pose Msg
        if (type(pose) == numpy.matrix) or (type(pose) == numpy.ndarray):
            sphere_pose = mat_to_pose(pose)
        elif type(pose) == Pose:
            sphere_pose = pose
        elif type(pose) == Point:
            pose_msg = Pose()
            pose_msg.position = pose
            sphere_pose = pose_msg
        else:
            rospy.logerr("Pose is unsupported type '%s' in publishSphere()", type(pose).__name__)
            return False

        # Convert input scale to a ROS Vector3 Msg
        if type(scale) == Vector3:
            sphere_scale = scale
        elif type(scale) == float:
            sphere_scale = Vector3(scale, scale, scale)
        else:
            rospy.logerr("Scale is unsupported type '%s' in publishSphere()", type(scale).__name__)
            return False

        # Increment the ID number
        self.sphere_marker.id += 1

        # Get the default parameters
        sphere_marker = self.sphere_marker2 # sphere_marker2 = SPHERE_LIST

        if lifetime == None:
            sphere_marker.lifetime = rospy.Duration(0.0) # 0 = Marker never expires
        else:
            sphere_marker.lifetime = rospy.Duration(lifetime) # in seconds

        # Set the timestamp
        sphere_marker.header.stamp = rospy.Time.now()

        # Set marker size
        sphere_marker.scale = sphere_scale

        # Set marker color
        sphere_marker.color = self.getColor(color)

        # Set the pose of one sphere in the list
        sphere_marker.points[0] = sphere_pose.position
        sphere_marker.colors[0] = self.getColor(color)

        return self.publishMarker(sphere_marker)

# 需要导入模块: from geometry_msgs.msg import Pose [as 别名]
# 或者: from geometry_msgs.msg.Pose import position [as 别名]
    def _create_vertices_array(self) :

        for node in self.topo_map.nodes :
            marker = Marker()
            marker.header.frame_id = "/map"
            marker.type = marker.LINE_STRIP
            #marker.lifetime=2
            marker.scale.x = 0.1
            marker.color.a = 0.8
            marker.color.r = 0.7
            marker.color.g = 0.1
            marker.color.b = 0.2
            node._get_coords()
            count=0
            for i in node.vertices :
                #print i[0], i[1]
                Vert = Point()
                Vert.z = 0.05
                Vert.x = node.px + i[0]
                Vert.y = node.py + i[1]
                marker.points.append(Vert)
                vertname = "%s-%d" %(node.name, count)
                Pos = Pose()
                Pos.position = Vert
                self._vertex_marker(vertname, Pos, vertname)
                count+=1
            marker.points.append(marker.points[0])
            self.node_zone.markers.append(marker)
        
        idn = 0
        for m in self.node_zone.markers:
            m.id = idn
            idn += 1 

# 需要导入模块: from geometry_msgs.msg import Pose [as 别名]
# 或者: from geometry_msgs.msg.Pose import position [as 别名]
 def find_joint_pose(self, pose, targ_x=0.0, targ_y=0.0, targ_z=0.0,
                     targ_ox=0.0, targ_oy=0.0, targ_oz=0.0):
     '''
     Finds the joint position of the arm given some pose and the
     offsets from it (to avoid opening the structure all the time
     outside of the function).
     '''
     ik_srv = "ExternalTools/right/PositionKinematicsNode/IKService"
     iksvc = rospy.ServiceProxy(ik_srv, SolvePositionIK)
     ik_request = SolvePositionIKRequest()
     the_pose = deepcopy(pose)
     the_pose['position'] = Point(x=targ_x + self.baxter_off.linear.x,
                                  y=targ_y + self.baxter_off.linear.y,
                                  z=targ_z + self.baxter_off.linear.z)
     angles = tf.transformations.quaternion_from_euler( \
         targ_ox + self.baxter_off.angular.x, \
         targ_oy + self.baxter_off.angular.y, \
         targ_oz + self.baxter_off.angular.z)
     the_pose['orientation'] = Quaternion(x=angles[0],
                                          y=angles[1],
                                          z=angles[2],
                                          w=angles[3])
     approach_pose = Pose()
     approach_pose.position = the_pose['position']
     approach_pose.orientation = the_pose['orientation']
     hdr = Header(stamp=rospy.Time.now(), frame_id='base')
     pose_req = PoseStamped(header=hdr, pose=approach_pose)
     ik_request.pose_stamp.append(pose_req)
     resp = iksvc(ik_request)
     return dict(zip(resp.joints[0].name, resp.joints[0].position))

# 需要导入模块: from geometry_msgs.msg import Pose [as 别名]
# 或者: from geometry_msgs.msg.Pose import position [as 别名]
    def __init__(self, from_frame, to_frame, position, orientation):
        self.server = InteractiveMarkerServer("posestamped_im")
        o = orientation
        r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w])
        rospy.loginfo("Publishing transform and PoseStamped from: " +
                      from_frame + " to " + to_frame +
                      " at " + str(position.x) + " " + str(position.y) + " " + str(position.z) +
                      " and orientation " + str(o.x) + " " + str(o.y) +
                      " " + str(o.z) + " " + str(o.w) + " or rpy " +
                      str(r) + " " + str(p) + " " + str(y))
        self.menu_handler = MenuHandler()
        self.from_frame = from_frame
        self.to_frame = to_frame
        # Transform broadcaster
        self.tf_br = tf.TransformBroadcaster()

        self.pub = rospy.Publisher('/posestamped', PoseStamped, queue_size=1)
        rospy.loginfo("Publishing posestampeds at topic: " + str(self.pub.name))
        pose = Pose()
        pose.position = position
        pose.orientation = orientation
        self.last_pose = pose
        self.print_commands(pose)
        self.makeGraspIM(pose)

        self.server.applyChanges()

# 需要导入模块: from geometry_msgs.msg import Pose [as 别名]
# 或者: from geometry_msgs.msg.Pose import position [as 别名]
def main(argv):
    x = float(sys.argv[1])
    y = float(sys.argv[2])
    th = float(sys.argv[3])

    print("x: %.3f  y: %.3f  Th: %.1f") % (x, y, th) # print 2D pose data to terminal
	
    time.sleep(0.5)
    #publish pose data to ros topic
    msg = Pose()

    q = tf.transformations.quaternion_from_euler(0, 0, th)

    msg.position = Point(x,y,0)

    msg.orientation.x = q[0]
    msg.orientation.y = q[1]
    msg.orientation.z = q[2]
    msg.orientation.w = q[3]

    pose_pub.publish(msg)
    
    key_pressed = False
    while key_pressed == False:
         key_pressed = select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], []) # is key pressed?
         time.sleep(0.5)
         
    sys.exit()

# 需要导入模块: from geometry_msgs.msg import Pose [as 别名]
# 或者: from geometry_msgs.msg.Pose import position [as 别名]
 def callback_Distance(data):
   #print data
   global i_distance
   m = marker_array.markers[markers_list[id_]]
   m.header.stamp = rospy.Time.now()
   m.action = 0
   m.type = m.SPHERE
   p = Pose()
   #print data.data[0], data.data[1], data.data[2]
   
   p.position = Point(data.data[0], data.data[1], data.data[2])
   if data.data[3] == data.data[4] :
     scale = data.data[3]
   else :
     i_distance = i_distance + 1
     scale = data.data[3]
     if i_distance%2 == 0:
       scale = data.data[4]
   m.pose = p
   m.scale.x = scale * 2
   m.scale.y = scale * 2
   m.scale.z = scale * 2
   m.color.r = 0
   m.color.g = 1
   m.color.b = 0
   m.color.a = 0.2
   m.lifetime = rospy.Duration(0)
   m.frame_locked = False

# 需要导入模块: from geometry_msgs.msg import Pose [as 别名]
# 或者: from geometry_msgs.msg.Pose import position [as 别名]
def eef_pose_pub():
  rospy.init_node('eef_publisher')
  rospy.loginfo("Publishing right EEF gripper location")
  listener = tf.TransformListener()
  pub = rospy.Publisher('eef_pose', Pose, queue_size=10)

  DEFAULT_LINK = '/right_ee_link'
  DEFAULT_RATE = 100

  # Pull from param server the hz and EEF link
  eef_link = rospy.get_param("~eef_link", DEFAULT_LINK)
  publish_rate = rospy.get_param("~eef_rate", DEFAULT_RATE)

  rate = rospy.Rate(publish_rate)
  while not rospy.is_shutdown():
    try: 
      trans, rot = listener.lookupTransform('/base_link', eef_link, rospy.Time(0))
    except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
      continue
    msg = Pose()
    msg.position = Point()
    msg.position.x, msg.position.y, msg.position.z = trans[0], trans[1], trans[2]
    msg.orientation = Quaternion() 
    msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w = rot[0], rot[1], rot[2], rot[3]
    pub.publish(msg)
    rate.sleep()