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

try fix lidar taking long to send messages

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-06-05 17:51:09 +02:00
parent cbf01d7875
commit b7037a3e6f
2 changed files with 10 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -104,6 +104,7 @@ class ApiListener(Node):
self.event_loop = None self.event_loop = None
self.armed = False self.armed = False
self.failsafe_enabled = False
def wait_for_service(self,client,service_name): def wait_for_service(self,client,service_name):
"""Waits for a client service to be available """Waits for a client service to be available
@@ -147,6 +148,9 @@ class ApiListener(Node):
Args: Args:
msg (FailSAfe): The message that was received msg (FailSAfe): The message that was received
""" """
if self.failsafe_enabled:
return
self.status_data['failsafe'] = msg.enabled self.status_data['failsafe'] = msg.enabled
self.message_queue.append(json.dumps( self.message_queue.append(json.dumps(
{'type': ResponseMessage.FAILSAFE.name, 'message': msg.msg})) {'type': ResponseMessage.FAILSAFE.name, 'message': msg.msg}))

13
src/drone_controls/src/PositionChanger.cpp
View File

@@ -256,17 +256,16 @@ public:
*/ */
void check_lidar_health() void check_lidar_health()
{ {
if (!this->received_lidar_message && !this->failsafe_enabled && this->lidar_health_checks > 20) if (this->has_received_first_lidar_message)
{ {
RCLCPP_WARN(this->get_logger(), "Lidar not sending messages, enabling failsafe"); if (!this->received_lidar_message)
enable_failsafe(u"No healthy connection to LIDAR! Check the LIDAR USB cable and restart the drone."); {
RCLCPP_WARN(this->get_logger(), "Lidar not sending messages, enabling failsafe");
enable_failsafe(u"No healthy connection to LIDAR! Check the LIDAR USB cable and restart the drone.");
}
} }
this->received_lidar_message = false; this->received_lidar_message = false;
this->has_received_first_lidar_message = true; this->has_received_first_lidar_message = true;
if (this->lidar_health_checks <= 20)
{
this->lidar_health_checks++;
}
} }
/** /**