Python TransformListener.getLatestCommonTime方法代码示例

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import getLatestCommonTime [as 别名]
def subscriber_callback(data):
	occupancyMap = transformation(data.data, data.info.width, data.info.height)
	way_points = find_goals(occupancyMap, data.info.width, data.info.height)
	result = random.choice(way_points)

	try:
		planMaker = rospy.ServiceProxy('move_base/make_plan', nav_srv.GetPlan)
		listener = TransformListener()
		listener.waitForTransform("base_link", "map", rospy.Time(), rospy.Duration(4.0))
		t = listener.getLatestCommonTime("base_link", "map")
		position, quaternion = listener.lookupTransform("base_link", "map", t)
		pos_x = position[0]
		pos_y = position[1]
		pos_z = position[2]
		goal_robot = create_goal((result[1]-2000)*data.info.resolution,(result[0]-2000)*data.info.resolution)
		#Make plan with 0.5 meter flexibility, from target pose and current pose (with same header)
		start_pose = create_message(pos_x,pos_y,pos_z,quaternion)

		plan = planMaker(
			start_pose,
			goal_robot.target_pose,
			0.5)
		action_server = actionlib.SimpleActionClient('move_base', mb_msg.MoveBaseAction);
		action_server.wait_for_server()
		send_goal(goal_robot, action_server)

	except rospy.ServiceException, e:
		print "plan service call failed: %s"%e

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import getLatestCommonTime [as 别名]
class ForceFromGravity(object):
    def __init__(self):
        self.tf_l = TransformListener()
        self.last_wrench = None
        self.wrench_sub = rospy.Subscriber('/left_wrist_ft',
                                           WrenchStamped,
                                           self.wrench_cb,
                                           queue_size=1)
    def wrench_cb(self, data):
        self.last_wrench = data

    def compute_forces(self):
        qs = self.get_orientation()
        o = qs.quaternion
        r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w])
        rospy.loginfo("wrist_left_ft_link rpy vs world: " + str( (round(r, 3), round(p, 3), round(y, 3)) ))
        rospy.loginfo("in degrees: " + str( (round(degrees(r), 3), round(degrees(p), 3), round(degrees(y), 3)) ))
        hand_total_force = 10 # 10 Newton, near to a Kg
        fx = hand_total_force * cos(r) * -1.0 # hack
        fy = hand_total_force * cos(p)
        fz = hand_total_force * cos(y)
        #total = abs(fx) + abs(fy) + abs(fz)
        #rospy.loginfo("fx, fy, fz, total:")
        #rospy.loginfo( str( (round(fx, 3), round(fy, 3), round(fz, 3), round(total, 3)) ) )
        return fx, fy, fz

    def get_last_forces(self):
        f = self.last_wrench.wrench.force
        return f.x, f.y, f.z

    def get_orientation(self, from_frame="wrist_left_ft_link"):
        qs = QuaternionStamped()
        qs.quaternion.w = 1.0
        qs.header.stamp = self.tf_l.getLatestCommonTime("world", from_frame)
        qs.header.frame_id = "/" + from_frame
        transform_ok = False
        while not transform_ok and not rospy.is_shutdown():
            try:
                world_q = self.tf_l.transformQuaternion("/world", qs)
                transform_ok = True
                return world_q
            except ExtrapolationException as e:
                rospy.logwarn(
                    "Exception on transforming pose... trying again \n(" + str(e) + ")")
                rospy.sleep(0.05)
                qs.header.stamp = self.tf_l.getLatestCommonTime(
                    "world", from_frame)

    def run(self):
        r = rospy.Rate(1)
        while not rospy.is_shutdown():
            cx, cy, cz = self.compute_forces()
            c_total = abs(cx) + abs(cy) + abs(cz)
            fx, fy, fz = self.get_last_forces()
            f_total = abs(fx) + abs(fy) + abs(fz)
            rospy.loginfo("Computed Forces:" + str(round(c_total, 3)) + "\n" + str( (round(cx, 3), round(cy, 3), round(cz, 3) )))
            rospy.loginfo("Real Forces    :" + str(round(f_total, 3)) + "\n" + str( (round(fx, 3), round(fy, 3), round(fz, 3) )))
            r.sleep()

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import getLatestCommonTime [as 别名]
class TfFilter:
    def __init__(self, buffer_size):
        self.tf = TransformListener(True, rospy.Duration(5))
        self.target = ''
        self.buffer = np.zeros((buffer_size, 1))
        self.buffer_ptr = 0
        self.buffer_size = buffer_size
        self.tf_sub = rospy.Subscriber('tf', tfMessage, self.tf_cb)
        self.tf_pub = rospy.Publisher('tf', tfMessage)
        self.srv = rospy.Service(SERVICE, PublishGoal, self.publish_goal_cb)

    def tf_cb(self, msg):
        for t in msg.transforms:
            if t.child_frame_id == self.target:
                time = self.tf.getLatestCommonTime(self.source, self.target)
                p, q = self.tf.lookupTransform(self.source, self.target, time)
                rm = tfs.quaternion_matrix(q)
                # assemble a new coordinate frame that has z-axis aligned with
                # the vertical direction and x-axis facing the z-axis of the
                # original frame
                z = rm[:3, 2]
                z[2] = 0
                axis_x = tfs.unit_vector(z)
                axis_z = tfs.unit_vector([0, 0, 1])
                axis_y = np.cross(axis_x, axis_z)
                rm = np.vstack((axis_x, axis_y, axis_z)).transpose()
                # shift the pose along the x-axis
                self.position = p + np.dot(rm, self.d)[:3]
                self.buffer[self.buffer_ptr] = tfs.euler_from_matrix(rm)[2]
                self.buffer_ptr = (self.buffer_ptr + 1) % self.buffer_size
                self.publish_filtered_tf(t.header)

    def publish_filtered_tf(self, header):
        yaw = np.median(self.buffer)
        q = tfs.quaternion_from_euler(0, 0, yaw - math.pi)
        ts = TransformStamped()
        ts.header = header
        ts.header.frame_id = self.source
        ts.child_frame_id = self.goal
        ts.transform.translation.x = self.position[0]
        ts.transform.translation.y = self.position[1]
        ts.transform.translation.z = 0
        ts.transform.rotation.x = q[0]
        ts.transform.rotation.y = q[1]
        ts.transform.rotation.z = q[2]
        ts.transform.rotation.w = q[3]
        msg = tfMessage()
        msg.transforms.append(ts)
        self.tf_pub.publish(msg)

    def publish_goal_cb(self, r):
        rospy.loginfo('Received [%s] request. Will start publishing %s frame' %
                      (SERVICE, r.goal_frame_id))
        self.source = r.source_frame_id
        self.target = r.target_frame_id
        self.goal = r.goal_frame_id
        self.d = [r.displacement.x, r.displacement.y, r.displacement.z]
        return []

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import getLatestCommonTime [as 别名]
class KinectNiteHelp:
    def __init__(self, name="kinect_listener", user=1):
        # rospy.init_node(name, anonymous=True)
        self.tf = TransformListener()
        self.user = user

    def getLeftHandPos(self):
        if self.tf.frameExists(BASE_FRAME) and self.tf.frameExists("left_hand_1"):
            print " Inside TF IF Get LEft HAnd POS "
            t = self.tf.getLatestCommonTime(BASE_FRAME, "left_hand_1")
            (leftHandPos, quaternion) = self.tf.lookupTransform(BASE_FRAME, "left_hand_1", t)
            return leftHandPos

    def getRightHandPos(self):
        if self.tf.frameExists(BASE_FRAME) and self.tf.frameExists("right_hand_1"):
            t = self.tf.getLatestCommonTime(BASE_FRAME, "right_hand_1")
            (rightHandPos, quaternion) = self.tf.lookupTransform(BASE_FRAME, "right_hand_1", t)
            return rightHandPos

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import getLatestCommonTime [as 别名]
class TransformNode:
    def __init__(self, *args):
        self.tf = TransformListener()
        rospy.Subscriber("/tf", TFMessage, transform, queue_size=1)

    def transform(self, msg):
        if self.tf.frameExists("/base_link") and self.tf.frameExists("/map"):
            t = self.tf.getLatestCommonTime("/base_link", "/map")
            position, quaternion = self.tf.lookupTransform("/base_link", "/map", t)
            print position, quaternion

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import getLatestCommonTime [as 别名]
class Transformation():
	def __init__(self):
		pub = 0
		self.tf = TransformListener()
		self.tf1 = TransformerROS()
		self.fdata = geometry_msgs.msg.TransformStamped()
		self.fdata_base = geometry_msgs.msg.TransformStamped()
		self.transform = tf.TransformBroadcaster()
		self.wrench = WrenchStamped()
		self.wrench_bl = WrenchStamped()
		
	def wrench_cb(self,msg):
		self.wrench = msg.wrench
		self.transform.sendTransform((0,0,-0.025),quaternion_from_euler(3.14, 0, 3.665195102, 'rxyz'),rospy.Time.now(),'/ft_debug_link','/arm_7_link')
		self.fdata.transform.translation.x = self.wrench.force.x
		self.fdata.transform.translation.y = self.wrench.force.y
		self.fdata.transform.translation.z = self.wrench.force.z
		
		try:
			if self.tf.frameExists("/dummy_link") and self.tf.frameExists("ft_debug_link"):
				t = self.tf.getLatestCommonTime("/dummy_link", "/ft_debug_link")
				(transform_ee_base_position,transform_ee_base_quaternion) = self.tf.lookupTransform("/dummy_link", '/ft_debug_link', t)
	   				#print transform_ee_base_position
	   				#print transform_ee_base_quaternion
	   				#print self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)
		except(tf.LookupException,tf.ConnectivityException):
			print("TRANSFORMATION ERROR")
			sss.say(["error"])	
			#return 'failed'
		
		self.fdata_base.transform.translation.x =((self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[0,0] * self.fdata.transform.translation.x)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[0,1] * self.fdata.transform.translation.y)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[0,2] * self.fdata.transform.translation.z)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[0,3]))
		
		self.fdata_base.transform.translation.y =((self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[1,0] * self.fdata.transform.translation.x)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[1,1] * self.fdata.transform.translation.y)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[1,2] * self.fdata.transform.translation.z)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[1,3]))

		self.fdata_base.transform.translation.z =((self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[2,0] * self.fdata.transform.translation.x)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[2,1] * self.fdata.transform.translation.y)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[2,2] * self.fdata.transform.translation.z)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[2,3]))	
		
		self.wrench_bl.wrench.force.y = self.fdata_base.transform.translation.y
		self.wrench_bl.wrench.force.x = self.fdata_base.transform.translation.x
		self.wrench_bl.wrench.force.z = self.fdata_base.transform.translation.z
		self.wrench_bl.header.stamp = rospy.Time.now();
		self.pub.publish(self.wrench_bl)

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import getLatestCommonTime [as 别名]
class Demo:
    def __init__(self, goals):
        rospy.init_node("demo", anonymous=True)
        self.frame = rospy.get_param("~frame")
        self.pubGoal = rospy.Publisher("goal", PoseStamped, queue_size=1)
        self.listener = TransformListener()
        self.goals = goals
        self.goalIndex = 0

    def run(self):
        self.listener.waitForTransform("/world", self.frame, rospy.Time(), rospy.Duration(5.0))
        goal = PoseStamped()
        goal.header.seq = 0
        goal.header.frame_id = "world"
        while not rospy.is_shutdown():
            goal.header.seq += 1
            goal.header.stamp = rospy.Time.now()
            goal.pose.position.x = self.goals[self.goalIndex][0]
            goal.pose.position.y = self.goals[self.goalIndex][1]
            goal.pose.position.z = self.goals[self.goalIndex][2]
            quaternion = tf.transformations.quaternion_from_euler(0, 0, self.goals[self.goalIndex][3])
            goal.pose.orientation.x = quaternion[0]
            goal.pose.orientation.y = quaternion[1]
            goal.pose.orientation.z = quaternion[2]
            goal.pose.orientation.w = quaternion[3]

            self.pubGoal.publish(goal)

            t = self.listener.getLatestCommonTime("/world", self.frame)
            if self.listener.canTransform("/world", self.frame, t):
                position, quaternion = self.listener.lookupTransform("/world", self.frame, t)
                rpy = tf.transformations.euler_from_quaternion(quaternion)
                if (
                    math.fabs(position[0] - self.goals[self.goalIndex][0]) < 0.3
                    and math.fabs(position[1] - self.goals[self.goalIndex][1]) < 0.3
                    and math.fabs(position[2] - self.goals[self.goalIndex][2]) < 0.3
                    and math.fabs(rpy[2] - self.goals[self.goalIndex][3]) < math.radians(10)
                    and self.goalIndex < len(self.goals) - 1
                ):
                    rospy.sleep(self.goals[self.goalIndex][4])
                    self.goalIndex += 1

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import getLatestCommonTime [as 别名]
class Controller():
    Manual = 0
    Automatic = 1
    TakeOff = 2

    def __init__(self):
        self.pubNav = rospy.Publisher('cmd_vel', Twist, queue_size=1)
        self.listener = TransformListener()
        rospy.Subscriber("joy", Joy, self._joyChanged)
        rospy.Subscriber("cmd_vel_telop", Twist, self._cmdVelTelopChanged)
        self.cmd_vel_telop = Twist()
        self.pidX = PID(20, 12, 0.0, -30, 30, "x")
        self.pidY = PID(-20, -12, 0.0, -30, 30, "y")
        self.pidZ = PID(15000, 3000.0, 1500.0, 10000, 60000, "z")
        self.pidYaw = PID(-50.0, 0.0, 0.0, -200.0, 200.0, "yaw")
        self.state = Controller.Manual
        self.targetZ = 0.5
        self.lastJoy = Joy()

    def _cmdVelTelopChanged(self, data):
        self.cmd_vel_telop = data
        if self.state == Controller.Manual:
            self.pubNav.publish(data)

    def pidReset(self):
        self.pidX.reset()
        self.pidZ.reset()
        self.pidZ.reset()
        self.pidYaw.reset()

    def _joyChanged(self, data):
        if len(data.buttons) == len(self.lastJoy.buttons):
            delta = np.array(data.buttons) - np.array(self.lastJoy.buttons)
            print ("Buton ok")
            #Button 1
            if delta[0] == 1 and self.state != Controller.Automatic:
                print("Automatic!")
                thrust = self.cmd_vel_telop.linear.z
                print(thrust)
                self.pidReset()
                self.pidZ.integral = thrust / self.pidZ.ki
                self.targetZ = 0.5
                self.state = Controller.Automatic
            #Button 2
            if delta[1] == 1 and self.state != Controller.Manual:
                print("Manual!")
                self.pubNav.publish(self.cmd_vel_telop)
                self.state = Controller.Manual
            #Button 3
            if delta[2] == 1:
                self.state = Controller.TakeOff
                print("TakeOff!")
            #Button 5
            if delta[4] == 1:
                self.targetZ += 0.1
                print(self.targetZ)
            #Button 6
            if delta[5] == 1:
                self.targetZ -= 0.1
                print(self.targetZ)
        self.lastJoy = data

    def run(self):
        thrust = 0
        print("jello")
        while not rospy.is_shutdown():
            if self.state == Controller.TakeOff:
                t = self.listener.getLatestCommonTime("/mocap", "/Nano_Mark")
                if self.listener.canTransform("/mocap", "/Nano_Mark", t):
                    position, quaternion = self.listener.lookupTransform("/mocap", "/Nano_Mark", t)

                    if position[2] > 0.1 or thrust > 50000:
                        self.pidReset()
                        self.pidZ.integral = thrust / self.pidZ.ki
                        self.targetZ = 0.5
                        self.state = Controller.Automatic
                        thrust = 0
                    else:
                        thrust += 100
                        msg = self.cmd_vel_telop
                        msg.linear.z = thrust
                        self.pubNav.publish(msg)

            if self.state == Controller.Automatic:
                # transform target world coordinates into local coordinates
                t = self.listener.getLatestCommonTime("/Nano_Mark", "/mocap")
                print(t);
                if self.listener.canTransform("/Nano_Mark", "/mocap", t):
                    targetWorld = PoseStamped()
                    targetWorld.header.stamp = t
                    targetWorld.header.frame_id = "mocap"
                    targetWorld.pose.position.x = 0
                    targetWorld.pose.position.y = 0
                    targetWorld.pose.position.z = self.targetZ
                    quaternion = tf.transformations.quaternion_from_euler(0, 0, 0)
                    targetWorld.pose.orientation.x = quaternion[0]
                    targetWorld.pose.orientation.y = quaternion[1]
                    targetWorld.pose.orientation.z = quaternion[2]
                    targetWorld.pose.orientation.w = quaternion[3]

