def subscribe(self):
subscribe_to_topic(c.HEAD, c.ID_PAN_ANGLE, self.on_pan_angle_update)
subscribe_to_topic(c.HEAD, c.ID_TILT_ANGLE, self.on_tilt_angle_update)
rospy.Subscriber('/qr_tracker/matches', Percept, self.on_qr_match)
def __init__(self, name):
self.running = True
self.motor_speed = c.MOTOR_EIGTH_SPEED
self._lock = RLock()
self._last_action = None, time.time() # action, last set
self.edge_states = [0] * 3
self.bumper_states = [0] * 3
self.ultrasonic_states = [Buffer(length=1) for _ in range(3)]
self.last_edge_update = 0
self.last_bumper_update = 0
self.last_ultrasonic_update = 0
self.ultrasonic_update_start = time.time()
self.ultrasonic_update_count = 0
self.enable_ultrasonics()
subscribe_to_topic(c.TORSO, c.ID_EDGE, self.on_edge_update)
subscribe_to_topic(c.TORSO, c.ID_BUMPER, self.on_bumper_update)
subscribe_to_topic(c.TORSO, c.ID_ULTRASONIC, self.on_ultrasonic_update)
subscribe_to_topic(c.TORSO, c.ID_STATUS_BUTTON,
self.on_status_button_update)
#TODO:accel/gyro?
rospy.on_shutdown(self.shutdown)
print 'Starting servers...'
self.wander_server = actionlib.SimpleActionServer(
c.MOTION_WANDER, ros_homebot.msg.WanderAction, self.execute_wander,
False)
self.wander_server.start()
self.forward_server = actionlib.SimpleActionServer(
c.MOTION_FORWARD_X_MM, ros_homebot.msg.ForwardAction,
self.execute_forward, False)
self.forward_server.start()
self.turn_server = actionlib.SimpleActionServer(
c.MOTION_TURN_X_DEGREES, ros_homebot.msg.TurnAction,
self.execute_turn, False)
self.turn_server.start()
print 'Ready!'
def _subscribe(self, packet_id, device):
assert device in (c.HEAD, c.TORSO)
msg_type = packet_to_message_type(packet_id)
self.handlers[msg_type] = MessageHandler()
subscribe_to_topic(device, packet_id, self.record_topic)
def __init__(self):
rospy.init_node('homebot_teleop', log_level=rospy.DEBUG)
self.verbose = int(rospy.get_param("~verbose", 0))
self.rate = int(rospy.get_param("~rate", 50))
r = rospy.Rate(self.rate)
subscribe_to_topic(c.HEAD, c.ID_ARDUINO_TEMP,
self.packet_read_callback)
subscribe_to_topic(c.HEAD, c.ID_PAN_ANGLE, self.packet_read_callback)
subscribe_to_topic(c.HEAD, c.ID_PONG, self.packet_read_callback)
#subscribe_to_topic(c.TORSO, c.ID_ACCELGYRO, self.packet_read_callback)
subscribe_to_topic(c.TORSO, c.ID_ARDUINO_TEMP,
self.packet_read_callback)
subscribe_to_topic(c.TORSO, c.ID_BATTERY_CHARGE_RATIO,
self.packet_read_callback)
subscribe_to_topic(c.TORSO, c.ID_BATTERY_TEMP,
self.packet_read_callback)
subscribe_to_topic(c.TORSO, c.ID_BATTERY_VOLTAGE,
self.packet_read_callback)
subscribe_to_topic(c.TORSO, c.ID_BUMPER, self.packet_read_callback)
subscribe_to_topic(c.TORSO, c.ID_EDGE, self.packet_read_callback)
subscribe_to_topic(c.TORSO, c.ID_EXTERNAL_POWER,
self.packet_read_callback)
subscribe_to_topic(c.TORSO, c.ID_PONG, self.packet_read_callback)
subscribe_to_topic(c.TORSO, c.ID_ULTRASONIC, self.packet_read_callback)
rospy.Subscriber('/system_node/cpu', msgs.CPUUsage,
self.packet_read_callback)
rospy.Subscriber('/system_node/disk', msgs.DiskUsage,
self.packet_read_callback)
rospy.Subscriber('/system_node/memory', msgs.MemoryUsage,
self.packet_read_callback)
self.server_thread = ServerThread()
self.server_thread.start()
# Cleanup when termniating the node
rospy.on_shutdown(self.shutdown)
# Start polling the sensors and base controller
while not rospy.is_shutdown():
# Publish all sensor values on a single topic for convenience
# now = rospy.Time.now()
#
# r.sleep()
time.sleep(1)