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

simple_offboard: make yaw in set_velocity work

Browse Source
This commit is contained in:
Oleg Kalachev
2019-01-22 17:47:27 +03:00
parent a43ff641ba
commit 48e670d7aa
1 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
clever/src/simple_offboard.cpp
View File

@@ -338,7 +338,7 @@ void publish(const ros::Time stamp)
try {
// transform position and/or yaw
if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION || setpoint_type == ATTITUDE) {
if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION || setpoint_type == VELOCITY || setpoint_type == ATTITUDE) {
setpoint_position.header.stamp = stamp;
tf_buffer.transform(setpoint_position, setpoint_position_transformed, local_frame, ros::Duration(0.05));
}
@@ -473,7 +473,7 @@ inline void serve(enum setpoint_type_t sp_type, float x, float y, float z, float
speed = default_speed;
}
if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION) {
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");
@@ -520,7 +520,7 @@ inline void serve(enum setpoint_type_t sp_type, float x, float y, float z, float
nav_speed = speed;
}
if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION) {
if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) {
if (std::isnan(yaw) && yaw_rate == 0) {
// keep yaw unchanged
yaw = tf2::getYaw(local_position.pose.orientation);