setpoint_trajectory.cpp

Go to the documentation of this file.

/*
 * Copyright 2019 Jaeyoung Lim.
 *
 * This file is part of the mavros package and subject to the license terms
 * in the top-level LICENSE file of the mavros repository.
 * https://github.com/mavlink/mavros/tree/master/LICENSE.md
 */

#include <mavros/mavros_plugin.h>
#include <mavros/setpoint_mixin.h>
#include <eigen_conversions/eigen_msg.h>

#include <nav_msgs/Path.h>
#include <mavros_msgs/PositionTarget.h>
#include <mavros_msgs/SetMavFrame.h>
#include <trajectory_msgs/MultiDOFJointTrajectory.h>
#include <trajectory_msgs/MultiDOFJointTrajectoryPoint.h>
#include <std_srvs/Trigger.h>

namespace mavros {
namespace std_plugins {
using mavlink::common::MAV_FRAME;

class SetpointTrajectoryPlugin : public plugin::PluginBase,
        private plugin::SetPositionTargetLocalNEDMixin<SetpointTrajectoryPlugin> {
public:
        SetpointTrajectoryPlugin() : PluginBase(),
                sp_nh("~setpoint_trajectory")
        { }

        void initialize(UAS &uas_) override
        {
                PluginBase::initialize(uas_);

                sp_nh.param<std::string>("frame_id", frame_id, "map");

                local_sub = sp_nh.subscribe("local", 10, &SetpointTrajectoryPlugin::local_cb, this);
                desired_pub = sp_nh.advertise<nav_msgs::Path>("desired", 10);

                trajectory_reset_srv = sp_nh.advertiseService("reset", &SetpointTrajectoryPlugin::reset_cb, this);
                mav_frame_srv = sp_nh.advertiseService("mav_frame", &SetpointTrajectoryPlugin::set_mav_frame_cb, this);
                
                sp_timer = sp_nh.createTimer(ros::Duration(0.01), &SetpointTrajectoryPlugin::reference_cb, this, true);

                // mav_frame
                std::string mav_frame_str;
                if (!sp_nh.getParam("mav_frame", mav_frame_str)) {
                        mav_frame = MAV_FRAME::LOCAL_NED;
                } else {
                        mav_frame = utils::mav_frame_from_str(mav_frame_str);
                }
        }

        Subscriptions get_subscriptions() override
        {
                return { /* Rx disabled */ };
        }

private:
        friend class SetPositionTargetLocalNEDMixin;
        using lock_guard = std::lock_guard<std::mutex>;
        std::mutex mutex;

        ros::NodeHandle sp_nh;

        ros::Timer sp_timer;

        ros::Subscriber local_sub;
        ros::Publisher desired_pub;

        ros::ServiceServer trajectory_reset_srv;
        ros::ServiceServer mav_frame_srv;

        trajectory_msgs::MultiDOFJointTrajectory::ConstPtr trajectory_target_msg;
        std::vector<trajectory_msgs::MultiDOFJointTrajectoryPoint>::const_iterator setpoint_target;
        std::vector<trajectory_msgs::MultiDOFJointTrajectoryPoint>::const_iterator next_setpoint_target;

        std::string frame_id;
        MAV_FRAME mav_frame;
        ftf::StaticTF transform;

        void reset_timer(ros::Duration duration){
                sp_timer.stop();
                sp_timer.setPeriod(duration);
                sp_timer.start();               
        }

        void publish_path(const trajectory_msgs::MultiDOFJointTrajectory::ConstPtr &req){
                nav_msgs::Path msg;

                msg.header.stamp = ros::Time::now();
                msg.header.frame_id = frame_id;
                for (const auto &p : req->points) {
                        if (p.transforms.empty())
                                continue;
        
                        geometry_msgs::PoseStamped pose_msg;
                        pose_msg.pose.position.x = p.transforms[0].translation.x;
                        pose_msg.pose.position.y = p.transforms[0].translation.y;
                        pose_msg.pose.position.z = p.transforms[0].translation.z;
                        pose_msg.pose.orientation = p.transforms[0].rotation;
                        msg.poses.emplace_back(pose_msg);
                }
                desired_pub.publish(msg);
        }

        /* -*- callbacks -*- */

        void local_cb(const trajectory_msgs::MultiDOFJointTrajectory::ConstPtr &req)
        {
                lock_guard lock(mutex);

                if(static_cast<MAV_FRAME>(mav_frame) == MAV_FRAME::BODY_NED || static_cast<MAV_FRAME>(mav_frame) == MAV_FRAME::BODY_OFFSET_NED){
                        transform = ftf::StaticTF::BASELINK_TO_AIRCRAFT;
                } else {
                        transform = ftf::StaticTF::ENU_TO_NED;
                }

                trajectory_target_msg = req;

                // Read first duration of the setpoint and set the timer
                setpoint_target = req->points.cbegin();
                reset_timer(setpoint_target->time_from_start);
                publish_path(req);
        }

        void reference_cb(const ros::TimerEvent &event)
        {
                using mavlink::common::POSITION_TARGET_TYPEMASK;
                lock_guard lock(mutex);

                if(!trajectory_target_msg)
                        return;

                Eigen::Vector3d position, velocity, af;
                Eigen::Quaterniond attitude = Eigen::Quaterniond::Identity();
                float yaw = 0.f;
                float yaw_rate = 0.f;
                uint16_t type_mask = 0;         
                if(!setpoint_target->transforms.empty()){
                        position = ftf::detail::transform_static_frame(ftf::to_eigen(setpoint_target->transforms[0].translation), transform);
                        attitude = ftf::detail::transform_orientation(ftf::to_eigen(setpoint_target->transforms[0].rotation), transform);

                } else {
                        type_mask = type_mask | uint16_t(POSITION_TARGET_TYPEMASK::X_IGNORE)
                                                        | uint16_t(POSITION_TARGET_TYPEMASK::Y_IGNORE)
                                                        | uint16_t(POSITION_TARGET_TYPEMASK::Z_IGNORE)
                                                        | uint16_t(POSITION_TARGET_TYPEMASK::YAW_IGNORE);

                }

                if(!setpoint_target->velocities.empty()){
                        velocity = ftf::detail::transform_static_frame(ftf::to_eigen(setpoint_target->velocities[0].linear), transform);
                        yaw_rate = setpoint_target->velocities[0].angular.z;

                } else {
                        type_mask = type_mask | uint16_t(POSITION_TARGET_TYPEMASK::VX_IGNORE)
                                                        | uint16_t(POSITION_TARGET_TYPEMASK::VY_IGNORE)
                                                        | uint16_t(POSITION_TARGET_TYPEMASK::VZ_IGNORE)
                                                        | uint16_t(POSITION_TARGET_TYPEMASK::YAW_RATE_IGNORE);

                }

                if(!setpoint_target->accelerations.empty()){
                        af = ftf::detail::transform_static_frame(ftf::to_eigen(setpoint_target->accelerations[0].linear), transform);

                } else {
                        type_mask = type_mask | uint16_t(POSITION_TARGET_TYPEMASK::AX_IGNORE)
                                                        | uint16_t(POSITION_TARGET_TYPEMASK::AY_IGNORE)
                                                        | uint16_t(POSITION_TARGET_TYPEMASK::AZ_IGNORE);

                }

                set_position_target_local_ned(
                                        ros::Time::now().toNSec() / 1000000,
                                        utils::enum_value(mav_frame),
                                        type_mask,
                                        position,
                                        velocity,
                                        af,
                                        ftf::quaternion_get_yaw(attitude),
                                        yaw_rate);

                next_setpoint_target = setpoint_target + 1;
                if (next_setpoint_target != trajectory_target_msg->points.cend()) {
                        reset_timer(setpoint_target->time_from_start);
                        setpoint_target = next_setpoint_target;
                } else {
                        trajectory_target_msg.reset();
                }

                return;
        }

        bool reset_cb(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
        {
                if(trajectory_target_msg){
                        trajectory_target_msg.reset();
                        res.success = true;
                } else {
                        res.success = false;
                        res.message = "Trajectory reset denied: Empty trajectory";
                }

                return true;
        }

        bool set_mav_frame_cb(mavros_msgs::SetMavFrame::Request &req, mavros_msgs::SetMavFrame::Response &res)
        {
                mav_frame = static_cast<MAV_FRAME>(req.mav_frame);
                const std::string mav_frame_str = utils::to_string(mav_frame);
                sp_nh.setParam("mav_frame", mav_frame_str);
                res.success = true;
                return true;
        }
};
}       // namespace std_plugins
}       // namespace mavros

#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::SetpointTrajectoryPlugin, mavros::plugin::PluginBase)