Python TransformListener.fromTranslationRotation方法代码示例

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

    def __init__(self):

        self.avg_pose = None
        self.tl = TransformListener()

        self.topics = rospy.get_param('~poses',[])
        print self.topics
        if len(self.topics) == 0:
            rospy.logerr('Please provide a poses list as input parameter')
            return
        self.output_pose = rospy.get_param('~output_pose','ar_avg_pose')
        self.output_frame = rospy.get_param('~output_frame', 'rf_map')
        
        self.subscribers = []
        for i in self.topics:
            subi = rospy.Subscriber(i, PoseStamped, self.callback, queue_size=1)
            self.subscribers.append(subi)

        self.pub = rospy.Publisher(self.output_pose, PoseStamped, queue_size=1)
            
        self.mutex_avg = threading.Lock()
        self.mutex_t = threading.Lock()
        
        self.transformations = {}
        
    def callback(self, pose_msg):

        # get transformation to common frame
        position = None
        quaternion = None
        if self.transformations.has_key(pose_msg.header.frame_id):
            position, quaternion = self.transformations[pose_msg.header.frame_id]
        else:
            if self.tl.frameExists(pose_msg.header.frame_id) and \
            self.tl.frameExists(self.output_frame):
                t = self.tl.getLatestCommonTime(self.output_frame, pose_msg.header.frame_id)
                position, quaternion = self.tl.lookupTransform(self.output_frame,
                                                               pose_msg.header.frame_id, t)
                self.mutex_t.acquire()
                self.transformations[pose_msg.header.frame_id] = (position, quaternion)
                self.mutex_t.release()
                rospy.loginfo("Got static transform for %s" % pose_msg.header.frame_id)

        # transform pose
        framemat = self.tl.fromTranslationRotation(position, quaternion)

        posemat = self.tl.fromTranslationRotation([pose_msg.pose.position.x,
                                                   pose_msg.pose.position.y,
                                                   pose_msg.pose.position.z],
                                                  [pose_msg.pose.orientation.x,
                                                   pose_msg.pose.orientation.y,
                                                   pose_msg.pose.orientation.z,
                                                   pose_msg.pose.orientation.w])

        newmat = numpy.dot(framemat,posemat)
        
        xyz = tuple(translation_from_matrix(newmat))[:3]
        quat = tuple(quaternion_from_matrix(newmat))

        tmp_pose = PoseStamped()
        tmp_pose.header.stamp = pose_msg.header.stamp
        tmp_pose.header.frame_id = self.output_frame
        tmp_pose.pose = Pose(Point(*xyz),Quaternion(*quat))
        
        tmp_angles = euler_from_quaternion([tmp_pose.pose.orientation.x,
                                            tmp_pose.pose.orientation.y,
                                            tmp_pose.pose.orientation.z,
                                            tmp_pose.pose.orientation.w])
        
        # compute average
        self.mutex_avg.acquire()
        
        if self.avg_pose == None or (pose_msg.header.stamp - self.avg_pose.header.stamp).to_sec() > 0.5:
            self.avg_pose = tmp_pose
        else:            
            self.avg_pose.header.stamp = pose_msg.header.stamp
            a = 0.7
            b = 0.3
            self.avg_pose.pose.position.x = a*self.avg_pose.pose.position.x + b*tmp_pose.pose.position.x
            self.avg_pose.pose.position.y = a*self.avg_pose.pose.position.y + b*tmp_pose.pose.position.y
            self.avg_pose.pose.position.z = a*self.avg_pose.pose.position.z + b*tmp_pose.pose.position.z

            angles = euler_from_quaternion([self.avg_pose.pose.orientation.x,
                                            self.avg_pose.pose.orientation.y,
                                            self.avg_pose.pose.orientation.z,
                                            self.avg_pose.pose.orientation.w])
            angles = list(angles)
            angles[0] = avgAngles(angles[0], tmp_angles[0], 0.7, 0.3)
            angles[1] = avgAngles(angles[1], tmp_angles[1], 0.7, 0.3)
            angles[2] = avgAngles(angles[2], tmp_angles[2], 0.7, 0.3)

            q = quaternion_from_euler(angles[0], angles[1], angles[2])
            
            self.avg_pose.pose.orientation.x = q[0]
            self.avg_pose.pose.orientation.y = q[1]
            self.avg_pose.pose.orientation.z = q[2]
            self.avg_pose.pose.orientation.w = q[3]

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

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

#.........这里部分代码省略.........
    # Subscriber callbacks:
    def control_input_callback(self, msg):
        rospy.logdebug('Current command is: ' + str(msg))
        self.latest_cmd_msg = msg
    
    def motor_enable_callback(self, msg):
        if msg.data != self.motor_enable:
            #rospy.loginfo('Motor enable: ' + str(msg.data))
            self.motor_enable = msg.data
    
    # Timer callbacks:
    def simulation_timer_callback(self, event):
        if False:
            print event.__dict__
