diff --git a/src/drone_status/drone_status/drone_status.py b/src/drone_status/drone_status/drone_status.py index ab8046af..4e98f77e 100644 --- a/src/drone_status/drone_status/drone_status.py +++ b/src/drone_status/drone_status/drone_status.py @@ -12,24 +12,31 @@ CONTROL_MODE_ATTITUDE = 1 CONTROL_MODE_VELOCITY = 2 CONTROL_MODE_POSITION = 3 + class DroneStatus(Node): def __init__(self): super().__init__('drone_status') - #publish to drone/status topic - self.publisher = self.create_publisher(DroneStatus, '/drone/status', 10) - self.control_mode_subscriber = self.create_subscription(DroneControlMode, '/drone/control_mode', self.control_mode_callback, 10) - self.arm_status_subscriber = self.create_subscription(DroneArmStatus, '/drone/arm_status', self.arm_status_callback, 10) - self.route_status_subscriber = self.create_subscription(DroneRouteStatus, '/drone/route_status', self.route_status_callback, 10) - self.battery_status_subscriber = self.create_subscription(BatteryStatus, '/fmu/out/battery_status', self.battery_status_callback, 10) - self.cpu_load_subscriber = self.create_subscription(Cpuload, '/fmu/out/cpuload', self.cpu_load_callback, 10) - #publish every 0.5 seconds + # publish to drone/status topic + self.publisher = self.create_publisher( + DroneStatus, '/drone/status', 10) + self.control_mode_subscriber = self.create_subscription( + DroneControlMode, '/drone/control_mode', self.control_mode_callback, 10) + self.arm_status_subscriber = self.create_subscription( + DroneArmStatus, '/drone/arm_status', self.arm_status_callback, 10) + self.route_status_subscriber = self.create_subscription( + DroneRouteStatus, '/drone/route_status', self.route_status_callback, 10) + self.battery_status_subscriber = self.create_subscription( + BatteryStatus, '/fmu/out/battery_status', self.battery_status_callback, 10) + self.cpu_load_subscriber = self.create_subscription( + Cpuload, '/fmu/out/cpuload', self.cpu_load_callback, 10) + # publish every 0.5 seconds self.timer = self.create_timer(0.5, self.publish_status) self.armed = False self.control_mode = "attitude" self.battery_percentage = 100.0 self.cpu_usage = 0.0 self.route_setpoint = 0 - + def publish_status(self): msg = DroneStatus() msg.armed = self.armed @@ -40,7 +47,7 @@ class DroneStatus(Node): self.publisher.publish(msg) self.get_logger().info('Publishing: "%s"' % msg.status) - def control_mode_callback(self,msg): + def control_mode_callback(self, msg): if msg.control_mode == CONTROL_MODE_ATTITUDE: self.control_mode = "attitude" elif msg.control_mode == CONTROL_MODE_VELOCITY: @@ -50,14 +57,29 @@ class DroneStatus(Node): else: self.control_mode = "unknown" - def arm_status_callback(self,msg): + def arm_status_callback(self, msg): self.armed = msg.armed - - def route_status_callback(self,msg): + + def route_status_callback(self, msg): self.route_setpoint = msg.current_setpoint_index def battery_status_callback(self, msg): self.battery_percentage = msg.remaining * 100.0 - + def cpu_load_callback(self, msg): - self.cpu_usage = msg.load \ No newline at end of file + self.cpu_usage = msg.load + + +def main(args=None): + rclpy.init(args=args) + + drone_status = DroneStatus() + + rclpy.spin(drone_status) + + drone_status.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main()