print("a/d: turn left / right")
print("x: Exit program")
while True:
char = getch()
# The Pan-Tilt-Kit turns up.
if (char == "w"):
if tilt.get_current_pwm() > tilt.get_servo_min():
tilt.set_current_pwm(tilt.get_current_pwm() -
tilt.get_intervall_pwm())
printscreen()
# The Pan-Tilt-Kit turns down.
if (char == "s"):
if tilt.get_servo_max() > tilt.get_current_pwm():
tilt.set_current_pwm(tilt.get_current_pwm() +
tilt.get_intervall_pwm())
printscreen()
# The Pan-Tilt-Kit turns to the right.
if (char == "d"):
if pan.get_current_pwm() > pan.get_servo_min():
pan.set_current_pwm(pan.get_current_pwm() -
pan.get_intervall_pwm())
printscreen()
# The Pan-Tilt-Kit turns to the left.
if (char == "a"):
if pan.get_servo_max() > pan.get_current_pwm():
pan.set_current_pwm(pan.get_current_pwm() +
class SteerNode(object):
"""Node example class."""
def __init__(self):
self.conf = ServoConfig()
self.mg996r = ServoMotor(0, self.conf.getNeutral('steering'), self.conf.getMin('steering'), self.conf.getMax('steering'), 5)
# Initialize message variables.
self.enable = False
self.pwm = self.conf.getNeutral('steering')
self.mg996r.set_current_pwm(self.pwm)
self.robot_host = re.sub("-", "_", socket.gethostname())
self.steerSub = rospy.Subscriber(self.robot_host + '/steer/set', Steer, self.steerCallback)
self.steerPWMSub = rospy.Subscriber(self.robot_host + '/steer/set/pwm', Steer, self.steerCallback)
self.steerDegressSub = rospy.Subscriber(self.robot_host + '/steer/set/degree', ServoDeg, self.steerDegreeCallback)
self.steerResetSub = rospy.Subscriber(self.robot_host + '/steer/reset', Bool, self.steerResetCallback)
self.steerPub = rospy.Publisher(self.robot_host + '/steer/get', Steer, queue_size=10)
self.steerPWMPub = rospy.Publisher(self.robot_host + '/steer/get/pwm', Steer, queue_size=10)
self.steerDegreePub = rospy.Publisher(self.robot_host + '/steer/get/degree', ServoDeg, queue_size=10)
self.steerIntervallPub = rospy.Publisher(self.robot_host + '/steer/get/intervall', ServoIntervall, queue_size=10)
self.steerIntervallPWMPub = rospy.Publisher(self.robot_host + '/steer/get/intervall/pwm', ServoIntervall, queue_size=10)
self.steerIntervallDegreePub = rospy.Publisher(self.robot_host + '/steer/get/intervall/degree', ServoIntervallDeg, queue_size=10)
self.steerChannelPub = rospy.Publisher(self.robot_host + '/steer/get/channel', Int8, queue_size=10)
self.steerMinPub = rospy.Publisher(self.robot_host + '/steer/get/min', Steer, queue_size=10)
self.steerMinPWMPub = rospy.Publisher(self.robot_host + '/steer/get/min/pwm', Steer, queue_size=10)
self.steerMinDegreePub = rospy.Publisher(self.robot_host + '/steer/get/min/degree', ServoDeg, queue_size=10)
self.steerMaxPub = rospy.Publisher(self.robot_host + '/steer/get/max', Steer, queue_size=10)
self.steerMaxPWMPub = rospy.Publisher(self.robot_host + '/steer/get/max/pwm', Steer, queue_size=10)
self.steerMaxDegreePub = rospy.Publisher(self.robot_host + '/steer/get/max/degree', ServoDeg, queue_size=10)
self.steerNeutralPub = rospy.Publisher(self.robot_host + '/steer/get/neutral', Steer, queue_size=10)
self.steerNeutralPWMPub = rospy.Publisher(self.robot_host + '/steer/get/neutral/pwm', Steer, queue_size=10)
self.steerNeutralDegreePub = rospy.Publisher(self.robot_host + '/steer/get/neutral/degree', ServoDeg, queue_size=10)
self.steerRangePub = rospy.Publisher(self.robot_host + '/steer/get/range', ServoRange, queue_size=10)
self.steerRangePWMPub = rospy.Publisher(self.robot_host + '/steer/get/range/pwm', ServoRange, queue_size=10)
self.steerRangeDegreePub = rospy.Publisher(self.robot_host + '/steer/get/range/degree', ServoRangeDeg, queue_size=10)
self.rate = rospy.Rate(10) # 10hz
if self.enable:
self.start()
else:
self.stop()
def start(self):
"""Turn on publisher and subscriber."""
self.enable = True
self.steerSub = rospy.Subscriber(self.robot_host + '/steer/set', Steer, self.steerCallback)
self.steerPWMSub = rospy.Subscriber(self.robot_host + '/steer/set/pwm', Steer, self.steerCallback)
self.steerDegressSub = rospy.Subscriber(self.robot_host + '/steer/set/degree', ServoDeg, self.steerDegreeCallback)
self.steerResetSub = rospy.Subscriber(self.robot_host + '/steer/reset', Bool, self.steerResetCallback)
self.steerPub = rospy.Publisher(self.robot_host + '/steer/get', Steer, queue_size=10)
self.steerPWMPub = rospy.Publisher(self.robot_host + '/steer/get/pwm', Steer, queue_size=10)
self.steerDegreePub = rospy.Publisher(self.robot_host + '/steer/get/degree', ServoDeg, queue_size=10)
self.steerIntervallPub = rospy.Publisher(self.robot_host + '/steer/get/intervall', ServoIntervall, queue_size=10)
self.steerIntervallPWMPub = rospy.Publisher(self.robot_host + '/steer/get/intervall/pwm', ServoIntervall, queue_size=10)
self.steerIntervallDegreePub = rospy.Publisher(self.robot_host + '/steer/get/intervall/degree', ServoIntervallDeg, queue_size=10)
self.steerChannelPub = rospy.Publisher(self.robot_host + '/steer/get/channel', Int8, queue_size=10)
self.steerMinPub = rospy.Publisher(self.robot_host + '/steer/get/min', Steer, queue_size=10)
self.steerMinPWMPub = rospy.Publisher(self.robot_host + '/steer/get/min/pwm', Steer, queue_size=10)
self.steerMinDegreePub = rospy.Publisher(self.robot_host + '/steer/get/min/degree', ServoDeg, queue_size=10)
self.steerMaxPub = rospy.Publisher(self.robot_host + '/steer/get/max', Steer, queue_size=10)
self.steerMaxPWMPub = rospy.Publisher(self.robot_host + '/steer/get/max/pwm', Steer, queue_size=10)
self.steerMaxDegreePub = rospy.Publisher(self.robot_host + '/steer/get/max/degree', ServoDeg, queue_size=10)
self.steerNeutralPub = rospy.Publisher(self.robot_host + '/steer/get/neutral', Steer, queue_size=10)
self.steerNeutralPWMPub = rospy.Publisher(self.robot_host + '/steer/get/neutral/pwm', Steer, queue_size=10)
self.steerNeutralDegreePub = rospy.Publisher(self.robot_host + '/steer/get/neutral/degree', ServoDeg, queue_size=10)
self.steerRangePub = rospy.Publisher(self.robot_host + '/steer/get/range', ServoRange, queue_size=10)
self.steerRangePWMPub = rospy.Publisher(self.robot_host + '/steer/get/range/pwm', ServoRange, queue_size=10)
self.steerRangeDegreePub = rospy.Publisher(self.robot_host + '/steer/get/range/degree', ServoRangeDeg, queue_size=10)
while not rospy.is_shutdown():
steerMsg = Steer()
steerDegMsg = ServoDeg()
steerIntervallMsg = ServoIntervall()
steerIntervallDegMsg = ServoIntervallDeg()
steerChannelMsg = Int8()
steerMinMsg = Steer()
steerMinDegMsg = ServoDeg()
steerMaxMsg = Steer()
steerMaxDegMsg = ServoDeg()
steerNeutralMsg = Steer()
steerNeutralDegMsg = ServoDeg()
steerRangeMsg = ServoRange()
steerRangeDegMsg = ServoRangeDeg()
steerMsg.pwm = self.mg996r.get_current_pwm()
steerDegMsg.angle = self.mg996r.get_current_degree()
steerIntervallMsg.intervall_pwm = self.mg996r.get_intervall_pwm()
steerIntervallDegMsg.intervall_angle = self.mg996r.get_intervall_degree()
steerChannelMsg.data = self.mg996r.get_channel()
steerMinMsg.pwm = self.mg996r.get_servo_min()
steerMinDegMsg.angle = self.mg996r.get_servo_min_degree()
steerMaxMsg.pwm = self.mg996r.get_servo_max()
steerMaxDegMsg.angle = self.mg996r.get_servo_max_degree()
steerNeutralMsg.pwm = self.mg996r.get_servo_neutral()
steerNeutralDegMsg.angle = 0.0
steerRangeMsg.range_pwm = self.mg996r.get_servo_range_pwm()
steerRangeDegMsg.range_angle = self.mg996r.get_servo_range_degree()
self.steerPub.publish(steerMsg)
self.steerPWMPub.publish(steerMsg)
self.steerDegreePub.publish(steerDegMsg)
self.steerIntervallPub.publish(steerIntervallMsg)
self.steerIntervallPWMPub.publish(steerIntervallMsg)
self.steerIntervallDegreePub.publish(steerIntervallDegMsg)
self.steerChannelPub.publish(steerChannelMsg)
self.steerMinPub.publish(steerMinMsg)
self.steerMinPWMPub.publish(steerMinMsg)
self.steerMinDegreePub.publish(steerMinDegMsg)
self.steerMaxPub.publish(steerMaxMsg)
self.steerMaxPWMPub.publish(steerMaxMsg)
self.steerMaxDegreePub.publish(steerMaxDegMsg)
self.steerNeutralPub.publish(steerNeutralMsg)
self.steerNeutralPWMPub.publish(steerNeutralMsg)
self.steerNeutralDegreePub.publish(steerNeutralDegMsg)
self.steerRangePub.publish(steerRangeMsg)
self.steerRangePWMPub.publish(steerRangeMsg)
self.steerRangeDegreePub.publish(steerRangeDegMsg)
self.rate.sleep()
def stop(self):
"""Turn off publisher and subscriber."""
self.enable = False
# self.mg996r.reset()
self.steerSub.unregister()
self.steerPWMSub.unregister()
self.steerDegressSub.unregister()
self.steerResetSub.unregister()
self.steerPub.unregister()
self.steerPWMPub.unregister()
self.steerDegreePub.unregister()
self.steerIntervallPub.unregister()
self.steerIntervallPWMPub.unregister()
self.steerIntervallDegreePub.unregister()
self.steerChannelPub.unregister()
self.steerMinPub.unregister()
self.steerMinPWMPub.unregister()
self.steerMinDegreePub.unregister()
self.steerMaxPub.unregister()
self.steerMaxPWMPub.unregister()
self.steerMaxDegreePub.unregister()
self.steerNeutralPub.unregister()
self.steerNeutralPWMPub.unregister()
self.steerNeutralDegreePub.unregister()
self.steerRangePub.unregister()
self.steerRangePWMPub.unregister()
self.steerRangeDegreePub.unregister()
def steerCallback(self, data):
"""Handle subscriber data."""
self.pwm = data.pwm
self.mg996r.set_current_pwm(int(self.pwm))
rospy.loginfo(rospy.get_caller_id() + ' Set steering angle to %s', self.pwm)
def steerDegreeCallback(self, data):
"""Handle subscriber data."""
self.mg996r.set_current_degree(data.angle) # set degree
self.pwm = self.mg996r.get_current_pwm() # get pwm
rospy.loginfo(rospy.get_caller_id() + ' Got steering angle degree %s and set angle to %s' % (data.angle, self.pwm))
def steerResetCallback(self, data):
"""Handle subscriber data."""
if(data.data == True):
self.pwm = self.conf.getNeutral('steering')
self.mg996r.set_current_pwm(self.pwm)
rospy.loginfo(rospy.get_caller_id() + ' Steering reseted to angle %s', self.pwm)
