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)