#.........这里部分代码省略.........

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import getLatestCommonTime [as 别名]

#.........这里部分代码省略.........
            #Button 5
            if delta[4] == 1:
                
                #self.targetX = -1.0
		#self.targetY = -1.0
		self.targetZ = 1.0
                self.des_angle = -90.0
		#if self.des_angle > 179:
		#    self.des_angle = -179.0
                #print(self.targetZ)
                #self.power += 100.0
                #print(self.power)
                self.state = Controller.Automatic
            #Button 6
            if delta[5] == 1:
		#self.targetX = 1.0
                #self.targetY = 1.0
		self.targetZ = 0.5
                self.des_angle = 90.0
                #if self.des_angle < -179:
                #    self.des_angle = 179.0

                #print(self.targetZ)
                #self.power -= 100.0
                #print(self.power)
                self.state = Controller.Automatic
        self.lastJoy = data

    def run(self):
        thrust = 0
        print("jello")
        while not rospy.is_shutdown():
            if self.state == Controller.TakeOff:
                t = self.listener.getLatestCommonTime("/mocap", "/Nano_Mark_Gon4")
                if self.listener.canTransform("/mocap", "/Nano_Mark_Gon4", t):
                    position, quaternion = self.listener.lookupTransform("/mocap","/Nano_Mark_Gon4", t)
                    print(position[0],position[1],position[2])			
                    #if position[2] > 0.1 or thrust > 50000:
		    if thrust > 51000:
                        self.pidReset()
                        self.pidZ.integral = thrust / self.pidZ.ki
                        #self.targetZ = 0.5
                        self.state = Controller.Automatic
                        thrust = 0
                    else:
                        thrust += 300
                        self.power = thrust
                        msg = self.cmd_vel_telop
                        msg.linear.z = thrust
                        self.pubNav.publish(msg)

            if self.state == Controller.Automatic:
                # transform target world coordinates into local coordinates
                t = self.listener.getLatestCommonTime("/mocap","/Nano_Mark_Gon4")
                print(t)
                if self.listener.canTransform("/mocap","/Nano_Mark_Gon4", t):
                    position, quaternion = self.listener.lookupTransform("/mocap","/Nano_Mark_Gon4",t)
                    #print(position[0],position[1],position[2])	
                    euler = tf.transformations.euler_from_quaternion(quaternion)
		    print(euler[2]*(180/math.pi))
                    msg = self.cmd_vel_telop
                    #print(self.power)

                    #Descompostion of the x and y contributions following the Z-Rotation
                    x_prim = self.pidX.update(0.0, self.targetX-position[0])
                    y_prim = self.pidY.update(0.0,self.targetY-position[1])

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import getLatestCommonTime [as 别名]
class Bag2UosConverter:
    def __init__(self, rootdir, scale_factor, transform_pitch_angle):
	rospy.init_node('bad_to_uos_convert', anonymous=False)
	rospy.Subscriber('velotime_points', PointCloud2, self.processMsg)
	self.counter = 0
	self.rootdir = rootdir
	self.scale_factor = scale_factor
	self.tf = TransformListener()
	self.init = True
	self.transform_pitch_angle = math.radians(transform_pitch_angle)
	pass
    def transformPoint(self,x ,y , z):
	yy = y
	xx =  math.cos(self.transform_pitch_angle) * x + math.sin(self.transform_pitch_angle) * z
	zz = -math.sin(self.transform_pitch_angle) * x + math.cos(self.transform_pitch_angle) * z
	return xx, yy, zz
    def run(self):
	rospy.spin()
	pass
    def processMsg(self, msg):
	print 'data arrived'
	# print list(cloud)
	# print self.tf.frameExists("velodyne") , self.tf.frameExists("odom")

	if self.tf.frameExists("world") and self.tf.frameExists("odom"):
	    t = self.tf.getLatestCommonTime("world", "odom")
	    position, quat= self.tf.lookupTransform("world", "odom", t)
	    print position, quat
	    euler = tf.transformations.euler_from_quaternion(quat)
	    # heading_rad = math.degrees(euler[2])
	    heading_rad = euler[2]
	    print heading_rad
	    cloud = pc2.read_points(msg, skip_nans=True)
	    self.saveData(list(cloud))
	    self.savePose(position[0],position[1],0,0,0,heading_rad)

	    self.counter = self.counter + 1
	if self.init:
	    self.init = False
	    cloud = pc2.read_points(msg, skip_nans=True)
	    self.saveData(list(cloud))
	    self.savePose(0,0,0,0,0,0)
	    self.counter = self.counter + 1

    def saveData(self, cloud):
	file_string = 'scan'+format(self.counter,'03d')
	fullfilename_data = self.rootdir + file_string + '.3d'
	fh_data  = open(fullfilename_data, 'w')
	for (x,y,z,intensity,ring) in cloud:
	    # print x,y,z
	    x, y, z = self.transformPoint(x, y, z)
	    x = x*self.scale_factor
	    y = y*self.scale_factor
	    z = z*self.scale_factor
	    str_ = format(x, '.1f')    + ' ' + format(z, '.1f')     + ' ' + format(y, '.1f')   + '\n'
	    fh_data.write(str_)
	fh_data.close()

    def savePose(self, x, y, z, roll, pitch, yaw):
	file_string = 'scan'+format(self.counter,'03d')
	fullfilename_pose = self.rootdir + file_string + '.pose'
	fh_pose  = open(fullfilename_pose, 'w')
	x = x*self.scale_factor
	y = y*self.scale_factor
	z = z*self.scale_factor
	roll  = roll*180/math.pi
	yaw   = yaw*180/math.pi
	pitch = pitch*180/math.pi
	str_first_line = format(x, '.1f')    + ' ' + format(-z, '.1f')     + ' ' + format(y, '.1f')   + '\n'
	str_2nd_line   = format(roll, '.1f') + ' ' + format(-yaw, '.1f') + ' ' + format(pitch, '.1f') + '\n'
	print str_first_line+str_2nd_line
	fh_pose.writelines([str_first_line, str_2nd_line])
	fh_pose.close()

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import getLatestCommonTime [as 别名]

