simple_offboard.cpp

Go to the documentation of this file.

/*
 * Simplified copter control in OFFBOARD mode
 * Copyright (C) 2019 Copter Express Technologies
 *
 * Author: Oleg Kalachev <okalachev@gmail.com>
 *
 * Distributed under MIT License (available at https://opensource.org/licenses/MIT).
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 */

#include <algorithm>
#include <string>
#include <cmath>
#include <boost/format.hpp>
#include <stdexcept>
#include <GeographicLib/Geodesic.hpp>
#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include <tf2/utils.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <std_srvs/Trigger.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/QuaternionStamped.h>
#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/BatteryState.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/PositionTarget.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <mavros_msgs/Thrust.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/StatusText.h>
#include <mavros_msgs/ManualControl.h>

#include <clover/GetTelemetry.h>
#include <clover/Navigate.h>
#include <clover/NavigateGlobal.h>
#include <clover/SetPosition.h>
#include <clover/SetVelocity.h>
#include <clover/SetAttitude.h>
#include <clover/SetRates.h>

using std::string;
using std::isnan;
using namespace geometry_msgs;
using namespace sensor_msgs;
using namespace clover;
using mavros_msgs::PositionTarget;
using mavros_msgs::AttitudeTarget;
using mavros_msgs::Thrust;

// tf2
tf2_ros::Buffer tf_buffer;
std::shared_ptr<tf2_ros::TransformBroadcaster> transform_broadcaster;
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_transform_broadcaster;

// Parameters
string local_frame;
string fcu_frame;
ros::Duration transform_timeout;
ros::Duration telemetry_transform_timeout;
ros::Duration offboard_timeout;
ros::Duration land_timeout;
ros::Duration arming_timeout;
ros::Duration local_position_timeout;
ros::Duration state_timeout;
ros::Duration velocity_timeout;
ros::Duration global_position_timeout;
ros::Duration battery_timeout;
ros::Duration manual_control_timeout;
float default_speed;
bool auto_release;
bool land_only_in_offboard, nav_from_sp, check_kill_switch;
std::map<string, string> reference_frames;

// Publishers
ros::Publisher attitude_pub, attitude_raw_pub, position_pub, position_raw_pub, rates_pub, thrust_pub;

// Service clients
ros::ServiceClient arming, set_mode;

// Containers
ros::Timer setpoint_timer;
tf::Quaternion tq;
PoseStamped position_msg;
PositionTarget position_raw_msg;
AttitudeTarget att_raw_msg;
Thrust thrust_msg;
TwistStamped rates_msg;
TransformStamped target, setpoint;
geometry_msgs::TransformStamped body;

// State
PoseStamped nav_start;
PoseStamped setpoint_position, setpoint_position_transformed;
Vector3Stamped setpoint_velocity, setpoint_velocity_transformed;
QuaternionStamped setpoint_attitude, setpoint_attitude_transformed;
float setpoint_yaw_rate;
float nav_speed;
bool busy = false;
bool wait_armed = false;
bool nav_from_sp_flag = false;

enum setpoint_type_t {
        NONE,
        NAVIGATE,
        NAVIGATE_GLOBAL,
        POSITION,
        VELOCITY,
        ATTITUDE,
        RATES
};

enum setpoint_type_t setpoint_type = NONE;

enum { YAW, YAW_RATE, TOWARDS } setpoint_yaw_type;

// Last received telemetry messages
mavros_msgs::State state;
mavros_msgs::StatusText statustext;
mavros_msgs::ManualControl manual_control;
PoseStamped local_position;
TwistStamped velocity;
NavSatFix global_position;
BatteryState battery;

// Common subscriber callback template that stores message to the variable
template<typename T, T& STORAGE>
void handleMessage(const T& msg)
{
        STORAGE = msg;
}

void handleState(const mavros_msgs::State& s)
{
        state = s;
        if (s.mode != "OFFBOARD") {
                // flight intercepted
                nav_from_sp_flag = false;
        }
}

