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

52
src/drone_status/drone_status/drone_status.py
View File

@@ -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
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()