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

Implement set_yaw and set_yaw_rate services, remove yaw_rate from other services

Browse Source
This commit is contained in:
Oleg Kalachev
2022-12-20 12:37:05 +03:00
parent df6ecdb594
commit 6474a61544
9 changed files with 137 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
clover/CMakeLists.txt
View File

@@ -92,6 +92,8 @@ add_service_files(
Navigate.srv
NavigateGlobal.srv
SetAltitude.srv
SetYaw.srv
SetYawRate.srv
SetPosition.srv
SetVelocity.srv
SetAttitude.srv

62
clover/src/simple_offboard.cpp
View File

@@ -44,6 +44,8 @@
#include <clover/Navigate.h>
#include <clover/NavigateGlobal.h>
#include <clover/SetAltitude.h>
#include <clover/SetYaw.h>
#include <clover/SetYawRate.h>
#include <clover/SetPosition.h>
#include <clover/SetVelocity.h>
#include <clover/SetAttitude.h>
@@ -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);

1
clover/srv/Navigate.srv
View File

@@ -2,7 +2,6 @@ float32 x
float32 y
float32 z
float32 yaw
float32 yaw_rate
float32 speed
string frame_id
bool auto_arm

1
clover/srv/NavigateGlobal.srv
View File

@@ -2,7 +2,6 @@ float64 lat
float64 lon
float32 z
float32 yaw
float32 yaw_rate
float32 speed
string frame_id
bool auto_arm

1
clover/srv/SetPosition.srv
View File

@@ -2,7 +2,6 @@ float32 x
float32 y
float32 z
float32 yaw
float32 yaw_rate
string frame_id
bool auto_arm
---

1
clover/srv/SetVelocity.srv
View File

@@ -2,7 +2,6 @@ float32 vx
float32 vy
float32 vz
float32 yaw
float32 yaw_rate
string frame_id
bool auto_arm
---

5
clover/srv/SetYaw.srv Normal file
View File

@@ -0,0 +1,5 @@
float32 yaw
string frame_id
---
bool success
string message

4
clover/srv/SetYawRate.srv Normal file
View File

@@ -0,0 +1,4 @@
float32 yaw_rate
---
bool success
string message

97
clover/test/offboard.py
View File

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