sem/5g_drone_ROS2
1
0
Fork 0
Code Issues 1 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

add main:

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-22 14:23:47 +02:00
parent e573c4a658
commit 426b8c3d6f
1 changed files with 37 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
src/drone_status/drone_status/drone_status.py
View File

@@ -12,16 +12,23 @@ 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)
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
@@ -61,3 +68,18 @@ class DroneStatus(Node):
def cpu_load_callback(self, msg):
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()