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

made failsafe enable when battery level too low

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-06-05 21:41:14 +02:00
parent bcd4f891d2
commit 902adb42f3
1 changed files with 9 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
src/api_communication/api_communication/api_listener.py
View File

@@ -133,6 +133,8 @@ class ApiListener(Node):
try: try:
self.status_data_received = True self.status_data_received = True
self.status_data['battery_percentage'] = msg.battery_percentage self.status_data['battery_percentage'] = msg.battery_percentage
if msg.battery_percentage < 0.15:
self.enable_failsafe("Battery level too low! Failsafe enabled to prevent damage to battery")
self.status_data['cpu_usage'] = msg.cpu_usage self.status_data['cpu_usage'] = msg.cpu_usage
self.status_data['armed'] = msg.armed self.status_data['armed'] = msg.armed
self.armed = msg.armed self.armed = msg.armed
@@ -300,18 +302,21 @@ class ApiListener(Node):
self.get_logger().error( self.get_logger().error(
'Something went wrong while sending a move position request!\n' + str(e)) 'Something went wrong while sending a move position request!\n' + str(e))
def emergency_stop(self): def enable_failsafe(self, message):
"""Sends an emergency stop request to the failsafe service"""
try: try:
self.enable_failsafe_request.message = "Emergency stop activated" self.enable_failsafe_request.message = message
future = self.enable_failsafe_client.call_async(self.enable_failsafe_request) future = self.enable_failsafe_client.call_async(self.enable_failsafe_request)
rclpy.spin_until_future_complete(self, future) rclpy.spin_until_future_complete(self, future)
result = future.result() result = future.result()
if (result.enabled == True): if (result.enabled == True):
self.get_logger().info("Emergency stop activated") self.get_logger().info("Failsafe activated")
except Exception as e: except Exception as e:
self.get_logger().error("Something went wrong while trying to enable failsafe!\n" + str(e)) self.get_logger().error("Something went wrong while trying to enable failsafe!\n" + str(e))
def emergency_stop(self):
"""Sends an emergency stop request to the failsafe service"""
self.enable_failsafe("Emergency stop activated")
def takeoff(self): def takeoff(self):
"""Sends a takeoff request to the move position service""" """Sends a takeoff request to the move position service"""
self.get_logger().info('Takeoff command received') self.get_logger().info('Takeoff command received')