def main():
print(" +--------------------------------------------------+")
print(" | XBee Python Library Explicit Data Polling Sample |")
print(" +--------------------------------------------------+\n")
device = ZigBeeDevice(PORT, BAUD_RATE)
try:
device.open()
device.set_api_output_mode(APIOutputMode.EXPLICIT)
device.flush_queues()
print("Waiting for data in explicit format...\n")
while True:
explicit_xbee_message = device.read_expl_data(None)
if explicit_xbee_message is not None:
print("From %s >> %s"
% (explicit_xbee_message.remote_device.get_64bit_addr(),
explicit_xbee_message.data.decode()))
print(" - Source endpoint: %s"
% utils.hex_to_string(utils.int_to_length(explicit_xbee_message.source_endpoint)))
print(" - Destination endpoint: %s"
% utils.hex_to_string(utils.int_to_length(explicit_xbee_message.dest_endpoint)))
print(" - Cluster ID: %s"
% utils.hex_to_string(utils.int_to_length(explicit_xbee_message.cluster_id)))
print(" - Profile ID: %s"
% utils.hex_to_string(utils.int_to_length(explicit_xbee_message.profile_id)))
finally:
if device is not None and device.is_open():
device.close()
class xbee_read():
def __init__(self):
rospy.init_node("xbee_read")
PORT = rospy.get_param("~device", "/dev/ttyUSB1")
BAUD_RATE = rospy.get_param("~baud", 115200)
self.joy_msg = Joy()
self.device = ZigBeeDevice(PORT, BAUD_RATE)
hz = rospy.Rate(10)
self.pub = rospy.Publisher("joy", Joy, queue_size=10)
try:
self.device.open()
except Exception as exc:
rospy.logerr(exc)
if self.device.is_open:
self.device.close()
return
self.device.flush_queues()
self.device.set_api_output_mode(APIOutputMode.EXPLICIT)
rospy.sleep(rospy.Duration.from_sec(2.0))
rospy.on_shutdown(self.shutdown_handle)
while not rospy.is_shutdown():
try:
data_msg = self.device.read_expl_data()
except TimeoutException:
rospy.logerr("Timeout!")
except InvalidPacketException:
rospy.logerr("Bad Checksum")
except XBeeException as exc:
rospy.logerr("XBee Error!: {}".format(exc))
else:
if data_msg:
self.handle_data(data_msg)
hz.sleep()
def shutdown_handle(self):
if self.device._is_open:
self.device.close()
def handle_data(self, data_msg):
if len(data_msg.data) == 16:
formatspec = '>cchhhhh????'
self.parse_joystick(struct.unpack(formatspec, data_msg.data))
elif len(data_msg.data) == 3:
formatspec = 'cc?'
self.parse_commands(struct.unpack(formatspec, data_msg.data))
def parse_joystick(self, data):
rospy.loginfo(data[2:7])
self.joy_msg.axes = data[2:7]
self.joy_msg.buttons = data[7:]
rospy.logdebug(data)
self.joy_msg.header.stamp = rospy.Time.now()
self.joy_msg.header.frame_id = "override"
self.pub.publish(self.joy_msg)
def parse_commands(self, data):
cmd = data[0] + data[1]
if cmd == 'st':
if data[2]:
rospy.loginfo("Start Mission Request")
try:
rospy.wait_for_service("supervisor/start", 2.0)
except:
rospy.logerr("No supervisor/start service available.")
else:
service_handle = rospy.ServiceProxy(
"supervisor/start", Trigger)
response = service_handle()
if response.success:
rospy.loginfo(
"Successful start mission request (xbee)")
else:
rospy.logerr(response.message)
else:
rospy.loginfo("Stop Mission Request")
try:
rospy.wait_for_service("supervisor/pause", 2.0)
except:
rospy.logerr("No supervisor/pause service available.")
else:
service_handle = rospy.ServiceProxy(
"supervisor/pause", SetBool)
response = service_handle(True)
if response.success:
rospy.loginfo(
"Successful pause mission request (xbee)")
else:
rospy.logerr(response.message)
elif cmd == 'rt':
if data[2]:
rospy.loginfo("Return Home Request")
else:
rospy.loginfo("Resume Mission Request")
try:
rospy.wait_for_service("supervisor/pause", 2.0)
except:
rospy.logerr("No supervisor/pause service available.")
else:
service_handle = rospy.ServiceProxy(
"supervisor/pause", SetBool)
response = service_handle(False)
if response.success:
rospy.loginfo(
"Successful resume mission request (xbee)")
else:
rospy.logerr(response.message)
else:
rospy.logwarn("Unknown Command: {}".format(cmd))
