def __init__(self): rospy.init_node('fable_connection', anonymous=True, disable_signals=True) self.soundhandle = SoundClient() self.manager = None self.motor_last_sent = {} try: self.manager = ROSEmbeddedManager( self.setup, SerialTransport( port=rospy.get_param('~port', get_usb()), baud=rospy.get_param('~baudrate', '57600') ) ) rospy.Subscriber('/posture_detected', PostureDetected, self.posture_detected_cb) r = rospy.Rate(5) while not rospy.is_shutdown(): try: self.manager.act() r.sleep() except KeyboardInterrupt: self.manager.stop() break except: if self.manager is not None: self.manager.stop() raise
class FableConnectionNode: def __init__(self): rospy.init_node('fable_connection', anonymous=True, disable_signals=True) self.soundhandle = SoundClient() self.manager = None self.motor_last_sent = {} try: self.manager = ROSEmbeddedManager( self.setup, SerialTransport( port=rospy.get_param('~port', get_usb()), baud=rospy.get_param('~baudrate', '57600') ) ) rospy.Subscriber('/posture_detected', PostureDetected, self.posture_detected_cb) r = rospy.Rate(5) while not rospy.is_shutdown(): try: self.manager.act() r.sleep() except KeyboardInterrupt: self.manager.stop() break except: if self.manager is not None: self.manager.stop() raise def echo_cb(self, message): """Callback for echo message.""" rospy.loginfo('ECHO: ' + str(message.data)) def debug_cb(self, response): """Callback for clear text messages.""" rospy.loginfo('DEBUG: %s' % str(response)) def say_cb(self, message): """Callback for say message.""" rospy.loginfo("Say: %s" % message.array) if self.soundhandle is not None: self.soundhandle.say(message.array) def posture_detected_cb(self, data): """Callback for ROS /posture_detected message.""" if self.manager is not None and posture_table.has_key(data.posture): self.manager.send( msgs.posture_detected, user_id=data.user_id, posture=posture_table[data.posture] ) def create_motor_callback(self, i, desc): """Create a callback function for ROS /motor_N/command topic.""" def callback(data): d = self.manager.convert_ROS_to_embedded(desc, data) d['motor_id'] = i pos = d['pos'] if motor_last_sent[i] is not None and \ (pos < motor_last_sent[i] - PM_FILTER or \ self.motor_last_sent[i] + PM_FILTER < pos) or \ self.motor_last_sent[i] is None: self.motor_last_sent[i] = pos self.manager.send(msgs.motor_command, **d) return callback def motor_subscription_cb(self, message): """Callback for motor subscription (embedded) message.""" desc = self.manager.get_message_description(msgs.motor_command) self.manager.link( msgs.motor_command, Float64, field_map={'pos': 'data'}, ros_emb_conv={'pos': conv_motor_command_pos} ) for i in range(0, message.motor_num): self.motor_last_sent[i] = None rospy.Subscriber( '/motor_%d/command' % i, Float64, self.create_motor_callback(i, desc) ) rospy.loginfo("Motor command subscribed! %d motor(s)." % message.motor_num); def setup(self, obj): """Setup links between ROS messages and embedded messages.""" obj.register_messages(messages_collection) obj.add_field_table(field_table) obj.add_field_conv_table(field_conv_table) obj.link(msgs.echo, String, '/echo_in', '/echo_out') obj.subscribe(msgs.echo, self.echo_cb) obj.subscribe(None, self.debug_cb) obj.link(msgs.user_detected, Int32, '/user_detected', field_map={'user_id': 'data'}) obj.link(msgs.user_lost, Int32, '/user_lost', field_map={'user_id': 'data'}) obj.link(msgs.user_recognized, UserRecognized, '/user_recognized') obj.link(msgs.user_pos, UserPos, '/user_pos') obj.link(msgs.user_ang, UserAng, '/user_ang', ros_emb_conv={'array': conv_ang_list_to_raw}) obj.subscribe(msgs.say, self.say_cb) obj.subscribe(msgs.motor_subscription, self.motor_subscription_cb)