def _handle_motion_timer():
global _set_speed, _velocityhat
if _set_speed:
motion.drive(*_velocities, smooth=_smooth, theta=_thetahat)
_set_speed = False
(vx, vy, w, s1, s2, s3) = motion.get_velocities(theta=_thetahat)
_velocityhat = (vx, vy, w, s1, s2, s3)
def main():
rospy.init_node('motion', anonymous=False)
if rospy.get_param('/simulation_mode', False):
print "[motion] Bye!"
return
# Get this robot's motor's QPPS from rosparam
m1 = rospy.get_param('M1QPPS', None)
m2 = rospy.get_param('M2QPPS', None)
m3 = rospy.get_param('M3QPPS', None)
# Does this robot use v3 or v5 RoboClaws?
use_rcv3 = rospy.get_param('use_rcv3', True)
# init wheelbase
wheelbase.init(m1qpps=m1, m2qpps=m2, m3qpps=m3, use_rcv3=use_rcv3)
# Register a shutdown hook to kill motion
rospy.on_shutdown(_shutdown_hook)
rospy.Subscriber('vel_cmds', Twist, _handle_velocity_command)
pub = rospy.Publisher('encoder_estimates', EncoderEstimates, queue_size=10)
# Services
rospy.Service('battery', RoboClawRPC, _get_battery_voltage)
rospy.Service('kick', Trigger, _kick)
# So that we know the robot's theta
rospy.Subscriber('robot_state', RobotState, _handle_theta)
rospy.spin()
return
rate = rospy.Rate(10) # 100hz
while not rospy.is_shutdown():
(vx, vy, w, s1, s2, s3) = motion.get_velocities(theta=_theta)
estimate = EncoderEstimates()
estimate.world_velocities.vx = vx
estimate.world_velocities.vy = vy
estimate.world_velocities.w = w
estimate.pulses.s1 = s1
estimate.pulses.s2 = s2
estimate.pulses.s3 = s3
pub.publish(estimate)
rate.sleep()
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
