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?
评论列表
文章目录