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

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

@@ -256,17 +256,16 @@ public:
*/
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");
enable_failsafe(u"No healthy connection to LIDAR! Check the LIDAR USB cable and restart the drone.");
if (!this->received_lidar_message)
{
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->has_received_first_lidar_message = true;
if (this->lidar_health_checks <= 20)
{
this->lidar_health_checks++;
}
}
/**