inline void publishBodyFrame()
{
        if (body.child_frame_id.empty()) return;

        tf::Quaternion q;
        q.setRPY(0, 0, tf::getYaw(local_position.pose.orientation));
        tf::quaternionTFToMsg(q, body.transform.rotation);

        body.transform.translation.x = local_position.pose.position.x;
        body.transform.translation.y = local_position.pose.position.y;
        body.transform.translation.z = local_position.pose.position.z;
        body.header.frame_id = local_position.header.frame_id;
        body.header.stamp = local_position.header.stamp;
        transform_broadcaster->sendTransform(body);
}

void handleLocalPosition(const PoseStamped& pose)
{
        local_position = pose;
        publishBodyFrame();
        // TODO: terrain?, home?
}

// wait for transform without interrupting publishing setpoints
inline bool waitTransform(const string& target, const string& source,
                          const ros::Time& stamp, const ros::Duration& timeout) // editorconfig-checker-disable-line
{
        ros::Rate r(100);
        auto start = ros::Time::now();
        while (ros::ok()) {
                if (ros::Time::now() - start > timeout) return false;
                if (tf_buffer.canTransform(target, source, stamp)) return true;
                ros::spinOnce();
                r.sleep();
        }
}

#define TIMEOUT(msg, timeout) (ros::Time::now() - msg.header.stamp > timeout)

bool getTelemetry(GetTelemetry::Request& req, GetTelemetry::Response& res)
{
        ros::Time stamp = ros::Time::now();

        if (req.frame_id.empty())
                req.frame_id = local_frame;

        res.frame_id = req.frame_id;
        res.x = NAN;
        res.y = NAN;
        res.z = NAN;
        res.lat = NAN;
        res.lon = NAN;
        res.alt = NAN;
        res.vx = NAN;
        res.vy = NAN;
        res.vz = NAN;
        res.pitch = NAN;
        res.roll = NAN;
        res.yaw = NAN;
        res.pitch_rate = NAN;
        res.roll_rate = NAN;
        res.yaw_rate = NAN;
        res.voltage = NAN;
        res.cell_voltage = NAN;

        if (!TIMEOUT(state, state_timeout)) {
                res.connected = state.connected;
                res.armed = state.armed;
                res.mode = state.mode;
        }

        try {
                waitTransform(req.frame_id, fcu_frame, stamp, telemetry_transform_timeout);
                auto transform = tf_buffer.lookupTransform(req.frame_id, fcu_frame, stamp);
                res.x = transform.transform.translation.x;
                res.y = transform.transform.translation.y;
                res.z = transform.transform.translation.z;

                double yaw, pitch, roll;
                tf2::getEulerYPR(transform.transform.rotation, yaw, pitch, roll);
                res.yaw = yaw;
                res.pitch = pitch;
                res.roll = roll;
        } catch (const tf2::TransformException& e) {
                ROS_DEBUG("%s", e.what());
        }

        if (!TIMEOUT(velocity, velocity_timeout)) {
                try {
                        // transform velocity
                        waitTransform(req.frame_id, fcu_frame, velocity.header.stamp, telemetry_transform_timeout);
                        Vector3Stamped vec, vec_out;
                        vec.header.stamp = velocity.header.stamp;
                        vec.header.frame_id = velocity.header.frame_id;
                        vec.vector = velocity.twist.linear;
                        tf_buffer.transform(vec, vec_out, req.frame_id);

                        res.vx = vec_out.vector.x;
                        res.vy = vec_out.vector.y;
                        res.vz = vec_out.vector.z;
                } catch (const tf2::TransformException& e) {}

                // use angular velocities as they are
                res.yaw_rate = velocity.twist.angular.z;
                res.pitch_rate = velocity.twist.angular.y;
                res.roll_rate = velocity.twist.angular.x;
        }

        if (!TIMEOUT(global_position, global_position_timeout)) {
                res.lat = global_position.latitude;
                res.lon = global_position.longitude;
                res.alt = global_position.altitude;
        }

        if (!TIMEOUT(battery, battery_timeout)) {
                res.voltage = battery.voltage;
                if (!battery.cell_voltage.empty()) {
                        res.cell_voltage = battery.cell_voltage[0];
                }
        }

        return true;
}

