setpoint_attitude.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/PoseStamped.h>
00024 #include <geometry_msgs/TwistStamped.h>
00025 #include <std_msgs/Float64.h>
00026 
00027 namespace mavplugin {
00033 class SetpointAttitudePlugin : public MavRosPlugin,
00034         private TF2ListenerMixin<SetpointAttitudePlugin> {
00035 public:
00036         SetpointAttitudePlugin() :
00037                 sp_nh("~setpoint_attitude"),
00038                 uas(nullptr),
00039                 tf_rate(10.0),
00040                 reverse_throttle(false)
00041         { };
00042 
00043         void initialize(UAS &uas_)
00044         {
00045                 bool tf_listen;
00046 
00047                 uas = &uas_;
00048 
00049                 // main params
00050                 sp_nh.param("reverse_throttle", reverse_throttle, false);
00051                 // tf params
00052                 sp_nh.param("tf/listen", tf_listen, false);
00053                 sp_nh.param<std::string>("tf/frame_id", tf_frame_id, "map");
00054                 sp_nh.param<std::string>("tf/child_frame_id", tf_child_frame_id, "aircraft");
00055                 sp_nh.param("tf/rate_limit", tf_rate, 10.0);
00056 
00057                 if (tf_listen) {
00058                         ROS_INFO_STREAM_NAMED("attitude",
00059                                         "Listen to desired attitude transform "
00060                                         << tf_frame_id << " -> " << tf_child_frame_id);
00061                         tf2_start("AttitudeSpTF", &SetpointAttitudePlugin::transform_cb);
00062                 }
00063                 else {
00064                         twist_sub = sp_nh.subscribe("cmd_vel", 10, &SetpointAttitudePlugin::twist_cb, this);
00065                         pose_sub = sp_nh.subscribe("attitude", 10, &SetpointAttitudePlugin::pose_cb, this);
00066                 }
00067 
00068                 throttle_sub = sp_nh.subscribe("att_throttle", 10, &SetpointAttitudePlugin::throttle_cb, this);
00069         }
00070 
00071         const message_map get_rx_handlers() {
00072                 return { /* Rx disabled */ };
00073         }
00074 
00075 private:
00076         friend class TF2ListenerMixin;
00077         ros::NodeHandle sp_nh;
00078         UAS *uas;
00079 
00080         ros::Subscriber twist_sub;
00081         ros::Subscriber pose_sub;
00082         ros::Subscriber throttle_sub;
00083 
00084         std::string tf_frame_id;
00085         std::string tf_child_frame_id;
00086         double tf_rate;
00087         bool reverse_throttle;
00088 
00089         /* -*- low-level send -*- */
00090 
00091         void set_attitude_target(uint32_t time_boot_ms,
00092                         uint8_t type_mask,
00093                         float q[4],
00094                         float roll_rate, float pitch_rate, float yaw_rate,
00095                         float thrust) {
00096                 mavlink_message_t msg;
00097                 mavlink_msg_set_attitude_target_pack_chan(UAS_PACK_CHAN(uas), &msg,
00098                                 time_boot_ms,
00099                                 UAS_PACK_TGT(uas),
00100                                 type_mask,
00101                                 q,
00102                                 roll_rate, pitch_rate, yaw_rate,
00103                                 thrust);
00104                 UAS_FCU(uas)->send_message(&msg);
00105         }
00106 
00107         /* -*- mid-level helpers -*- */
00108 
00114         void send_attitude_target(const ros::Time &stamp, const Eigen::Affine3d &tr) {
00115                 /* Thrust + RPY, also bits numbering started from 1 in docs
00116                  */
00117                 const uint8_t ignore_all_except_q = (1 << 6) | (7 << 0);
00118                 float q[4];
00119 
00120                 UAS::quaternion_to_mavlink(
00121                                 UAS::transform_orientation_enu_ned(
00122                                         UAS::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation()))),q);
00123 
00124                 set_attitude_target(stamp.toNSec() / 1000000,
00125                                 ignore_all_except_q,
00126                                 q,
00127                                 0.0, 0.0, 0.0,
00128                                 0.0);
00129         }
00130 
00136         void send_attitude_ang_velocity(const ros::Time &stamp, const Eigen::Vector3d &ang_vel) {
00137                 /* Q + Thrust, also bits noumbering started from 1 in docs
00138                  */
00139                 const uint8_t ignore_all_except_rpy = (1 << 7) | (1 << 6);
00140                 float q[4] = { 1.0, 0.0, 0.0, 0.0 };
00141 
00142                 auto av = UAS::transform_frame_baselink_aircraft(ang_vel);
00143 
00144                 set_attitude_target(stamp.toNSec() / 1000000,
00145                                 ignore_all_except_rpy,
00146                                 q,
00147                                 av.x(), av.y(), av.z(),
00148                                 0.0);
00149         }
00150 
00154         void send_attitude_throttle(const float throttle) {
00155                 // Q + RPY
00156                 const uint8_t ignore_all_except_throttle = (1 << 7) | (7 << 0);
00157                 float q[4] = { 1.0, 0.0, 0.0, 0.0 };
00158 
00159                 set_attitude_target(ros::Time::now().toNSec() / 1000000,
00160                                 ignore_all_except_throttle,
00161                                 q,
00162                                 0.0, 0.0, 0.0,
00163                                 throttle);
00164         }
00165 
00166         /* -*- callbacks -*- */
00167 
00168         void transform_cb(const geometry_msgs::TransformStamped &transform) {
00169                 Eigen::Affine3d tr;
00170                 tf::transformMsgToEigen(transform.transform, tr);
00171 
00172                 send_attitude_target(transform.header.stamp, tr);
00173         }
00174 
00175         void pose_cb(const geometry_msgs::PoseStamped::ConstPtr &req) {
00176                 Eigen::Affine3d tr;
00177                 tf::poseMsgToEigen(req->pose, tr);
00178 
00179                 send_attitude_target(req->header.stamp, tr);
00180         }
00181 
00182         void twist_cb(const geometry_msgs::TwistStamped::ConstPtr &req) {
00183                 Eigen::Vector3d ang_vel;
00184                 tf::vectorMsgToEigen(req->twist.angular, ang_vel);
00185 
00186                 send_attitude_ang_velocity(req->header.stamp, ang_vel);
00187         }
00188 
00189         inline bool is_normalized(float throttle, const float min, const float max) {
00190                 if (throttle < min) {
00191                         ROS_WARN_NAMED("attitude", "Not normalized throttle! Thd(%f) < Min(%f)", throttle, min);
00192                         return false;
00193                 }
00194                 else if (throttle > max) {
00195                         ROS_WARN_NAMED("attitude", "Not normalized throttle! Thd(%f) > Max(%f)", throttle, max);
00196                         return false;
00197                 }
00198 
00199                 return true;
00200         }
00201 
00202         void throttle_cb(const std_msgs::Float64::ConstPtr &req) {
00203                 float throttle_normalized = req->data;
00204 
00208                 if (reverse_throttle && !is_normalized(throttle_normalized, -1.0, 1.0))
00209                         return;
00210                 else if (!is_normalized(throttle_normalized, 0.0, 1.0))
00211                         return;
00212 
00213                 send_attitude_throttle(throttle_normalized);
00214         }
00215 };
00216 };      // namespace mavplugin
00217 
00218 PLUGINLIB_EXPORT_CLASS(mavplugin::SetpointAttitudePlugin, mavplugin::MavRosPlugin)


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