setpoint_velocity.cpp

Go to the documentation of this file.

/*
 * Copyright 2014 Nuno Marques.
 *
 * 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 <pluginlib/class_list_macros.h>
#include <eigen_conversions/eigen_msg.h>

#include <geometry_msgs/TwistStamped.h>

namespace mavplugin {
class SetpointVelocityPlugin : public MavRosPlugin,
        private SetPositionTargetLocalNEDMixin<SetpointVelocityPlugin> {
public:
        SetpointVelocityPlugin() :
                sp_nh("~setpoint_velocity"),
                uas(nullptr)
        { };

        void initialize(UAS &uas_)
        {
                uas = &uas_;

                //cmd_vel usually is the topic used for velocity control in many controllers / planners
                vel_sub = sp_nh.subscribe("cmd_vel", 10, &SetpointVelocityPlugin::vel_cb, this);
        }

        const message_map get_rx_handlers() {
                return { /* Rx disabled */ };
        }

private:
        friend class SetPositionTargetLocalNEDMixin;
        ros::NodeHandle sp_nh;
        UAS *uas;

        ros::Subscriber vel_sub;

        /* -*- mid-level helpers -*- */

        void send_setpoint_velocity(const ros::Time &stamp, Eigen::Vector3d &vel_enu, double yaw_rate) {
                uint16_t ignore_all_except_v_xyz_yr = (1 << 10) | (7 << 6) | (7 << 0);

                auto vel = UAS::transform_frame_enu_ned(vel_enu);
                auto yr = UAS::transform_frame_baselink_aircraft(Eigen::Vector3d(0.0, 0.0, yaw_rate));

                set_position_target_local_ned(stamp.toNSec() / 1000000,
                                MAV_FRAME_LOCAL_NED,
                                ignore_all_except_v_xyz_yr,
                                0.0, 0.0, 0.0,
                                vel.x(), vel.y(), vel.z(),
                                0.0, 0.0, 0.0,
                                0.0, yr.z());
        }

        /* -*- callbacks -*- */

        void vel_cb(const geometry_msgs::TwistStamped::ConstPtr &req) {
                Eigen::Vector3d vel_enu;

                tf::vectorMsgToEigen(req->twist.linear, vel_enu);
                send_setpoint_velocity(req->header.stamp, vel_enu,
                                req->twist.angular.z);
        }
};
};      // namespace mavplugin

PLUGINLIB_EXPORT_CLASS(mavplugin::SetpointVelocityPlugin, mavplugin::MavRosPlugin)