// throws std::runtime_error
void offboardAndArm()
{
        ros::Rate r(10);

        if (state.mode != "OFFBOARD") {
                auto start = ros::Time::now();
                ROS_INFO("switch to OFFBOARD");
                static mavros_msgs::SetMode sm;
                sm.request.custom_mode = "OFFBOARD";

                if (!set_mode.call(sm))
                        throw std::runtime_error("Error calling set_mode service");

                // wait for OFFBOARD mode
                while (ros::ok()) {
                        ros::spinOnce();
                        if (state.mode == "OFFBOARD") {
                                break;
                        } else if (ros::Time::now() - start > offboard_timeout) {
                                string report = "OFFBOARD timed out";
                                if (statustext.header.stamp > start)
                                        report += ": " + statustext.text;
                                throw std::runtime_error(report);
                        }
                        ros::spinOnce();
                        r.sleep();
                }
        }

        if (!state.armed) {
                ros::Time start = ros::Time::now();
                ROS_INFO("arming");
                mavros_msgs::CommandBool srv;
                srv.request.value = true;
                if (!arming.call(srv)) {
                        throw std::runtime_error("Error calling arming service");
                }

                // wait until armed
                while (ros::ok()) {
                        ros::spinOnce();
                        if (state.armed) {
                                break;
                        } else if (ros::Time::now() - start > arming_timeout) {
                                string report = "Arming timed out";
                                if (statustext.header.stamp > start)
                                        report += ": " + statustext.text;
                                throw std::runtime_error(report);
                        }
                        ros::spinOnce();
                        r.sleep();
                }
        }
}

inline double hypot(double x, double y, double z)
{
        return std::sqrt(x * x + y * y + z * z);
}

inline float getDistance(const Point& from, const Point& to)
{
        return hypot(from.x - to.x, from.y - to.y, from.z - to.z);
}

void getNavigateSetpoint(const ros::Time& stamp, float speed, Point& nav_setpoint)
{
        if (wait_armed) {
                // don't start navigating if we're waiting arming
                nav_start.header.stamp = stamp;
        }

        float distance = getDistance(nav_start.pose.position, setpoint_position_transformed.pose.position);
        float time = distance / speed;
        float passed = std::min((stamp - nav_start.header.stamp).toSec() / time, 1.0);

        nav_setpoint.x = nav_start.pose.position.x + (setpoint_position_transformed.pose.position.x - nav_start.pose.position.x) * passed;
        nav_setpoint.y = nav_start.pose.position.y + (setpoint_position_transformed.pose.position.y - nav_start.pose.position.y) * passed;
        nav_setpoint.z = nav_start.pose.position.z + (setpoint_position_transformed.pose.position.z - nav_start.pose.position.z) * passed;
}

PoseStamped globalToLocal(double lat, double lon)
{
        auto earth = GeographicLib::Geodesic::WGS84();

        // Determine azimuth and distance between current and destination point
        double _, distance, azimuth;
        earth.Inverse(global_position.latitude, global_position.longitude, lat, lon, distance, _, azimuth);

        double x_offset, y_offset;
        double azimuth_radians = azimuth * M_PI / 180;
        x_offset = distance * sin(azimuth_radians);
        y_offset = distance * cos(azimuth_radians);

        if (!waitTransform(local_frame, fcu_frame, global_position.header.stamp, ros::Duration(0.2))) {
                throw std::runtime_error("No local position");
        }

        auto local = tf_buffer.lookupTransform(local_frame, fcu_frame, global_position.header.stamp);

        PoseStamped pose;
        pose.header.stamp = global_position.header.stamp; // TODO: ?
        pose.header.frame_id = local_frame;
        pose.pose.position.x = local.transform.translation.x + x_offset;
        pose.pose.position.y = local.transform.translation.y + y_offset;
        pose.pose.orientation.w = 1;
        return pose;
}

