acker.py 文件源码

def get_angle(self, link, spin_center, steer_angle, stamp):
        # lookup the position of each link in the back axle frame
        ts = self.tf_buffer.lookup_transform(spin_center.header.frame_id, link,
                                             stamp, rospy.Duration(4.0))

        dy = ts.transform.translation.y - spin_center.point.y
        dx = ts.transform.translation.x - spin_center.point.x
        angle = math.atan2(dx, abs(dy))
        if steer_angle < 0:
            angle = -angle

        # visualize the trajectory forward or back of the current wheel
        # given the spin center
        radius = math.sqrt(dx * dx + dy * dy)
        self.marker.points = []
        for pt_angle in numpy.arange(abs(angle) - 1.0, abs(angle) + 1.0 + 0.025, 0.05):
            pt = Point()
            pt.x = spin_center.point.x + radius * math.sin(pt_angle)
            if steer_angle < 0:
                pt.y = spin_center.point.y - radius * math.cos(pt_angle)
            else:
                pt.y = spin_center.point.y + radius * math.cos(pt_angle)
            self.marker.ns = link
            self.marker.header.stamp = stamp
            self.marker.points.append(pt)
        self.marker_pub.publish(self.marker)
        return angle, radius

    # TODO(lucasw) want to receive a joint state that has position
    # and velocity and command a joint_state to achieve it, but ros_control
    # can only take a command for a single value, would need a pvt style
    # interface running upstream of ros_control, or make custom own ros_control
    # controller?