#.........这里部分代码省略.........
            if sign == '+':
                args.append(1.0)
            else:
                args.append(-1.0)

        args.extend([self.axis_force[0], self.axis_force[1], self.axis_force[2],
                     self.axis_torque[0], self.axis_torque[1], self.axis_torque[2]])

        self.frame_fixer = WrenchFixer(*args)

        return config

    def follow_pose_cb(self, data):
        if data.header.frame_id[0] == '/':
            frame_id = data.header.frame_id[1:]
        else:
            frame_id = data.header.frame_id
        if frame_id != self.global_frame_id:
            rospy.logwarn(
                "Poses to follow should be in frame " + self.global_frame_id +
                 " and is in " + str(frame_id) + ", transforming into " + self.global_frame_id + "...")
            world_ps = self.transform_pose_to_world(
                data.pose, from_frame=data.header.frame_id)
            ps = PoseStamped()
            ps.header.stamp = data.header.stamp
            ps.header.frame_id = self.global_frame_id
            ps.pose = world_ps
            self.last_pose_to_follow = ps
        else:
            self.last_pose_to_follow = data

    def transform_pose_to_world(self, pose, from_frame="arm_right_tool_link"):
        ps = PoseStamped()
        ps.header.stamp = self.tf_l.getLatestCommonTime(
            self.global_frame_id, from_frame)
        ps.header.frame_id = "/" + from_frame
        # TODO: check pose being PoseStamped or Pose
        ps.pose = pose
        transform_ok = False
        while not transform_ok and not rospy.is_shutdown():
            try:
                world_ps = self.tf_l.transformPose(self.global_frame_id, ps)
                transform_ok = True
                return world_ps
            except ExtrapolationException as e:
                rospy.logwarn(
                    "Exception on transforming pose... trying again \n(" + str(e) + ")")
                rospy.sleep(0.05)
                ps.header.stamp = self.tf_l.getLatestCommonTime(
                    self.global_frame_id, from_frame)

    def wrench_cb(self, data):
        self.last_wrench = data

    def get_initial_pose(self):
        zero_pose = Pose()
        zero_pose.orientation.w = 1.0

        current_pose = self.transform_pose_to_world(
            zero_pose, from_frame=self.goal_frame_id)
        return current_pose

    def get_abs_total_force(self, wrench_msg):
        f = wrench_msg.wrench.force
        return abs(f.x) + abs(f.y) + abs(f.z)

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import getLatestCommonTime [as 别名]
class GripperMarkers:

    _offset = 0.09
    def __init__(self, side_prefix):
        
        self.ik = IK(side_prefix)
        self.side_prefix = side_prefix
        self._im_server = InteractiveMarkerServer('ik_request_markers')
        self._tf_listener = TransformListener()
        self._menu_handler = MenuHandler()
        self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb)
        self.is_control_visible = False
        self.ee_pose = self.get_ee_pose()
        self.update_viz()
        self._menu_handler.apply(self._im_server, 'ik_target_marker')
        self._im_server.applyChanges()

    def get_ee_pose(self):
        from_frame = '/base_link'
        to_frame = '/' + self.side_prefix + '_wrist_roll_link'
        try:
            t = self._tf_listener.getLatestCommonTime(from_frame, to_frame)
            (pos, rot) = self._tf_listener.lookupTransform(from_frame, to_frame, t)
        except:
            rospy.logwarn('Could not get end effector pose through TF.')
            pos = [1.0, 0.0, 1.0]
            rot = [0.0, 0.0, 0.0, 1.0]

	return Pose(Point(pos[0], pos[1], pos[2]),
			Quaternion(rot[0], rot[1], rot[2], rot[3]))

    def move_to_pose_cb(self, dummy):
        rospy.loginfo('You pressed the `Move arm here` button from the menu.')

        target_pose = GripperMarkers._offset_pose(self.ee_pose)
        ik_solution = self.ik.get_ik_for_ee(target_pose)
    
        #TODO: Send the arms to ik_solution

    def marker_clicked_cb(self, feedback):
        if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
            self.ee_pose = feedback.pose
            self.update_viz()
            self._menu_handler.reApply(self._im_server)
            self._im_server.applyChanges()

        elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
            rospy.loginfo('Changing visibility of the pose controls.')
            if (self.is_control_visible):
                self.is_control_visible = False
            else:
                self.is_control_visible = True
        else:
            rospy.loginfo('Unhandled event: ' + str(feedback.event_type))

    def update_viz(self):
        
        menu_control = InteractiveMarkerControl()
        menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
        menu_control.always_visible = True
        frame_id = 'base_link'
        pose = self.ee_pose
        
        menu_control = self._add_gripper_marker(menu_control)
        text_pos = Point()
        text_pos.x = pose.position.x
        text_pos.y = pose.position.y
        text_pos.z = pose.position.z + 0.1
        text = 'x=' + str(pose.position.x) + ' y=' + str(pose.position.y) + ' x=' + str(pose.position.z)
        menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
                                           id=0, scale=Vector3(0, 0, 0.03),
                                           text=text,
                                           color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
                                           header=Header(frame_id=frame_id),
                                           pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
        int_marker = InteractiveMarker()
        int_marker.name = 'ik_target_marker'
        int_marker.header.frame_id = frame_id
        int_marker.pose = pose
        int_marker.scale = 0.2
        self._add_6dof_marker(int_marker)
        int_marker.controls.append(menu_control)
        self._im_server.insert(int_marker, self.marker_clicked_cb)

    def _add_gripper_marker(self, control):
        is_hand_open=False
        if is_hand_open:
            angle = 28 * numpy.pi / 180.0
        else:
            angle = 0
        transform1 = tf.transformations.euler_matrix(0, 0, angle)
        transform1[:3, 3] = [0.07691 - GripperMarkers._offset, 0.01, 0]
        transform2 = tf.transformations.euler_matrix(0, 0, -angle)
        transform2[:3, 3] = [0.09137, 0.00495, 0]
        t_proximal = transform1
        t_distal = tf.transformations.concatenate_matrices(transform1, transform2)
        mesh1 = self._make_mesh_marker()
        mesh1.mesh_resource = ('package://pr2_description/meshes/gripper_v0/gripper_palm.dae')
        mesh1.pose.position.x = -GripperMarkers._offset
        mesh1.pose.orientation.w = 1