void publish(const ros::Time stamp)
{
        if (setpoint_type == NONE) return;

        position_raw_msg.header.stamp = stamp;
        thrust_msg.header.stamp = stamp;
        rates_msg.header.stamp = stamp;

        try {
                // transform position and/or yaw
                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));
                }

                // transform velocity
                if (setpoint_type == VELOCITY) {
                        setpoint_velocity.header.stamp = stamp;
                        tf_buffer.transform(setpoint_velocity, setpoint_velocity_transformed, local_frame, ros::Duration(0.05));
                }

        } catch (const tf2::TransformException& e) {
                ROS_WARN_THROTTLE(10, "can't transform");
        }

        if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL) {
                position_msg.pose.orientation = setpoint_position_transformed.pose.orientation; // copy yaw
                getNavigateSetpoint(stamp, nav_speed, position_msg.pose.position);

                if (setpoint_yaw_type == TOWARDS) {
                        double yaw_towards = atan2(position_msg.pose.position.y - nav_start.pose.position.y,
                                                   position_msg.pose.position.x - nav_start.pose.position.x);
                        position_msg.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, yaw_towards);
                }
        }

        if (setpoint_type == POSITION) {
                position_msg = setpoint_position_transformed;
        }

        if (setpoint_type == POSITION || setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL) {
                position_msg.header.stamp = stamp;

                if (setpoint_yaw_type == YAW || setpoint_yaw_type == TOWARDS) {
                        position_pub.publish(position_msg);

                } else {
                        position_raw_msg.type_mask = PositionTarget::IGNORE_VX +
                                                     PositionTarget::IGNORE_VY +
                                                     PositionTarget::IGNORE_VZ +
                                                     PositionTarget::IGNORE_AFX +
                                                     PositionTarget::IGNORE_AFY +
                                                     PositionTarget::IGNORE_AFZ +
                                                     PositionTarget::IGNORE_YAW;
                        position_raw_msg.yaw_rate = setpoint_yaw_rate;
                        position_raw_msg.position = position_msg.pose.position;
                        position_raw_pub.publish(position_raw_msg);
                }

                // publish setpoint frame
                if (!setpoint.child_frame_id.empty()) {
                        setpoint.transform.translation.x = position_msg.pose.position.x;
                        setpoint.transform.translation.y = position_msg.pose.position.y;
                        setpoint.transform.translation.z = position_msg.pose.position.z;
                        setpoint.transform.rotation = position_msg.pose.orientation;
                        setpoint.header.frame_id = position_msg.header.frame_id;
                        setpoint.header.stamp = position_msg.header.stamp;
                        transform_broadcaster->sendTransform(setpoint);
                }
        }

        if (setpoint_type == VELOCITY) {
                position_raw_msg.type_mask = PositionTarget::IGNORE_PX +
                                             PositionTarget::IGNORE_PY +
                                             PositionTarget::IGNORE_PZ +
                                             PositionTarget::IGNORE_AFX +
                                             PositionTarget::IGNORE_AFY +
                                             PositionTarget::IGNORE_AFZ;
                position_raw_msg.type_mask += setpoint_yaw_type == YAW ? PositionTarget::IGNORE_YAW_RATE : PositionTarget::IGNORE_YAW;
                position_raw_msg.velocity = setpoint_velocity_transformed.vector;
                position_raw_msg.yaw = tf2::getYaw(setpoint_position_transformed.pose.orientation);
                position_raw_msg.yaw_rate = setpoint_yaw_rate;
                position_raw_pub.publish(position_raw_msg);
        }

        if (setpoint_type == ATTITUDE) {
                attitude_pub.publish(setpoint_position_transformed);
                thrust_pub.publish(thrust_msg);
        }

        if (setpoint_type == RATES) {
                // rates_pub.publish(rates_msg);
                // thrust_pub.publish(thrust_msg);
                // mavros rates topics waits for rates in local frame
                // use rates in body frame for simplicity
                att_raw_msg.header.stamp = stamp;
                att_raw_msg.header.frame_id = fcu_frame;
                att_raw_msg.type_mask = AttitudeTarget::IGNORE_ATTITUDE;
                att_raw_msg.body_rate = rates_msg.twist.angular;
                att_raw_msg.thrust = thrust_msg.thrust;
                attitude_raw_pub.publish(att_raw_msg);
        }
}

void publishSetpoint(const ros::TimerEvent& event)
{
        publish(event.current_real);
}

