setpoint_velocity.cpp
Go to the documentation of this file.
00001 
00010 /*
00011  * Copyright 2014 Nuno Marques.
00012  *
00013  * This file is part of the mavros package and subject to the license terms
00014  * in the top-level LICENSE file of the mavros repository.
00015  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
00016  */
00017 
00018 #include <mavros/mavros_plugin.h>
00019 #include <mavros/setpoint_mixin.h>
00020 #include <pluginlib/class_list_macros.h>
00021 #include <eigen_conversions/eigen_msg.h>
00022 
00023 #include <geometry_msgs/TwistStamped.h>
00024 
00025 namespace mavplugin {
00031 class SetpointVelocityPlugin : public MavRosPlugin,
00032         private SetPositionTargetLocalNEDMixin<SetpointVelocityPlugin> {
00033 public:
00034         SetpointVelocityPlugin() :
00035                 sp_nh("~setpoint_velocity"),
00036                 uas(nullptr)
00037         { };
00038 
00039         void initialize(UAS &uas_)
00040         {
00041                 uas = &uas_;
00042 
00043                 //cmd_vel usually is the topic used for velocity control in many controllers / planners
00044                 vel_sub = sp_nh.subscribe("cmd_vel", 10, &SetpointVelocityPlugin::vel_cb, this);
00045         }
00046 
00047         const message_map get_rx_handlers() {
00048                 return { /* Rx disabled */ };
00049         }
00050 
00051 private:
00052         friend class SetPositionTargetLocalNEDMixin;
00053         ros::NodeHandle sp_nh;
00054         UAS *uas;
00055 
00056         ros::Subscriber vel_sub;
00057 
00058         /* -*- mid-level helpers -*- */
00059 
00065         void send_setpoint_velocity(const ros::Time &stamp, Eigen::Vector3d &vel_enu, double yaw_rate) {
00070                 uint16_t ignore_all_except_v_xyz_yr = (1 << 10) | (7 << 6) | (7 << 0);
00071 
00072                 auto vel = UAS::transform_frame_enu_ned(vel_enu);
00073                 auto yr = UAS::transform_frame_baselink_aircraft(Eigen::Vector3d(0.0, 0.0, yaw_rate));
00074 
00075                 set_position_target_local_ned(stamp.toNSec() / 1000000,
00076                                 MAV_FRAME_LOCAL_NED,
00077                                 ignore_all_except_v_xyz_yr,
00078                                 0.0, 0.0, 0.0,
00079                                 vel.x(), vel.y(), vel.z(),
00080                                 0.0, 0.0, 0.0,
00081                                 0.0, yr.z());
00082         }
00083 
00084         /* -*- callbacks -*- */
00085 
00086         void vel_cb(const geometry_msgs::TwistStamped::ConstPtr &req) {
00087                 Eigen::Vector3d vel_enu;
00088 
00089                 tf::vectorMsgToEigen(req->twist.linear, vel_enu);
00090                 send_setpoint_velocity(req->header.stamp, vel_enu,
00091                                 req->twist.angular.z);
00092         }
00093 };
00094 };      // namespace mavplugin
00095 
00096 PLUGINLIB_EXPORT_CLASS(mavplugin::SetpointVelocityPlugin, mavplugin::MavRosPlugin)


mavros
Author(s): Vladimir Ermakov
autogenerated on Sat Jun 8 2019 19:55:19