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()