#            print 'last_expected:        ', event.last_expected
#            print 'last_real:            ', event.last_real
#            print 'current_expected:     ', event.current_expected
#            print 'current_real:         ', event.current_real
#            print 'current_error:        ', (event.current_real - event.current_expected).to_sec()
#            print 'profile.last_duration:', event.last_duration.to_sec()
#            if event.last_real:
#                print 'last_error:           ', (event.last_real - event.last_expected).to_sec(), 'secs'
#            print
        if event.last_real is None:
            dt = 0.0
        else:
            dt = (event.current_real - event.last_real).to_sec()
            self.update_controller(dt)
            self.update_state(dt)
        #rospy.loginfo("position: " + str(self.position) + " velocity: " + str(self.velocity) + " thrust_cmd: " + str(self.thrust_cmd))
        if self.publish_odometry_messages:
            self.publish_odometry()
        if self.publish_state_estimate:
            self.publish_state(event.current_real)
        
    def update_state(self, dt):
        # The following model is completely arbitrary and should not be taken to be representative of
        # real vehicle performance!
        # But, it should be good enough to test out control modes etc.
        self.model.update(dt)
            
    def update_controller(self, dt):
        lcm = self.latest_cmd_msg
        self.model.set_input(lcm.motors_on, lcm.roll_cmd, lcm.pitch_cmd, lcm.direct_yaw_rate_commands, 
                  lcm.yaw_cmd, lcm.yaw_rate_cmd, lcm.direct_thrust_commands, lcm.alt_cmd, lcm.thrust_cmd)
        #rospy.loginfo("thrust_cmd = %f, dt = %f" % (self.thrust_cmd, dt))
                    
    def publish_odometry(self):
        odom_msg = Odometry()
        odom_msg.header.stamp = rospy.Time.now()
        odom_msg.header.frame_id = "/ned"
        odom_msg.child_frame_id = "/simflyer1/flyer_imu"
        oppp = odom_msg.pose.pose.position
        oppp.x, oppp.y, oppp.z  = self.model.get_position()
        ottl = odom_msg.twist.twist.linear
        ottl.x, ottl.y, ottl.z = self.model.get_velocity()
        oppo = odom_msg.pose.pose.orientation
        oppo.x, oppo.y, oppo.z, oppo.w = self.model.get_orientation()
        otta = odom_msg.twist.twist.angular
        otta.x, otta.y, otta.z = self.model.get_angular_velocity()
        self.pub_odom.publish(odom_msg)
        
    def publish_state(self, t):
        state_msg = TransformStamped()
        t_ned_imu = tft.translation_matrix(self.model.get_position())
        R_ned_imu = tft.quaternion_matrix(self.model.get_orientation())
        T_ned_imu = tft.concatenate_matrices(t_ned_imu, R_ned_imu)
        #rospy.loginfo("T_ned_imu = \n" + str(T_ned_imu))
        if self.T_imu_vicon is None:
            # grab the static transform from imu to vicon frame from param server:
            frames = rospy.get_param("frames", None)
            ypr = frames['vicon_to_imu']['rotation']
            q_vicon_imu = tft.quaternion_from_euler(*[radians(x) for x in ypr], axes='rzyx') # xyzw
            R_vicon_imu = tft.quaternion_matrix(q_vicon_imu)
            t_vicon_imu = tft.translation_matrix(frames['vicon_to_imu']['translation'])
#            rospy.loginfo(str(R_vicon_imu))
#            rospy.loginfo(str(t_vicon_imu))
            self.T_vicon_imu = tft.concatenate_matrices(t_vicon_imu, R_vicon_imu)
            self.T_imu_vicon = tft.inverse_matrix(self.T_vicon_imu)
            self.T_enu_ned = tft.euler_matrix(radians(90), 0, radians(180), 'rzyx')
            rospy.loginfo(str(self.T_enu_ned))
            rospy.loginfo(str(self.T_imu_vicon))
            #rospy.loginfo(str(T_vicon_imu))
        # we have /ned -> /imu, need to output /enu -> /vicon:
        T_enu_vicon = tft.concatenate_matrices(self.T_enu_ned, T_ned_imu, self.T_imu_vicon )
        state_msg.header.stamp  = t
        state_msg.header.frame_id = "/enu"
        state_msg.child_frame_id = "/simflyer1/flyer_vicon"
        stt = state_msg.transform.translation
        stt.x, stt.y, stt.z = T_enu_vicon[:3,3]
        stro = state_msg.transform.rotation
        stro.x, stro.y, stro.z, stro.w = tft.quaternion_from_matrix(T_enu_vicon)
        
        self.pub_state.publish(state_msg)
        
    def get_transform(self, tgt, src):
        t = self.tfl.getLatestCommonTime(tgt, src)
        t_src_tgt, q_src_tgt = self.tfl.lookupTransform(tgt, src, t)
        T_src_tgt =self.tfl.fromTranslationRotation(t_src_tgt, q_src_tgt)
        return T_src_tgt