#.........这里部分代码省略.........

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import getLatestCommonTime [as 别名]
class GripperMarkers:

    _offset = 0.09
    def __init__(self, side_prefix):
        self._ik_service = IK(side_prefix)

        r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
        self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction)
        rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...')
        self.r_traj_action_client.wait_for_server()

        l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
        self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction)
        rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...')
        self.l_traj_action_client.wait_for_server()

        self.r_joint_names = ['r_shoulder_pan_joint',
                              'r_shoulder_lift_joint',
                              'r_upper_arm_roll_joint',
                              'r_elbow_flex_joint',
                              'r_forearm_roll_joint',
                              'r_wrist_flex_joint',
                              'r_wrist_roll_joint']
        self.l_joint_names = ['l_shoulder_pan_joint',
                              'l_shoulder_lift_joint',
                              'l_upper_arm_roll_joint',
                              'l_elbow_flex_joint',
                              'l_forearm_roll_joint',
                              'l_wrist_flex_joint',
                              'l_wrist_roll_joint']

        rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.marker_cb)
	
	self.side_prefix = side_prefix
        self._im_server = InteractiveMarkerServer("ik_request_markers_{}".format(side_prefix))
	self._tf_listener = TransformListener()
        self._menu_handler = MenuHandler()

        self._menu_handler.insert("Move {} arm here".format(side_prefix), callback=self.move_to_pose_cb)

        self.is_control_visible = False
        self.ee_pose = self.get_ee_pose()
        self.update_viz()
        self._menu_handler.apply(self._im_server, 'ik_target_marker')
        self._im_server.applyChanges()

    def get_ee_pose(self):
	from_frame = '/base_link'
	to_frame = '/' + self.side_prefix + '_wrist_roll_link'
	try:
	    t = self._tf_listener.getLatestCommonTime(from_frame, to_frame)
	    (pos, rot) = self._tf_listener.lookupTransform(from_frame, to_frame, t)
	except Exception as e:
	    rospy.logerr('Could not get end effector pose through TF.')
	    pos = [1.0, 0.0, 1.0]
	    rot = [0.0, 0.0, 0.0, 1.0]
	    import traceback
 	    traceback.print_exc()


	return Pose(Point(pos[0], pos[1], pos[2]),
			Quaternion(rot[0], rot[1], rot[2], rot[3]))

    def move_to_pose_cb(self, dummy):
        target_pose = GripperMarkers._offset_pose(self.ee_pose)
        ik_sol = self._ik_service.get_ik_for_ee(target_pose)
        self.move_to_joints(self.side_prefix, [ik_sol], 1.0)

    def marker_clicked_cb(self, feedback):
        if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
            self.ee_pose = feedback.pose
            self.update_viz()
            self._menu_handler.reApply(self._im_server)
            self._im_server.applyChanges()

        elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
            rospy.loginfo('Changing visibility of the pose controls.')
            if (self.is_control_visible):
                self.is_control_visible = False
            else:
                self.is_control_visible = True
        else:
            rospy.loginfo('Unhandled event: ' + str(feedback.event_type))

    def marker_cb(self, pose_markers):
        rospy.loginfo('AR Marker Pose updating')
        transform = GripperMarkers.get_matrix_from_pose(pose_markers.markers[0].pose.pose)
        offset_array = [0, 0, .03]
        offset_transform = tf.transformations.translation_matrix(offset_array)
        hand_transform = tf.transformations.concatenate_matrices(transform,
                                                             offset_transform)
        self.ee_pose = GripperMarkers.get_pose_from_transform(hand_transform)
        self.update_viz() 

    def update_viz(self):
        
        menu_control = InteractiveMarkerControl()
        menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
        menu_control.always_visible = True
        frame_id = 'base_link'