inline void checkManualControl()
{
        if (!manual_control_timeout.isZero() && TIMEOUT(manual_control, manual_control_timeout)) {
                throw std::runtime_error("Manual control timeout, RC is switched off?");
        }

        if (check_kill_switch) {
                // switch values: https://github.com/PX4/PX4-Autopilot/blob/c302514a0809b1765fafd13c014d705446ae1113/msg/manual_control_setpoint.msg#L3
                const uint8_t SWITCH_POS_NONE = 0; // switch is not mapped
                const uint8_t SWITCH_POS_ON = 1; // switch activated
                const uint8_t SWITCH_POS_MIDDLE = 2; // middle position
                const uint8_t SWITCH_POS_OFF = 3; // switch not activated

                const int KILL_SWITCH_BIT = 12; // https://github.com/PX4/Firmware/blob/c302514a0809b1765fafd13c014d705446ae1113/src/modules/mavlink/mavlink_messages.cpp#L3975
                uint8_t kill_switch = (manual_control.buttons & (0b11 << KILL_SWITCH_BIT)) >> KILL_SWITCH_BIT;

                if (kill_switch == SWITCH_POS_ON)
                        throw std::runtime_error("Kill switch is on");
        }
}

inline void checkState()
{
        if (TIMEOUT(state, state_timeout))
                throw std::runtime_error("State timeout, check mavros settings");

        if (!state.connected)
                throw std::runtime_error("No connection to FCU, https://clover.coex.tech/connection");
}

