diff --git a/clover/src/simple_offboard.cpp b/clover/src/simple_offboard.cpp index 76187a0c..f170887c 100644 --- a/clover/src/simple_offboard.cpp +++ b/clover/src/simple_offboard.cpp @@ -596,6 +596,8 @@ void publishState() #define ENSURE_FINITE(var) { if (!std::isfinite(var)) throw std::runtime_error(#var " argument cannot be NaN or Inf"); } +#define ENSURE_NON_INF(var) { if (std::isinf(var)) throw std::runtime_error(#var " argument cannot be Inf"); } + bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, float vy, float vz, float pitch, float roll, float yaw, float pitch_rate, float roll_rate, float yaw_rate, // editorconfig-checker-disable-line float lat, float lon, float thrust, float speed, string frame_id, bool auto_arm, // editorconfig-checker-disable-line @@ -685,6 +687,11 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl ENSURE_FINITE(pitch); ENSURE_FINITE(roll); ENSURE_FINITE(thrust); + } else if (sp_type == RATES) { + ENSURE_NON_INF(pitch_rate); + ENSURE_NON_INF(roll_rate); + ENSURE_NON_INF(yaw_rate); + ENSURE_NON_INF(thrust); } if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL) { diff --git a/clover/test/offboard.py b/clover/test/offboard.py index 7c8170a8..1a009c02 100755 --- a/clover/test/offboard.py +++ b/clover/test/offboard.py @@ -6,7 +6,7 @@ import mavros_msgs.msg from geometry_msgs.msg import PoseStamped from clover import srv from clover.msg import State -from math import nan +from math import nan, inf @pytest.fixture() def node(): @@ -134,3 +134,7 @@ def test_offboard(node): assert state.pitch_rate == approx(0.2) assert state.yaw_rate == approx(0.1) assert state.thrust == approx(0.3) + + res = set_rates(roll_rate=inf) + assert res.success == False + assert res.message == 'roll_rate argument cannot be Inf'