diff --git a/clover/CMakeLists.txt b/clover/CMakeLists.txt index e52fa723..7eca3f4c 100644 --- a/clover/CMakeLists.txt +++ b/clover/CMakeLists.txt @@ -92,6 +92,8 @@ add_service_files( Navigate.srv NavigateGlobal.srv SetAltitude.srv + SetYaw.srv + SetYawRate.srv SetPosition.srv SetVelocity.srv SetAttitude.srv diff --git a/clover/src/simple_offboard.cpp b/clover/src/simple_offboard.cpp index 0c68ba4b..e9bc2914 100644 --- a/clover/src/simple_offboard.cpp +++ b/clover/src/simple_offboard.cpp @@ -44,6 +44,8 @@ #include #include #include +#include +#include #include #include #include @@ -128,7 +130,9 @@ enum setpoint_type_t { VELOCITY, ATTITUDE, RATES, - _PARTIAL // for partial updates only + _ALTITUDE, + _YAW, + _YAW_RATE, }; enum setpoint_type_t setpoint_type = NONE; @@ -728,13 +732,13 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl } if (isfinite(yaw_rate)) { - if (sp_type == _PARTIAL && setpoint_type == ATTITUDE) { + if (sp_type > RATES && setpoint_type == ATTITUDE) { throw std::runtime_error("Yaw rate cannot be set in attitude mode."); } } // set_altitude - if (sp_type == _PARTIAL && isfinite(z)) { + if (sp_type == _ALTITUDE) { if (setpoint_type == VELOCITY || setpoint_type == ATTITUDE || setpoint_type == RATES) { throw std::runtime_error("Altitude cannot be set in velocity, attitude or rates mode."); } @@ -751,17 +755,13 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl speed = default_speed; } - if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) { - if (yaw_rate != 0 && !std::isnan(yaw)) - throw std::runtime_error("Yaw value should be NaN for setting yaw rate"); - } - if (sp_type == NAVIGATE_GLOBAL) { if (TIMEOUT(global_position, global_position_timeout)) throw std::runtime_error("No global position"); } - if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY || sp_type == ATTITUDE) { + // if any value need to be transformed to reference frame + if (isfinite(x) || isfinite(y) || isfinite(z) || isfinite(vx) || isfinite(vy) || isfinite(vz) || isfinite(yaw)) { // make sure transform from frame_id to reference frame available if (!waitTransform(reference_frame, frame_id, stamp, transform_timeout)) throw std::runtime_error("Can't transform from " + frame_id + " to " + reference_frame); @@ -783,7 +783,7 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl } // Everything fine - switch setpoint type - if (sp_type != _PARTIAL) { + if (sp_type <= RATES) { setpoint_type = sp_type; } @@ -814,14 +814,6 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl nav_from_sp_flag = true; } - // if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) { - // if (std::isnan(yaw) && yaw_rate == 0) { - // // keep yaw unchanged - // // TODO: this is incorrect, because we need yaw in desired frame - // yaw = tf2::getYaw(local_position.pose.orientation); - // } - // } - // handle position if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) { @@ -880,7 +872,7 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl } // handle yaw - if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY || sp_type == ATTITUDE) { + if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY || sp_type == ATTITUDE || sp_type == _YAW) { if (isfinite(yaw)) { setpoint_yaw_type = YAW; QuaternionStamped desired; @@ -897,8 +889,8 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl // yaw towards setpoint_yaw_type = TOWARDS; - } else if (yaw_frame_id.empty()) { - // yaw is nan and not set previously + } else if (yaw_frame_id.empty() || sp_type == _YAW) { + // yaw is nan and not set previously OR set_yaw(yaw=nan) was called setpoint_yaw_type = YAW; setpoint_yaw = tf2::getYaw(local_position.pose.orientation); yaw_frame_id = local_position.header.frame_id; @@ -916,11 +908,9 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl } // handle yaw rate - if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY || sp_type == RATES) { // TODO: simplify: isfinite(yaw_rate) - if (isnan(yaw) && isfinite(yaw_rate)) { - setpoint_yaw_type = YAW_RATE; - setpoint_rates.z = yaw_rate; - } + if (isfinite(yaw_rate)) { + setpoint_yaw_type = YAW_RATE; + setpoint_rates.z = yaw_rate; } // handle pitch rate @@ -973,23 +963,31 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl } bool navigate(Navigate::Request& req, Navigate::Response& res) { - return serve(NAVIGATE, req.x, req.y, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, NAN, NAN, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message); + return serve(NAVIGATE, req.x, req.y, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, NAN, NAN, NAN, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message); } bool navigateGlobal(NavigateGlobal::Request& req, NavigateGlobal::Response& res) { - return serve(NAVIGATE_GLOBAL, NAN, NAN, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, req.lat, req.lon, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message); + return serve(NAVIGATE_GLOBAL, NAN, NAN, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, NAN, req.lat, req.lon, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message); } bool setAltitude(SetAltitude::Request& req, SetAltitude::Response& res) { - return serve(_PARTIAL, NAN, NAN, req.z, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.frame_id, false, res.success, res.message); + return serve(_ALTITUDE, NAN, NAN, req.z, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.frame_id, false, res.success, res.message); +} + +bool setYaw(SetYaw::Request& req, SetYaw::Response& res) { + return serve(_YAW, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.frame_id, false, res.success, res.message); +} + +bool setYawRate(SetYawRate::Request& req, SetYawRate::Response& res) { + return serve(_YAW_RATE, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.yaw_rate, NAN, NAN, NAN, NAN, "", false, res.success, res.message); } bool setPosition(SetPosition::Request& req, SetPosition::Response& res) { - return serve(POSITION, req.x, req.y, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, NAN, NAN, NAN, NAN, req.frame_id, req.auto_arm, res.success, res.message); + return serve(POSITION, req.x, req.y, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.frame_id, req.auto_arm, res.success, res.message); } bool setVelocity(SetVelocity::Request& req, SetVelocity::Response& res) { - return serve(VELOCITY, NAN, NAN, NAN, req.vx, req.vy, req.vz, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, NAN, NAN, NAN, NAN, req.frame_id, req.auto_arm, res.success, res.message); + return serve(VELOCITY, NAN, NAN, NAN, req.vx, req.vy, req.vz, NAN, NAN, req.yaw, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.frame_id, req.auto_arm, res.success, res.message); } bool setAttitude(SetAttitude::Request& req, SetAttitude::Response& res) { @@ -1148,6 +1146,8 @@ int main(int argc, char **argv) auto na_serv = nh.advertiseService("navigate", &navigate); auto ng_serv = nh.advertiseService("navigate_global", &navigateGlobal); auto sl_serv = nh.advertiseService("set_altitude", &setAltitude); + auto ya_serv = nh.advertiseService("set_yaw", &setYaw); + auto yr_serv = nh.advertiseService("set_yaw_rate", &setYawRate); auto sp_serv = nh.advertiseService("set_position", &setPosition); auto sv_serv = nh.advertiseService("set_velocity", &setVelocity); auto sa_serv = nh.advertiseService("set_attitude", &setAttitude); diff --git a/clover/srv/Navigate.srv b/clover/srv/Navigate.srv index f607a437..7a4943ed 100644 --- a/clover/srv/Navigate.srv +++ b/clover/srv/Navigate.srv @@ -2,7 +2,6 @@ float32 x float32 y float32 z float32 yaw -float32 yaw_rate float32 speed string frame_id bool auto_arm diff --git a/clover/srv/NavigateGlobal.srv b/clover/srv/NavigateGlobal.srv index 69fbc444..5822f2b0 100644 --- a/clover/srv/NavigateGlobal.srv +++ b/clover/srv/NavigateGlobal.srv @@ -2,7 +2,6 @@ float64 lat float64 lon float32 z float32 yaw -float32 yaw_rate float32 speed string frame_id bool auto_arm diff --git a/clover/srv/SetPosition.srv b/clover/srv/SetPosition.srv index 20c73f05..80a01b29 100644 --- a/clover/srv/SetPosition.srv +++ b/clover/srv/SetPosition.srv @@ -2,7 +2,6 @@ float32 x float32 y float32 z float32 yaw -float32 yaw_rate string frame_id bool auto_arm --- diff --git a/clover/srv/SetVelocity.srv b/clover/srv/SetVelocity.srv index 25e174ac..908be1b8 100644 --- a/clover/srv/SetVelocity.srv +++ b/clover/srv/SetVelocity.srv @@ -2,7 +2,6 @@ float32 vx float32 vy float32 vz float32 yaw -float32 yaw_rate string frame_id bool auto_arm --- diff --git a/clover/srv/SetYaw.srv b/clover/srv/SetYaw.srv new file mode 100644 index 00000000..3e61d323 --- /dev/null +++ b/clover/srv/SetYaw.srv @@ -0,0 +1,5 @@ +float32 yaw +string frame_id +--- +bool success +string message diff --git a/clover/srv/SetYawRate.srv b/clover/srv/SetYawRate.srv new file mode 100644 index 00000000..82a21640 --- /dev/null +++ b/clover/srv/SetYawRate.srv @@ -0,0 +1,4 @@ +float32 yaw_rate +--- +bool success +string message diff --git a/clover/test/offboard.py b/clover/test/offboard.py index 71ae5bc2..426fb453 100755 --- a/clover/test/offboard.py +++ b/clover/test/offboard.py @@ -32,6 +32,8 @@ def test_offboard(node, tf_buffer): navigate = rospy.ServiceProxy('navigate', srv.Navigate) set_position = rospy.ServiceProxy('set_position', srv.SetPosition) set_altitude = rospy.ServiceProxy('set_altitude', srv.SetAltitude) + set_yaw = rospy.ServiceProxy('set_yaw', srv.SetYaw) + set_yaw_rate = rospy.ServiceProxy('set_yaw_rate', srv.SetYawRate) set_velocity = rospy.ServiceProxy('set_velocity', srv.SetVelocity) set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude) set_rates = rospy.ServiceProxy('set_rates', srv.SetRates) @@ -173,7 +175,7 @@ def test_offboard(node, tf_buffer): res = set_attitude() assert res.success == True - res = navigate(x=5, y=6, z=nan, frame_id='map') + res = navigate(x=5, y=6, z=nan, yaw=nan, frame_id='map') assert res.success == True state = get_state() assert state.mode == State.MODE_NAVIGATE @@ -200,8 +202,57 @@ def test_offboard(node, tf_buffer): assert state.z_frame_id == 'test' assert state.yaw_frame_id == 'map' + # test set_yaw + res = set_yaw(yaw=0.5, frame_id='test2') + assert res.success == True + state = get_state() + assert state.mode == State.MODE_NAVIGATE + assert state.yaw_mode == State.YAW_MODE_YAW + assert state.x == 5 + assert state.y == 6 + assert state.z == 7 + assert state.yaw == 0.5 + assert state.xy_frame_id == 'map' + assert state.z_frame_id == 'test' + assert state.yaw_frame_id == 'test2' + + # test set_yaw_rate + res = set_yaw_rate(yaw_rate=2) + assert res.success == True + state = get_state() + assert state.mode == State.MODE_NAVIGATE + assert state.yaw_mode == State.YAW_MODE_YAW_RATE + assert state.x == 5 + assert state.y == 6 + assert state.z == 7 + assert state.yaw_rate == 2 + assert state.xy_frame_id == 'map' + assert state.z_frame_id == 'test' + + # navigate(yaw=nan) should keep yaw rate mode + res = navigate(x=nan, y=nan, z=nan, yaw=nan) + assert res.success == True + state = get_state() + assert state.mode == State.MODE_NAVIGATE + assert state.yaw_mode == State.YAW_MODE_YAW_RATE + assert state.x == 5 + assert state.y == 6 + assert state.z == 7 + assert state.yaw_rate == 2 + assert state.xy_frame_id == 'map' + assert state.z_frame_id == 'test' + + # set_yaw(nan) should change back to yaw mode + res = set_yaw(yaw=nan) + assert res.success == True + state = get_state() + assert state.mode == State.MODE_NAVIGATE + assert state.yaw_mode == State.YAW_MODE_YAW + assert state.yaw == 0 + assert state.yaw_frame_id == 'map' + # test set_position - res = set_position(x=nan, y=nan, z=13, yaw=nan, yaw_rate=nan, frame_id='test2') + res = set_position(x=nan, y=nan, z=13, yaw=nan, frame_id='test2') assert res.success == True state = get_state() assert state.mode == State.MODE_POSITION @@ -228,6 +279,20 @@ def test_offboard(node, tf_buffer): assert state.z_frame_id == 'test' assert state.yaw_frame_id == 'map' + # set_yaw should not change the main mode + res = set_yaw(yaw=1, frame_id='test2') + assert res.success == True + state = get_state() + assert state.mode == State.MODE_POSITION + assert state.yaw_mode == State.YAW_MODE_YAW + assert state.x == 5 + assert state.y == 6 + assert state.z == 3 + assert state.yaw == 1 + assert state.xy_frame_id == 'map' + assert state.z_frame_id == 'test' + assert state.yaw_frame_id == 'test2' + # test set_velocity res = set_velocity(vx=1, frame_id='body') state = get_state() @@ -266,6 +331,23 @@ def test_offboard(node, tf_buffer): msg = rospy.wait_for_message('/mavros/setpoint_attitude/thrust', mavros_msgs.msg.Thrust, timeout=3) assert msg.thrust == approx(0.5) + # set_yaw should work in attitude mode + res = set_yaw(yaw=0.7, frame_id='test2') + assert res.success == True + state = get_state() + assert state.mode == State.MODE_ATTITUDE + assert state.yaw_mode == State.YAW_MODE_YAW + assert state.roll == approx(0.1) + assert state.pitch == approx(0.2) + assert state.yaw == approx(0.7) + assert state.thrust == approx(0.5) + assert state.yaw_frame_id == 'test2' + + # set_yaw_rate should not work in attitude mode + res = set_yaw_rate(yaw_rate=0.3) + assert res.success == False + assert res.message.startswith('Yaw rate cannot be set in') + # test set_rates res = set_rates(roll_rate=nan, pitch_rate=nan, yaw_rate=0.3, thrust=0.6) assert res.success == True @@ -304,6 +386,17 @@ def test_offboard(node, tf_buffer): assert msg.body_rate.y == approx(0.2) assert msg.body_rate.z == approx(0.1) + # set_yaw_rate should work in rates mode + res = set_yaw_rate(yaw_rate=0.4) + assert res.success == True + state = get_state() + assert state.mode == State.MODE_RATES + assert state.yaw_mode == State.YAW_MODE_YAW_RATE + assert state.roll_rate == approx(0.3) + assert state.pitch_rate == approx(0.2) + assert state.yaw_rate == approx(0.4) + 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'