#define ENSURE_FINITE(var) { if (!std::isfinite(var)) throw std::runtime_error(#var " argument cannot be NaN or 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
           uint8_t& success, string& message) // editorconfig-checker-disable-line
{
        auto stamp = ros::Time::now();

        try {
                if (busy)
                        throw std::runtime_error("Busy");

                busy = true;

                // Checks
                checkState();

                if (auto_arm) {
                        checkManualControl();
                }

                // default frame is local frame
                if (frame_id.empty())
                        frame_id = local_frame;

                // look up for reference frame
                auto search = reference_frames.find(frame_id);
                const string& reference_frame = search == reference_frames.end() ? frame_id : search->second;

                // Serve "partial" commands

                if (!auto_arm && std::isfinite(yaw) &&
                    isnan(x) && isnan(y) && isnan(z) && isnan(vx) && isnan(vy) && isnan(vz) &&
                    isnan(pitch) && isnan(roll) && isnan(thrust) &&
                    isnan(lat) && isnan(lon)) {
                        // change only the yaw
                        if (setpoint_type == POSITION || setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == VELOCITY) {
                                if (!waitTransform(setpoint_position.header.frame_id, frame_id, stamp, transform_timeout))
                                        throw std::runtime_error("Can't transform from " + frame_id + " to " + setpoint_position.header.frame_id);

                                message = "Changing yaw only";

                                QuaternionStamped q;
                                q.header.frame_id = frame_id;
                                q.header.stamp = stamp;
                                q.quaternion = tf::createQuaternionMsgFromYaw(yaw); // TODO: pitch=0, roll=0 is not totally correct
                                setpoint_position.pose.orientation = tf_buffer.transform(q, setpoint_position.header.frame_id).quaternion;
                                setpoint_yaw_type = YAW;
                                goto publish_setpoint;
                        } else {
                                throw std::runtime_error("Setting yaw is possible only when position or velocity setpoints active");
                        }
                }

                if (!auto_arm && std::isfinite(yaw_rate) &&
                    isnan(x) && isnan(y) && isnan(z) && isnan(vx) && isnan(vy) && isnan(vz) &&
                    isnan(pitch) && isnan(roll) && isnan(yaw) && isnan(thrust) &&
                    isnan(lat) && isnan(lon)) {
                        // change only the yaw rate
                        if (setpoint_type == POSITION || setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == VELOCITY) {
                                message = "Changing yaw rate only";

                                setpoint_yaw_type = YAW_RATE;
                                setpoint_yaw_rate = yaw_rate;
                                goto publish_setpoint;
                        } else {
                                throw std::runtime_error("Setting yaw rate is possible only when position or velocity setpoints active");
                        }
                }

                // Serve normal commands

                if (sp_type == NAVIGATE || sp_type == POSITION) {
                        ENSURE_FINITE(x);
                        ENSURE_FINITE(y);
                        ENSURE_FINITE(z);
                } else if (sp_type == NAVIGATE_GLOBAL) {
                        ENSURE_FINITE(lat);
                        ENSURE_FINITE(lon);
                        ENSURE_FINITE(z);
                } else if (sp_type == VELOCITY) {
                        ENSURE_FINITE(vx);
                        ENSURE_FINITE(vy);
                        ENSURE_FINITE(vz);
                } else if (sp_type == ATTITUDE) {
                        ENSURE_FINITE(pitch);
                        ENSURE_FINITE(roll);
                        ENSURE_FINITE(thrust);
                } else if (sp_type == RATES) {
                        ENSURE_FINITE(pitch_rate);
                        ENSURE_FINITE(roll_rate);
                        ENSURE_FINITE(thrust);
                }

                if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL) {
                        if (TIMEOUT(local_position, local_position_timeout))
                                throw std::runtime_error("No local position, check settings");

                        if (speed < 0)
                                throw std::runtime_error("Navigate speed must be positive, " + std::to_string(speed) + " passed");

                        if (speed == 0)
                                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 (std::isnan(yaw_rate) && std::isnan(yaw))
                                throw std::runtime_error("Both yaw and yaw_rate cannot be NaN");
                }

                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) {
                        // 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);

                        // make sure transform from reference frame to local frame available
                        if (!waitTransform(local_frame, reference_frame, stamp, transform_timeout))
                                throw std::runtime_error("Can't transform from " + reference_frame + " to " + local_frame);
                }

                if (sp_type == NAVIGATE_GLOBAL) {
                        // Calculate x and from lat and lot in request's frame
                        auto pose_local = globalToLocal(lat, lon);
                        pose_local.header.stamp = stamp; // TODO: fix
                        auto xy_in_req_frame = tf_buffer.transform(pose_local, frame_id);
                        x = xy_in_req_frame.pose.position.x;
                        y = xy_in_req_frame.pose.position.y;
                }

                // Everything fine - switch setpoint type
                setpoint_type = sp_type;

                if (sp_type != NAVIGATE && sp_type != NAVIGATE_GLOBAL) {
                        nav_from_sp_flag = false;
                }

                if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL) {
                        // starting point
                        if (nav_from_sp && nav_from_sp_flag) {
                                message = "Navigating from current setpoint";
                                nav_start = position_msg;
                        } else {
                                nav_start = local_position;
                        }
                        nav_speed = speed;
                        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);
                //      }
                // }

                if (sp_type == POSITION || sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == VELOCITY || sp_type == ATTITUDE) {
                        // destination point and/or yaw
                        PoseStamped ps;
                        ps.header.frame_id = frame_id;
                        ps.header.stamp = stamp;
                        ps.pose.position.x = x;
                        ps.pose.position.y = y;
                        ps.pose.position.z = z;
                        ps.pose.orientation.w = 1.0; // Ensure quaternion is always valid

                        if (std::isnan(yaw)) {
                                setpoint_yaw_type = YAW_RATE;
                                setpoint_yaw_rate = yaw_rate;
                        } else if (std::isinf(yaw) && yaw > 0) {
                                // yaw towards
                                setpoint_yaw_type = TOWARDS;
                                yaw = 0;
                                setpoint_yaw_rate = 0;
                        } else {
                                setpoint_yaw_type = YAW;
                                setpoint_yaw_rate = 0;
                                ps.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
                        }

                        tf_buffer.transform(ps, setpoint_position, reference_frame);
                }

                if (sp_type == VELOCITY) {
                        static Vector3Stamped vel;
                        vel.header.frame_id = frame_id;
                        vel.header.stamp = stamp;
                        vel.vector.x = vx;
                        vel.vector.y = vy;
                        vel.vector.z = vz;
                        tf_buffer.transform(vel, setpoint_velocity, reference_frame);
                }

                if (sp_type == ATTITUDE || sp_type == RATES) {
                        thrust_msg.thrust = thrust;
                }

                if (sp_type == RATES) {
                        rates_msg.twist.angular.x = roll_rate;
                        rates_msg.twist.angular.y = pitch_rate;
                        rates_msg.twist.angular.z = yaw_rate;
                }

                wait_armed = auto_arm;

