add drone status node

src/drone_services/msg/DroneArmStatus.msg

@@ -0,0 +1 @@
+bool armed false

src/drone_services/msg/DroneRouteStatus.msg

@@ -0,0 +1,2 @@
+int32 current_setpoint_index
+float32[5] current_setpoint # x,y,z,angle,take_picture

src/drone_services/msg/DroneStatus.msg

@@ -0,0 +1,5 @@
+float32 battery_percentage
+float32 cpu_usage
+int32 route_setpoint # -1 if no route
+wstring control_mode
+bool armed

src/drone_status/drone_status/drone_status.py

@@ -0,0 +1,63 @@
+import rclpy
+from rclpy.node import Node
+
+from drone_services.msg import DroneStatus
+from drone_services.msg import DroneControlMode
+from drone_services.msg import DroneArmStatus
+from drone_services.msg import DroneRouteStatus
+from px4_msgs.msg import BatteryStatus
+from px4_msgs.msg import Cpuload
+
+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
+        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
+        msg.control_mode = self.control_mode
+        msg.battery_percentage = self.battery_percentage
+        msg.cpu_usage = self.cpu_usage
+        msg.route_setpoint = self.route_setpoint
+        self.publisher.publish(msg)
+        self.get_logger().info('Publishing: "%s"' % msg.status)
+
+    def control_mode_callback(self,msg):
+        if msg.control_mode == CONTROL_MODE_ATTITUDE:
+            self.control_mode = "attitude"
+        elif msg.control_mode == CONTROL_MODE_VELOCITY:
+            self.control_mode = "velocity"
+        elif msg.control_mode == CONTROL_MODE_POSITION:
+            self.control_mode = "position"
+        else:
+            self.control_mode = "unknown"
+
+    def arm_status_callback(self,msg):
+        self.armed = msg.armed
+    
+    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

src/drone_status/package.xml

@@ -1,18 +1,21 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>drone_status</name>
-  <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="semmer99@gmail.com">ubuntu</maintainer>
-  <license>TODO: License declaration</license>
+    <name>drone_status</name>
+    <version>0.0.0</version>
+    <description>Package for combining several data points from the drone into a single topic</description>
+    <maintainer email="semmer99@gmail.com">ubuntu</maintainer>
+    <license>Apache License 2.0</license>
+    <exec_depend>rclpy</exec_depend>
+    <exec_depend>drone_services</exec_depend>
+    <exec_depend>px4_msgs</exec_depend>
 
-  <test_depend>ament_copyright</test_depend>
-  <test_depend>ament_flake8</test_depend>
-  <test_depend>ament_pep257</test_depend>
-  <test_depend>python3-pytest</test_depend>
+    <test_depend>ament_copyright</test_depend>
+    <test_depend>ament_flake8</test_depend>
+    <test_depend>ament_pep257</test_depend>
+    <test_depend>python3-pytest</test_depend>
 
-  <export>
-    <build_type>ament_python</build_type>
-  </export>
-</package>
+    <export>
+        <build_type>ament_python</build_type>
+    </export>
+</package>