Skip to content

Commit

Permalink
navigation: log error only once, add exception handling in examples
Browse files Browse the repository at this point in the history
  • Loading branch information
@GuillaumeLaine
GuillaumeLaine committed Dec 13, 2023
1 parent 2a01466 commit e2db92a
Show file tree
Hide file tree
Showing 4 changed files with 51 additions and 27 deletions.
12 changes: 10 additions & 2 deletions examples/cpp/navigation/global_navigation/include/global_navigation.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
12 changes: 10 additions & 2 deletions examples/cpp/navigation/local_navigation/include/local_navigation.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
16 changes: 10 additions & 6 deletions px4_ros2_cpp/src/navigation/experimental/global_position_measurement_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand All @@ -103,26 +105,28 @@ 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;
}
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;
}
Expand Down
38 changes: 21 additions & 17 deletions px4_ros2_cpp/src/navigation/experimental/local_position_measurement_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -132,15 +132,16 @@ 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;
}

if (measurement.velocity_xy.has_value() &&
(!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;
Expand All @@ -149,15 +150,16 @@ 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;
}

if (measurement.attitude_quaternion.has_value() &&
(!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;
Expand All @@ -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;
Expand All @@ -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;
Expand All @@ -194,62 +196,64 @@ 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;
}
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;
}
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;
}
Expand Down

0 comments on commit e2db92a

Please sign in to comment.