publish_setpoint:
                publish(stamp); // calculate initial transformed messages first
                setpoint_timer.start();

                // publish target frame
                if (!target.child_frame_id.empty()) {
                        if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) {
                                target.header.frame_id = setpoint_position.header.frame_id;
                                target.header.stamp = stamp;
                                target.transform.translation.x = setpoint_position.pose.position.x;
                                target.transform.translation.y = setpoint_position.pose.position.y;
                                target.transform.translation.z = setpoint_position.pose.position.z;
                                target.transform.rotation = setpoint_position.pose.orientation;
                                static_transform_broadcaster->sendTransform(target);
                        }
                }

                if (auto_arm) {
                        offboardAndArm();
                        wait_armed = false;
                } else if (state.mode != "OFFBOARD") {
                        setpoint_timer.stop();
                        throw std::runtime_error("Copter is not in OFFBOARD mode, use auto_arm?");
                } else if (!state.armed) {
                        setpoint_timer.stop();
                        throw std::runtime_error("Copter is not armed, use auto_arm?");
                }

        } catch (const std::exception& e) {
                message = e.what();
                ROS_INFO("%s", message.c_str());
                busy = false;
                return true;
        }

        success = true;
        busy = false;
        return true;
}

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);
}

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);
}

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);
}

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);
}

bool setAttitude(SetAttitude::Request& req, SetAttitude::Response& res) {
        return serve(ATTITUDE, NAN, NAN, NAN, NAN, NAN, NAN, req.pitch, req.roll, req.yaw, NAN, NAN, NAN, NAN, NAN, req.thrust, NAN, req.frame_id, req.auto_arm, res.success, res.message);
}

bool setRates(SetRates::Request& req, SetRates::Response& res) {
        return serve(RATES, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.pitch_rate, req.roll_rate, req.yaw_rate, NAN, NAN, req.thrust, NAN, "", req.auto_arm, res.success, res.message);
}

bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
{
        try {
                if (busy)
                        throw std::runtime_error("Busy");

                busy = true;

                checkState();

                if (land_only_in_offboard) {
                        if (state.mode != "OFFBOARD") {
                                throw std::runtime_error("Copter is not in OFFBOARD mode");
                        }
                }

                static mavros_msgs::SetMode sm;
                sm.request.custom_mode = "AUTO.LAND";

                if (!set_mode.call(sm))
                        throw std::runtime_error("Can't call set_mode service");

                if (!sm.response.mode_sent)
                        throw std::runtime_error("Can't send set_mode request");

                static ros::Rate r(10);
                auto start = ros::Time::now();
                while (ros::ok()) {
                        if (state.mode == "AUTO.LAND") {
                                res.success = true;
                                busy = false;
                                return true;
                        }
                        if (ros::Time::now() - start > land_timeout)
                                throw std::runtime_error("Land request timed out");

                        ros::spinOnce();
                        r.sleep();
                }

        } catch (const std::exception& e) {
                res.message = e.what();
                ROS_INFO("%s", e.what());
                busy = false;
                return true;
        }
}