#.........这里部分代码省略.........

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import getLatestCommonTime [as 别名]
class GripperMarkers:

    _offset = 0.09
    def __init__(self, side_prefix):
        self.ik = IK(side_prefix)
	
        self.side_prefix = side_prefix
        self._im_server = InteractiveMarkerServer('ik_request_markers')
        self._tf_listener = TransformListener()
        self._menu_handler = MenuHandler()

        self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb)

        self.is_control_visible = False
        self.ee_pose = self.get_ee_pose()
        self.update_viz()
        self._menu_handler.apply(self._im_server, 'ik_target_marker')
        self._im_server.applyChanges()
        
        # Code for moving the robots joints
        switch_srv_name = 'pr2_controller_manager/switch_controller'
        rospy.loginfo('Waiting for switch controller service...')
        rospy.wait_for_service(switch_srv_name)
        self.switch_service_client = rospy.ServiceProxy(switch_srv_name,
                                                 SwitchController)
                                                 
        self.r_joint_names = ['r_shoulder_pan_joint',
                              'r_shoulder_lift_joint',
                              'r_upper_arm_roll_joint',
                              'r_elbow_flex_joint',
                              'r_forearm_roll_joint',
                              'r_wrist_flex_joint',
                              'r_wrist_roll_joint']
                              
        self.l_joint_names = ['l_shoulder_pan_joint',
                              'l_shoulder_lift_joint',
                              'l_upper_arm_roll_joint',
                              'l_elbow_flex_joint',
                              'l_forearm_roll_joint',
                              'l_wrist_flex_joint',
                              'l_wrist_roll_joint']
                              
        # Create a trajectory action client
        r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
        self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction)
        rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...')
        self.r_traj_action_client.wait_for_server()
        
        l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
        self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction)
        rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...')
        self.l_traj_action_client.wait_for_server()

    def get_ee_pose(self):
        from_frame = 'base_link'
        to_frame = self.side_prefix + '_wrist_roll_link'
        try:
           
            t = self._tf_listener.getLatestCommonTime(from_frame, to_frame)
 
            (pos, rot) = self._tf_listener.lookupTransform(from_frame, to_frame, t)

        except:

            rospy.logerr('Could not get end effector pose through TF.')
            pos = [1.0, 0.0, 1.0]
            rot = [0.0, 0.0, 0.0, 1.0]

        
        return Pose(Point(pos[0], pos[1], pos[2]),
               Quaternion(rot[0], rot[1], rot[2], rot[3]))

    def move_to_pose_cb(self, dummy):
        rospy.loginfo('You pressed the `Move arm here` button from the menu.')
        joint_positions = self.ik.get_ik_for_ee(self.ee_pose)
        print 'joint: ' + str(joint_positions)
        if(joint_positions != None):
            self.toggle_arm(self.side_prefix, 'Freeze', False)
            #joint_positions = [-0.993560463247, -0.357041076593, 0.0534981848008, -1.24934444608, -3.10068096283, -1.7906747183, -28.4704855309]
            print 'returned from IK: ' + str(joint_positions)
            #print 'random position : ' + str([-0.993560463247, -0.357041076593, 0.0534981848008, -1.24934444608, -3.10068096283, -1.7906747183, -28.4704855309])
            #joint_positions = [-1.03129153, -0.35312086, -0.08, -1.25402906, -2.98718287, -1.7816027, 3.03965124]
            self.move_to_joints(self.side_prefix, list(joint_positions), 5.0)
        print 'done'
        
    def toggle_arm(self, side, toggle, button):
        controller_name = side + '_arm_controller'
        print 'toggle'
        start_controllers = []
        stop_controllers = []
        if (toggle == 'Relax'):
            stop_controllers.append(controller_name)
        else:
            start_controllers.append(controller_name)
        self.set_arm_mode(start_controllers, stop_controllers)

    def set_arm_mode(self, start_controllers, stop_controllers):
        try:
            self.switch_service_client(start_controllers, stop_controllers, 1)
        except rospy.ServiceException:
#.........这里部分代码省略.........

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import getLatestCommonTime [as 别名]
class Controller:
    Idle = 0
    Automatic = 1
    TakingOff = 2
    Landing = 3

    def __init__(self, frame):
        self.frame = frame
        self.pubNav = rospy.Publisher('cmd_vel', Twist, queue_size=1)
        self.listener = TransformListener()
        self.pidX = PID(35, 10, 0.0, -20, 20, "x")
        self.pidY = PID(-35, -10, -0.0, -20, 20, "y")
        self.pidZ = PID(4000, 3000.0, 2000.0, 10000, 60000, "z")
        self.pidYaw = PID(-50.0, 0.0, 0.0, -200.0, 200.0, "yaw")
        self.state = Controller.Idle
        self.goal = Pose()
        rospy.Subscriber("goal", Pose, self._poseChanged)
        rospy.Service("takeoff", std_srvs.srv.Empty, self._takeoff)
        rospy.Service("land", std_srvs.srv.Empty, self._land)

    def getTransform(self, source_frame, target_frame):
        now = rospy.Time.now()
        success = False
        if self.listener.canTransform(source_frame, target_frame, rospy.Time(0)):
            t = self.listener.getLatestCommonTime(source_frame, target_frame)
            if self.listener.canTransform(source_frame, target_frame, t):
                position, quaternion = self.listener.lookupTransform(source_frame, target_frame, t)
                success = True
            delta = (now - t).to_sec() * 1000 #ms
            if delta > 25:
                rospy.logwarn("Latency: %f ms.", delta)
            #     self.listener.clear()
            #     rospy.sleep(0.02)
        if success:
            return position, quaternion, t

    def pidReset(self):
        self.pidX.reset()
        self.pidZ.reset()
        self.pidZ.reset()
        self.pidYaw.reset()

    def _poseChanged(self, data):
        self.goal = data

    def _takeoff(self, req):
        rospy.loginfo("Takeoff requested!")
        self.state = Controller.TakingOff
        return std_srvs.srv.EmptyResponse()

    def _land(self, req):
        rospy.loginfo("Landing requested!")
        self.state = Controller.Landing
        return std_srvs.srv.EmptyResponse()

    def run(self):
        thrust = 0
        while not rospy.is_shutdown():
            now = rospy.Time.now()
            if self.state == Controller.TakingOff:
                r = self.getTransform("/world", self.frame)
                if r:
                    position, quaternion, t = r

                    if position[2] > 0.05 or thrust > 50000:
                        self.pidReset()
                        self.pidZ.integral = thrust / self.pidZ.ki
                        self.targetZ = 0.5
                        self.state = Controller.Automatic
                        thrust = 0
                    else:
                        thrust += 100
                        msg = Twist()
                        msg.linear.z = thrust
                        self.pubNav.publish(msg)
                else:
                    rospy.logerr("Could not transform from /world to %s.", self.frame)

            if self.state == Controller.Landing:
                self.goal.position.z = 0.05
                r = self.getTransform("/world", self.frame)
                if r:
                    position, quaternion, t = r
                    if position[2] <= 0.1:
                        self.state = Controller.Idle
                        msg = Twist()
                        self.pubNav.publish(msg)
                else:
                    rospy.logerr("Could not transform from /world to %s.", self.frame)

            if self.state == Controller.Automatic or self.state == Controller.Landing:
                # transform target world coordinates into local coordinates
                r = self.getTransform("/world", self.frame)
                if r:
                    position, quaternion, t = r
                    targetWorld = PoseStamped()
                    targetWorld.header.stamp = t
                    targetWorld.header.frame_id = "world"
                    targetWorld.pose = self.goal

#.........这里部分代码省略.........