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

add waiting on first lidar message

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-06-05 17:04:57 +02:00
parent 4916584e13
commit c967eea3f2
1 changed files with 11 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -262,8 +262,12 @@ public:
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->has_received_first_lidar_message = true;
if (this->lidar_health_checks <= 10)
{
this->lidar_health_checks++; this->lidar_health_checks++;
} }
}
/** /**
* @brief Callback function for receiving the odometry data from the drone. * @brief Callback function for receiving the odometry data from the drone.
@@ -292,6 +296,10 @@ public:
{ {
if (!this->failsafe_enabled) if (!this->failsafe_enabled)
{ {
if (!this->has_received_first_lidar_message) {
this->enable_failsafe(u"Waiting for LIDAR timed out! Consider restarting the drone.");
return;
}
RCLCPP_INFO(this->get_logger(), "Incoming request\nfront_back: %f\nleft_right: %f\nup_down: %f\nangle: %f", request->front_back, request->left_right, request->up_down, request->angle); RCLCPP_INFO(this->get_logger(), "Incoming request\nfront_back: %f\nleft_right: %f\nup_down: %f\nangle: %f", request->front_back, request->left_right, request->up_down, request->angle);
if (request->angle > 360 || request->angle < -360) if (request->angle > 360 || request->angle < -360)
{ {
@@ -367,7 +375,8 @@ 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; int lidar_health_checks = 0;
bool has_received_first_lidar_message = false;
/** /**
* @brief waits for a service to be available * @brief waits for a service to be available