Go to the documentation of this file.00001
00010
00011
00012
00013
00014
00015
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
00044 vel_sub = sp_nh.subscribe("cmd_vel", 10, &SetpointVelocityPlugin::vel_cb, this);
00045 }
00046
00047 const message_map get_rx_handlers() {
00048 return { };
00049 }
00050
00051 private:
00052 friend class SetPositionTargetLocalNEDMixin;
00053 ros::NodeHandle sp_nh;
00054 UAS *uas;
00055
00056 ros::Subscriber vel_sub;
00057
00058
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
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 };
00095
00096 PLUGINLIB_EXPORT_CLASS(mavplugin::SetpointVelocityPlugin, mavplugin::MavRosPlugin)