From e2db92a1ea4b0e225bb3873c322bcf5994e69241 Mon Sep 17 00:00:00 2001 From: GuillaumeLaine Date: Wed, 13 Dec 2023 14:15:55 +0100 Subject: [PATCH] navigation: log error only once, add exception handling in examples --- .../include/global_navigation.hpp | 12 +++++- .../include/local_navigation.hpp | 12 +++++- .../global_position_measurement_interface.cpp | 16 +++++--- .../local_position_measurement_interface.cpp | 38 ++++++++++--------- 4 files changed, 51 insertions(+), 27 deletions(-) diff --git a/examples/cpp/navigation/global_navigation/include/global_navigation.hpp b/examples/cpp/navigation/global_navigation/include/global_navigation.hpp index e7f7444..17b8213 100644 --- a/examples/cpp/navigation/global_navigation/include/global_navigation.hpp +++ b/examples/cpp/navigation/global_navigation/include/global_navigation.hpp @@ -34,8 +34,16 @@ class GlobalNavigationTest : public px4_ros2::GlobalPositionMeasurementInterface global_position_measurement.altitude_msl = 12.4F; global_position_measurement.vertical_variance = 0.2F; - update(global_position_measurement); - RCLCPP_DEBUG(_node.get_logger(), "Successfully sent position update to navigation interface."); + try { + update(global_position_measurement); + RCLCPP_DEBUG( + _node.get_logger(), + "Successfully sent position update to navigation interface."); + } catch (const px4_ros2::NavigationInterfaceInvalidArgument & e) { + RCLCPP_ERROR_THROTTLE( + _node.get_logger(), + *_node.get_clock(), 1000, "Exception caught: %s", e.what()); + } } private: diff --git a/examples/cpp/navigation/local_navigation/include/local_navigation.hpp b/examples/cpp/navigation/local_navigation/include/local_navigation.hpp index 2231d29..85c2289 100644 --- a/examples/cpp/navigation/local_navigation/include/local_navigation.hpp +++ b/examples/cpp/navigation/local_navigation/include/local_navigation.hpp @@ -38,8 +38,16 @@ class LocalNavigationTest : public px4_ros2::LocalPositionMeasurementInterface local_position_measurement.attitude_quaternion = Eigen::Quaternionf {0.1, -0.2, 0.3, 0.25}; local_position_measurement.attitude_variance = Eigen::Vector3f {0.2, 0.1, 0.05}; - update(local_position_measurement); - RCLCPP_DEBUG(_node.get_logger(), "Successfully sent position update to navigation interface."); + try { + update(local_position_measurement); + RCLCPP_DEBUG( + _node.get_logger(), + "Successfully sent position update to navigation interface."); + } catch (const px4_ros2::NavigationInterfaceInvalidArgument & e) { + RCLCPP_ERROR_THROTTLE( + _node.get_logger(), + *_node.get_clock(), 1000, "Exception caught: %s", e.what()); + } } private: diff --git a/px4_ros2_cpp/src/navigation/experimental/global_position_measurement_interface.cpp b/px4_ros2_cpp/src/navigation/experimental/global_position_measurement_interface.cpp index b769d80..8c175d7 100644 --- a/px4_ros2_cpp/src/navigation/experimental/global_position_measurement_interface.cpp +++ b/px4_ros2_cpp/src/navigation/experimental/global_position_measurement_interface.cpp @@ -79,13 +79,15 @@ const if (measurement.lat_lon.has_value() && (!measurement.horizontal_variance.has_value() || measurement.horizontal_variance.value() <= 0)) { - RCLCPP_ERROR(_node.get_logger(), "Measurement lat_lon has an invalid variance value."); + RCLCPP_ERROR_ONCE(_node.get_logger(), "Measurement lat_lon has an invalid variance value."); return false; } if (measurement.altitude_msl.has_value() && (!measurement.vertical_variance.has_value() || measurement.vertical_variance.value() <= 0)) { - RCLCPP_ERROR(_node.get_logger(), "Measurement altitude_msl has an invalid variance value."); + RCLCPP_ERROR_ONCE( + _node.get_logger(), + "Measurement altitude_msl has an invalid variance value."); return false; } @@ -103,18 +105,20 @@ bool GlobalPositionMeasurementInterface::isValueNotNAN( const { if (measurement.lat_lon.has_value() && measurement.lat_lon.value().hasNaN()) { - RCLCPP_ERROR(_node.get_logger(), "Measurement value lat_lon is defined but contains a NAN."); + RCLCPP_ERROR_ONCE( + _node.get_logger(), + "Measurement value lat_lon is defined but contains a NAN."); return false; } if (measurement.horizontal_variance.has_value() && std::isnan(measurement.horizontal_variance.value())) { - RCLCPP_ERROR( + RCLCPP_ERROR_ONCE( _node.get_logger(), "Measurement value horizontal_variance is defined but contains a NAN."); return false; } if (measurement.altitude_msl.has_value() && std::isnan(measurement.altitude_msl.value())) { - RCLCPP_ERROR( + RCLCPP_ERROR_ONCE( _node.get_logger(), "Measurement value altitude_msl is defined but contains a NAN."); return false; @@ -122,7 +126,7 @@ const if (measurement.vertical_variance.has_value() && std::isnan(measurement.vertical_variance.value())) { - RCLCPP_ERROR( + RCLCPP_ERROR_ONCE( _node.get_logger(), "Measurement value vertical_variance is defined but contains a NAN."); return false; } diff --git a/px4_ros2_cpp/src/navigation/experimental/local_position_measurement_interface.cpp b/px4_ros2_cpp/src/navigation/experimental/local_position_measurement_interface.cpp index 78395ed..c63011d 100644 --- a/px4_ros2_cpp/src/navigation/experimental/local_position_measurement_interface.cpp +++ b/px4_ros2_cpp/src/navigation/experimental/local_position_measurement_interface.cpp @@ -123,7 +123,7 @@ const (!measurement.position_xy_variance.has_value() || (measurement.position_xy_variance.value().array() <= 0).any())) { - RCLCPP_ERROR( + RCLCPP_ERROR_ONCE( _node.get_logger(), "Measurement value position_xy has an invalid variance value."); return false; @@ -132,7 +132,8 @@ const if (measurement.position_z.has_value() && (!measurement.position_z_variance.has_value() || measurement.position_z_variance <= 0)) { - RCLCPP_ERROR(_node.get_logger(), "Measurement value position_z has an invalid variance value."); + RCLCPP_ERROR_ONCE( + _node.get_logger(), "Measurement value position_z has an invalid variance value."); return false; } @@ -140,7 +141,7 @@ const (!measurement.velocity_xy_variance.has_value() || (measurement.velocity_xy_variance.value().array() <= 0).any())) { - RCLCPP_ERROR( + RCLCPP_ERROR_ONCE( _node.get_logger(), "Measurement value velocity_xy has an invalid variance value."); return false; @@ -149,7 +150,8 @@ const if (measurement.velocity_z.has_value() && (!measurement.velocity_z_variance.has_value() || measurement.velocity_z_variance <= 0)) { - RCLCPP_ERROR(_node.get_logger(), "Measurement value velocity_z has an invalid variance value."); + RCLCPP_ERROR_ONCE( + _node.get_logger(), "Measurement value velocity_z has an invalid variance value."); return false; } @@ -157,7 +159,7 @@ const (!measurement.attitude_variance.has_value() || (measurement.attitude_variance.value().array() <= 0).any())) { - RCLCPP_ERROR( + RCLCPP_ERROR_ONCE( _node.get_logger(), "Measurement value attitude_quaternion has an invalid variance value."); return false; @@ -172,7 +174,7 @@ const if ((measurement.position_xy.has_value() || measurement.position_z.has_value()) && _pose_frame == AuxLocalPosition::POSE_FRAME_UNKNOWN) { - RCLCPP_ERROR( + RCLCPP_ERROR_ONCE( _node.get_logger(), "Position measurement has unknown pose frame."); return false; @@ -181,7 +183,7 @@ const if ((measurement.velocity_xy.has_value() || measurement.velocity_z.has_value()) && _velocity_frame == AuxLocalPosition::VELOCITY_FRAME_UNKNOWN) { - RCLCPP_ERROR( + RCLCPP_ERROR_ONCE( _node.get_logger(), "Velocity measurement has unknown velocity frame."); return false; @@ -194,7 +196,7 @@ bool LocalPositionMeasurementInterface::isValueNotNAN(const LocalPositionMeasure const { if (measurement.position_xy.has_value() && measurement.position_xy.value().hasNaN()) { - RCLCPP_ERROR( + RCLCPP_ERROR_ONCE( _node.get_logger(), "Measurement value position_xy is defined but contains a NAN."); return false; @@ -202,23 +204,24 @@ const if (measurement.position_xy_variance.has_value() && measurement.position_xy_variance.value().hasNaN()) { - RCLCPP_ERROR( + RCLCPP_ERROR_ONCE( _node.get_logger(), "Measurement value position_xy_variance is defined but contains a NAN."); return false; } if (measurement.position_z.has_value() && std::isnan(measurement.position_z.value())) { - RCLCPP_ERROR(_node.get_logger(), "Measurement value position_z is defined but contains a NAN."); + RCLCPP_ERROR_ONCE( + _node.get_logger(), "Measurement value position_z is defined but contains a NAN."); return false; } if (measurement.position_z_variance.has_value() && std::isnan(measurement.position_z_variance.value())) { - RCLCPP_ERROR( + RCLCPP_ERROR_ONCE( _node.get_logger(), "Measurement value position_z_variance is defined but contains a NAN."); return false; } if (measurement.velocity_xy.has_value() && measurement.velocity_xy.value().hasNaN()) { - RCLCPP_ERROR( + RCLCPP_ERROR_ONCE( _node.get_logger(), "Measurement value velocity_xy is defined but contains a NAN."); return false; @@ -226,30 +229,31 @@ const if (measurement.velocity_xy_variance.has_value() && measurement.velocity_xy_variance.value().hasNaN()) { - RCLCPP_ERROR( + RCLCPP_ERROR_ONCE( _node.get_logger(), "Measurement value velocity_xy_variance is defined but contains a NAN."); return false; } if (measurement.velocity_z.has_value() && std::isnan(measurement.velocity_z.value())) { - RCLCPP_ERROR(_node.get_logger(), "Measurement value velocity_z is defined but contains a NAN."); + RCLCPP_ERROR_ONCE( + _node.get_logger(), "Measurement value velocity_z is defined but contains a NAN."); return false; } if (measurement.velocity_z_variance.has_value() && std::isnan(measurement.velocity_z_variance.value())) { - RCLCPP_ERROR( + RCLCPP_ERROR_ONCE( _node.get_logger(), "Measurement value velocity_z_variance is defined but contains a NAN."); return false; } if (measurement.attitude_quaternion.has_value() && measurement.attitude_quaternion.value().coeffs().hasNaN()) { - RCLCPP_ERROR( + RCLCPP_ERROR_ONCE( _node.get_logger(), "Measurement value attitude_quaternion is defined but contains a NAN."); return false; } if (measurement.attitude_variance.has_value() && measurement.attitude_variance.value().hasNaN()) { - RCLCPP_ERROR( + RCLCPP_ERROR_ONCE( _node.get_logger(), "Measurement value attitude_variance is defined but contains a NAN."); return false; }