int main(int argc, char **argv)
{
        ros::init(argc, argv, "simple_offboard");
        ros::NodeHandle nh, nh_priv("~");

        tf2_ros::TransformListener tf_listener(tf_buffer);
        transform_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>();
        static_transform_broadcaster = std::make_shared<tf2_ros::StaticTransformBroadcaster>();

        // Params
        nh.param<string>("mavros/local_position/tf/frame_id", local_frame, "map");
        nh.param<string>("mavros/local_position/tf/child_frame_id", fcu_frame, "base_link");
        nh_priv.param("target_frame", target.child_frame_id, string("navigate_target"));
        nh_priv.param("setpoint", setpoint.child_frame_id, string("setpoint"));
        nh_priv.param("auto_release", auto_release, true);
        nh_priv.param("land_only_in_offboard", land_only_in_offboard, true);
        nh_priv.param("nav_from_sp", nav_from_sp, true);
        nh_priv.param("check_kill_switch", check_kill_switch, true);
        nh_priv.param("default_speed", default_speed, 0.5f);
        nh_priv.param<string>("body_frame", body.child_frame_id, "body");
        nh_priv.getParam("reference_frames", reference_frames);

        state_timeout = ros::Duration(nh_priv.param("state_timeout", 3.0));
        local_position_timeout = ros::Duration(nh_priv.param("local_position_timeout", 2.0));
        velocity_timeout = ros::Duration(nh_priv.param("velocity_timeout", 2.0));
        global_position_timeout = ros::Duration(nh_priv.param("global_position_timeout", 10.0));
        battery_timeout = ros::Duration(nh_priv.param("battery_timeout", 2.0));
        manual_control_timeout = ros::Duration(nh_priv.param("manual_control_timeout", 0.0));

        transform_timeout = ros::Duration(nh_priv.param("transform_timeout", 0.5));
        telemetry_transform_timeout = ros::Duration(nh_priv.param("telemetry_transform_timeout", 0.5));
        offboard_timeout = ros::Duration(nh_priv.param("offboard_timeout", 3.0));
        land_timeout = ros::Duration(nh_priv.param("land_timeout", 3.0));
        arming_timeout = ros::Duration(nh_priv.param("arming_timeout", 4.0));

        // Service clients
        arming = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
        set_mode = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");

        // Telemetry subscribers
        auto state_sub = nh.subscribe("mavros/state", 1, &handleState);
        auto velocity_sub = nh.subscribe("mavros/local_position/velocity_body", 1, &handleMessage<TwistStamped, velocity>);
        auto global_position_sub = nh.subscribe("mavros/global_position/global", 1, &handleMessage<NavSatFix, global_position>);
        auto battery_sub = nh.subscribe("mavros/battery", 1, &handleMessage<BatteryState, battery>);
        auto statustext_sub = nh.subscribe("mavros/statustext/recv", 1, &handleMessage<mavros_msgs::StatusText, statustext>);
        auto manual_control_sub = nh.subscribe("mavros/manual_control/control", 1, &handleMessage<mavros_msgs::ManualControl, manual_control>);
        auto local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &handleLocalPosition);

        // Setpoint publishers
        position_pub = nh.advertise<PoseStamped>("mavros/setpoint_position/local", 1);
        position_raw_pub = nh.advertise<PositionTarget>("mavros/setpoint_raw/local", 1);
        attitude_pub = nh.advertise<PoseStamped>("mavros/setpoint_attitude/attitude", 1);
        attitude_raw_pub = nh.advertise<AttitudeTarget>("mavros/setpoint_raw/attitude", 1);
        rates_pub = nh.advertise<TwistStamped>("mavros/setpoint_attitude/cmd_vel", 1);
        thrust_pub = nh.advertise<Thrust>("mavros/setpoint_attitude/thrust", 1);

         // Service servers
        auto gt_serv = nh.advertiseService("get_telemetry", &getTelemetry);
        auto na_serv = nh.advertiseService("navigate", &navigate);
        auto ng_serv = nh.advertiseService("navigate_global", &navigateGlobal);
        auto sp_serv = nh.advertiseService("set_position", &setPosition);
        auto sv_serv = nh.advertiseService("set_velocity", &setVelocity);
        auto sa_serv = nh.advertiseService("set_attitude", &setAttitude);
        auto sr_serv = nh.advertiseService("set_rates", &setRates);
        auto ld_serv = nh.advertiseService("land", &land);

        // Setpoint timer
        setpoint_timer = nh.createTimer(ros::Duration(1 / nh_priv.param("setpoint_rate", 30.0)), &publishSetpoint, false, false);

        position_msg.header.frame_id = local_frame;
        position_raw_msg.header.frame_id = local_frame;
        position_raw_msg.coordinate_frame = PositionTarget::FRAME_LOCAL_NED;
        rates_msg.header.frame_id = fcu_frame;

        ROS_INFO("ready");
        ros::spin();
}