Go to the documentation of this file.00001
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include <mavros/mavros_plugin.h>
00018 #include <mavros/setpoint_mixin.h>
00019 #include <pluginlib/class_list_macros.h>
00020 #include <eigen_conversions/eigen_msg.h>
00021
00022 #include <geometry_msgs/PoseStamped.h>
00023
00024 namespace mavplugin {
00030 class SetpointPositionPlugin : public MavRosPlugin,
00031 private SetPositionTargetLocalNEDMixin<SetpointPositionPlugin>,
00032 private TF2ListenerMixin<SetpointPositionPlugin> {
00033 public:
00034 SetpointPositionPlugin() :
00035 sp_nh("~setpoint_position"),
00036 uas(nullptr),
00037 tf_rate(10.0)
00038 { };
00039
00040 void initialize(UAS &uas_)
00041 {
00042 bool tf_listen;
00043
00044 uas = &uas_;
00045
00046
00047 sp_nh.param("tf/listen", tf_listen, false);
00048 sp_nh.param<std::string>("tf/frame_id", tf_frame_id, "map");
00049 sp_nh.param<std::string>("tf/child_frame_id", tf_child_frame_id, "aircraft");
00050 sp_nh.param("tf/rate_limit", tf_rate, 50.0);
00051
00052 if (tf_listen) {
00053 ROS_INFO_STREAM_NAMED("setpoint", "Listen to position setpoint transform " << tf_frame_id
00054 << " -> " << tf_child_frame_id);
00055 tf2_start("PositionSpTF", &SetpointPositionPlugin::transform_cb);
00056 }
00057 else {
00058 setpoint_sub = sp_nh.subscribe("local", 10, &SetpointPositionPlugin::setpoint_cb, this);
00059 }
00060 }
00061
00062 const message_map get_rx_handlers() {
00063 return { };
00064 }
00065
00066 private:
00067 friend class SetPositionTargetLocalNEDMixin;
00068 friend class TF2ListenerMixin;
00069 ros::NodeHandle sp_nh;
00070 UAS *uas;
00071
00072 ros::Subscriber setpoint_sub;
00073
00074 std::string tf_frame_id;
00075 std::string tf_child_frame_id;
00076 double tf_rate;
00077
00078
00079
00085 void send_position_target(const ros::Time &stamp, const Eigen::Affine3d &tr) {
00086
00087
00088
00089
00090
00091
00092 const uint16_t ignore_all_except_xyz_y = (1 << 11) | (7 << 6) | (7 << 3);
00093
00094 auto p = UAS::transform_frame_enu_ned(Eigen::Vector3d(tr.translation()));
00095 auto q = UAS::transform_orientation_enu_ned(
00096 UAS::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation())));
00097
00098 set_position_target_local_ned(stamp.toNSec() / 1000000,
00099 MAV_FRAME_LOCAL_NED,
00100 ignore_all_except_xyz_y,
00101 p.x(), p.y(), p.z(),
00102 0.0, 0.0, 0.0,
00103 0.0, 0.0, 0.0,
00104 UAS::quaternion_get_yaw(q), 0.0);
00105 }
00106
00107
00108
00109
00110 void transform_cb(const geometry_msgs::TransformStamped &transform) {
00111 Eigen::Affine3d tr;
00112
00113
00114 tf::transformMsgToEigen(transform.transform, tr);
00115
00116 send_position_target(transform.header.stamp, tr);
00117 }
00118
00119 void setpoint_cb(const geometry_msgs::PoseStamped::ConstPtr &req) {
00120 Eigen::Affine3d tr;
00121 tf::poseMsgToEigen(req->pose, tr);
00122
00123 send_position_target(req->header.stamp, tr);
00124 }
00125 };
00126 };
00127
00128 PLUGINLIB_EXPORT_CLASS(mavplugin::SetpointPositionPlugin, mavplugin::MavRosPlugin)