minMetersPerSecond = rospy.get_param("minMetersPerSecond", 0.1) motorUpdateHz = rospy.get_param("motorUpdateHz", 10.0) rightMotorDirectionSign = rospy.get_param("rightMotorDirectionSign", 1) leftMotorDirectionSign = rospy.get_param("leftMotorDirectionSign", -1) leftMotorId = rospy.get_param("leftMotorId", 0) rightMotorId = rospy.get_param("rightMotorId", 1) rospy.loginfo("Max m/s: %0.3f, Min m/s: %0.3f, Hz: %0.3f" % ( maxMetersPerSecond, minMetersPerSecond, motorUpdateHz ) ) motorController = PhidgetMotorController( leftMotorId, rightMotorId, leftMotorDirectionSign, rightMotorDirectionSign ) motorController.setDefaultSpeed(75.0) rospy.Subscriber('lmotor', Float32, handleLeftMotorMessage) rospy.Subscriber('rmotor', Float32, handleRightMotorMessage) rate = rospy.Rate(motorUpdateHz) while not rospy.is_shutdown(): motorController.motorsDirect( leftVelocity, rightVelocity ) rate.sleep()
return def check_for_outdated_cmd_vel(): global lastCmdVelTime now = rospy.Time.now().to_sec() latency = now - lastCmdVelTime if latency > 2.0: rospy.logdebug("cmd_vel command is %.2f seconds old! Stopping!" % latency) motorController.move(0.0, 0.0) if __name__ == '__main__': rospy.init_node('base_controller') rospy.loginfo("Initializing base_controller node") motorController = PhidgetMotorController() motorController.setDefaultSpeed(75.0) lastCmdVelTime = rospy.Time.now().to_sec() rospy.Subscriber('cmd_vel', Twist, handleTwistMessage) rate = rospy.Rate(10.0) while not rospy.is_shutdown(): check_for_outdated_cmd_vel() rate.sleep() motorController.move(0.0, 0.0)