odometry_test.py 文件源码

def process_msg(msg, who):
    msg._type == 'nav_msgs/Odometry'

    global last_rear, last_front, last_yaw, last_front_t

    if who == 'rear':
        last_rear = get_position_from_odometry(msg)
    elif who == 'front' and last_rear is not None:
        cur_front = get_position_from_odometry(msg)
        last_yaw = get_yaw(cur_front, last_rear)

        twist_lin = np.dot(rotMatZ(-last_yaw), get_speed_from_odometry(msg))

        if last_front is not None:
            dt = msg.header.stamp.to_sec() - last_front_t
            speed = (cur_front - last_front)/dt
            speed = np.dot(rotMatZ(-last_yaw), speed)
            print '1', speed
            print '2', twist_lin
            print '3', np.sqrt((speed-twist_lin).dot(speed-twist_lin))

            odo = Odometry()
            odo.header.frame_id = '/base_link'
            odo.header.stamp = rospy.Time.now()
            speed_yaw = get_yaw([0,0,0], -speed) #[-speed[0],-speed[1]*100,0])
            speed_yaw_q = ros_tf.transformations.quaternion_from_euler(0, 0, speed_yaw)
            odo.pose.pose.orientation = Quaternion(*list(speed_yaw_q))
            #odo.twist.twist.linear = Point(x=speed[0], y=speed[1], z=speed[2])
            odo.twist.covariance = list(np.eye(6,6).reshape(1,-1).squeeze())
            pub = rospy.Publisher('odo_speed', Odometry, queue_size=1).publish(odo)

        #print last_yaw
        last_front = cur_front
        last_front_t = msg.header.stamp.to_sec()

        return twist_lin