def __init__(self):
self.rate = rospy.get_param("~rate", 20.0)
self.period = 1.0 / self.rate
# angular mode maps angular z directly to steering angle
# (adjusted appropriately)
# non-angular mode is somewhat suspect, but it turns
# a linear y into a command to turn just so that the
# achieved linear x and y match the desired, though
# the vehicle has to turn to do so.
self.angular_mode = rospy.get_param("~angular_mode", True)
self.tf_buffer = tf2_ros.Buffer()
self.tf = tf2_ros.TransformListener(self.tf_buffer)
self.steer_link = rospy.get_param("~steer_link", "lead_steer")
self.steer_joint = rospy.get_param("~steer_joint", "lead_steer_joint")
# +/- this angle
self.min_steer_angle = rospy.get_param("~min_steer_angle", -0.7)
self.max_steer_angle = rospy.get_param("~max_steer_angle", 0.7)
self.wheel_joint = rospy.get_param("~wheel_joint", "wheel_lead_axle")
self.wheel_radius = rospy.get_param("~wheel_radius", 0.15)
# the spin center is always on the fixed axle y axis of the fixed axle,
# it is assume zero rotation on the steer_joint puts the steering
# at zero rotation with respect to fixed axle x axis (or xz plane)
self.fixed_axle_link = rospy.get_param("~fixed_axle_link", "back_axle")
self.point_pub = rospy.Publisher("cmd_vel_spin_center", PointStamped, queue_size=1)
self.steer_pub = rospy.Publisher("steer_joint_states", JointState, queue_size=1)
# TODO(lucasw) is there a way to get TwistStamped out of standard
# move_base publishers?
self.joint_state = JointState()
self.joint_state.name.append(self.steer_joint)
self.joint_state.position.append(0.0)
self.joint_state.velocity.append(0.0)
self.joint_state.name.append(self.wheel_joint)
self.joint_state.position.append(0.0)
self.joint_state.velocity.append(0.0)
self.cmd_vel = Twist()
rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback, queue_size=2)
self.timer = rospy.Timer(rospy.Duration(self.period), self.update)
评论列表
文章目录
