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

add positionchanger wait 10 seconds before checking lidar health

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-06-05 16:50:46 +02:00
parent d80b20e05a
commit 561daf035c
1 changed files with 3 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -256,12 +256,13 @@ public:
*/ */
void check_lidar_health() void check_lidar_health()
{ {
if (!this->received_lidar_message) if (!this->received_lidar_message && !this->failsafe_enabled && this->lidar_health_checks > 10)
{ {
RCLCPP_WARN(this->get_logger(), "Lidar not sending messages, enabling failsafe"); RCLCPP_WARN(this->get_logger(), "Lidar not sending messages, enabling failsafe");
enable_failsafe(u"No healthy connection to LIDAR!"); enable_failsafe(u"No healthy connection to LIDAR!");
} }
this->received_lidar_message = false; this->received_lidar_message = false;
this->lidar_health_checks++;
} }
/** /**
@@ -366,6 +367,7 @@ private:
float collision_prevention_weights[4] = {0}; // the amount to move away from an object in a certain direction if the drone is too close float collision_prevention_weights[4] = {0}; // the amount to move away from an object in a certain direction if the drone is too close
bool failsafe_enabled = false; bool failsafe_enabled = false;
bool received_lidar_message = false; bool received_lidar_message = false;
char lidar_health_checks = 0;
/** /**
* @brief waits for a service to be available * @brief waits for a service to be available