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

add height to drone status

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-06-08 20:43:19 +02:00
parent d1e2b5960e
commit ca2049b0db
2 changed files with 8 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
src/drone_services/msg/DroneStatus.msg
View File

@@ -5,4 +5,5 @@ wstring control_mode
bool armed bool armed
float32[3] velocity float32[3] velocity
float32[3] position float32[3] position
float32 height
bool failsafe bool failsafe

7
src/drone_status/drone_status/drone_status.py
View File

@@ -6,6 +6,7 @@ from drone_services.msg import DroneStatus
from drone_services.msg import DroneControlMode from drone_services.msg import DroneControlMode
from drone_services.msg import DroneArmStatus from drone_services.msg import DroneArmStatus
from drone_services.msg import DroneRouteStatus from drone_services.msg import DroneRouteStatus
from drone_services.msg import HeightData
from px4_msgs.msg import BatteryStatus from px4_msgs.msg import BatteryStatus
from px4_msgs.msg import Cpuload from px4_msgs.msg import Cpuload
from px4_msgs.msg import VehicleOdometry from px4_msgs.msg import VehicleOdometry
@@ -33,6 +34,7 @@ class DroneStatusNode(Node):
DroneArmStatus, '/drone/arm_status', self.arm_status_callback, 10) DroneArmStatus, '/drone/arm_status', self.arm_status_callback, 10)
self.route_status_subscriber = self.create_subscription( self.route_status_subscriber = self.create_subscription(
DroneRouteStatus, '/drone/route_status', self.route_status_callback, 10) DroneRouteStatus, '/drone/route_status', self.route_status_callback, 10)
self.height_data_subscriber = self.create_subscription(HeightData, '/drone/height', self.height_data_callback, 10)
self.battery_status_subscriber = self.create_subscription( self.battery_status_subscriber = self.create_subscription(
BatteryStatus, '/fmu/out/battery_status', self.battery_status_callback, qos_profile=qos_profile) BatteryStatus, '/fmu/out/battery_status', self.battery_status_callback, qos_profile=qos_profile)
self.cpu_load_subscriber = self.create_subscription( self.cpu_load_subscriber = self.create_subscription(
@@ -42,6 +44,7 @@ class DroneStatusNode(Node):
# publish every 0.5 seconds # publish every 0.5 seconds
self.timer = self.create_timer(0.5, self.publish_status) self.timer = self.create_timer(0.5, self.publish_status)
self.armed = False self.armed = False
self.height = 0.0
self.control_mode = "attitude" self.control_mode = "attitude"
self.battery_percentage = 100.0 self.battery_percentage = 100.0
self.cpu_usage = 0.0 self.cpu_usage = 0.0
@@ -58,6 +61,7 @@ class DroneStatusNode(Node):
msg.route_setpoint = self.route_setpoint msg.route_setpoint = self.route_setpoint
msg.position = self.position msg.position = self.position
msg.velocity = self.velocity msg.velocity = self.velocity
msg.height = self.height
self.publisher.publish(msg) self.publisher.publish(msg)
# self.get_logger().info('Publishing status message') # self.get_logger().info('Publishing status message')
@@ -72,6 +76,9 @@ class DroneStatusNode(Node):
else: else:
self.control_mode = "unknown" self.control_mode = "unknown"
def height_data_callback(self, msg):
self.height = msg.min_height
def arm_status_callback(self, msg): def arm_status_callback(self, msg):
self.get_logger().info("Got arm status callback!") self.get_logger().info("Got arm status callback!")
if msg.armed: if msg.armed: