diff --git a/crazyflie_py/crazyflie_py/crazyflie.py b/crazyflie_py/crazyflie_py/crazyflie.py index 062a628b3..6e35c50e2 100644 --- a/crazyflie_py/crazyflie_py/crazyflie.py +++ b/crazyflie_py/crazyflie_py/crazyflie.py @@ -14,7 +14,7 @@ # from .visualizer import visNull -from crazyflie_interfaces.msg import FullState, Position, TrajectoryPolynomialPiece, Status +from crazyflie_interfaces.msg import FullState, Position, Status, TrajectoryPolynomialPiece from crazyflie_interfaces.srv import GoTo, Land,\ NotifySetpointsStop, StartTrajectory, Takeoff, UploadTrajectory from geometry_msgs.msg import Point @@ -136,7 +136,8 @@ def __init__(self, node, cfname, paramTypeDict): self.setParamsService = node.create_client( SetParameters, '/crazyflie_server/set_parameters') self.setParamsService.wait_for_service() - self.statusSubscriber = node.create_subscription(Status, f'{self.prefix}/status', self.status_topic_callback, 10) + self.statusSubscriber = node.create_subscription( + Status, f'{self.prefix}/status', self.status_topic_callback, 10) self.status = {} # Query some settings @@ -700,23 +701,30 @@ def cmdPosition(self, pos, yaw=0.): # self.setParam('ring/solidRed', int(r * 255)) # self.setParam('ring/solidGreen', int(g * 255)) # self.setParam('ring/solidBlue', int(b * 255)) - + def status_topic_callback(self, msg): - """Callback method updating the status attribute every time - a crazyflie_interfaces/msg/Status message is published on the topic /cfXXX/status """ + """ + Callback method updating the status attribute every time a + crazyflie_interfaces/msg/Status message is published on the topic /cfXXX/status - self.status = {'id' : msg.header.frame_id, 'timestamp_sec': msg.header.stamp.sec, - 'timestamp_nsec' : msg.header.stamp.nanosec, 'supervisor' : msg.supervisor_info, - 'battery' : msg.battery_voltage, 'pm_state' : msg.pm_state, 'rssi' : msg.rssi, - 'num_rx_broadcast':msg.num_rx_broadcast, 'num_tx_broadcast' : msg.num_tx_broadcast, - 'num_rx_unicast':msg.num_rx_unicast, 'num_tx_unicast' : msg.num_tx_unicast} + """ + self.status = {'id': msg.header.frame_id, + 'timestamp_sec': msg.header.stamp.sec, + 'timestamp_nsec': msg.header.stamp.nanosec, + 'supervisor': msg.supervisor_info, + 'battery': msg.battery_voltage, + 'pm_state' : msg.pm_state, + 'rssi': msg.rssi, + 'num_rx_broadcast':msg.num_rx_broadcast, + 'num_tx_broadcast': msg.num_tx_broadcast, + 'num_rx_unicast':msg.num_rx_unicast, + 'num_tx_unicast': msg.num_tx_unicast} - def get_status(self): - """Returns the status dictionary containing info about: - frame id, timestamp, supervisor info, battery voltage, pm state, rssi, number of received or - transmitted broadcast or unicast messages. see crazyflie_interfaces/msg/Status for details""" - + """Returns the status dictionary containing info about: + frame id, timestamp, supervisor info, battery voltage, pm state, rssi, nb of received or + transmitted broadcast or unicast messages. see crazyflie_interfaces/msg/Status for details + """ # self.node.get_logger().info(f'Crazyflie.get_status() was called {self.status}') return self.status