CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-26 11:43:25 +00:00
Code Issues Packages Projects Releases Wiki Activity

Ensure set_rates arguments are not inf

Browse Source
This commit is contained in:
Oleg Kalachev
2022-12-08 10:12:18 +03:00
parent 051d094f37
commit 68677fccdc
2 changed files with 12 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
clover/src/simple_offboard.cpp
View File

@@ -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) {

6
clover/test/offboard.py
View File

@@ -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'