cap_to_obs_tf.py 文件源码

def handle_msg_car(msg, who):
    assert isinstance(msg, Odometry)

    global last_cap_r, last_cap_f, last_cap_yaw

    if who == 'cap_r':
        last_cap_r = rtk_position_to_numpy(msg)
    elif who == 'cap_f' and last_cap_r is not None:
        cap_f = rtk_position_to_numpy(msg)
        cap_r = last_cap_r

        last_cap_f = cap_f
        last_cap_yaw = get_yaw(cap_f, cap_r)
    elif who == 'obs_r' and last_cap_f is not None and last_cap_yaw is not None:
        md = None
        for obs in metadata:
            if obs['obstacle_name'] == 'obs1':
                md = obs
        assert md, 'obs1 metadata not found'

        # find obstacle rear RTK to centroid vector
        lrg_to_gps = [md['rear_gps_l'], -md['rear_gps_w'], md['rear_gps_h']]
        lrg_to_centroid = [md['l'] / 2., -md['w'] / 2., md['h'] / 2.]
        obs_r_to_centroid = np.subtract(lrg_to_centroid, lrg_to_gps)

        # in the fixed GPS frame 
        cap_f = last_cap_f
        obs_r = rtk_position_to_numpy(msg)

        # in the capture vehicle front RTK frame
        cap_to_obs = np.dot(rotMatZ(-last_cap_yaw), obs_r - cap_f)
        cap_to_obs_centroid = cap_to_obs + obs_r_to_centroid

        br = tf.TransformBroadcaster()
        br.sendTransform(tuple(cap_to_obs_centroid), (0,0,0,1), rospy.Time.now(), 'obs_centroid', 'gps_antenna_front')

        # publish obstacle bounding box
        marker = Marker()
        marker.header.frame_id = "obs_centroid"
        marker.header.stamp = rospy.Time.now()

        marker.type = Marker.CUBE
        marker.action = Marker.ADD

        marker.scale.x = md['l']
        marker.scale.y = md['w']
        marker.scale.z = md['h']

        marker.color.r = 0.2
        marker.color.g = 0.5
        marker.color.b = 0.2
        marker.color.a = 0.5

        marker.lifetime = rospy.Duration()

        pub = rospy.Publisher("obs_bbox", Marker, queue_size=10)
        pub.publish(marker)