setpoint_position.cpp
Go to the documentation of this file.
00001 
00009 /*
00010  * Copyright 2014 Vladimir Ermakov.
00011  *
00012  * This file is part of the mavros package and subject to the license terms
00013  * in the top-level LICENSE file of the mavros repository.
00014  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
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                 // tf params
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 { /* Rx disabled */ };
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         /* -*- mid-level helpers -*- */
00079 
00085         void send_position_target(const ros::Time &stamp, const Eigen::Affine3d &tr) {
00086                 /* Documentation start from bit 1 instead 0;
00087                  * Ignore velocity and accel vectors, yaw rate.
00088                  *
00089                  * In past versions on PX4 there been bug described in #273.
00090                  * If you got similar issue please try update firmware first.
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         /* -*- callbacks -*- */
00108 
00109         /* common TF listener moved to mixin */
00110         void transform_cb(const geometry_msgs::TransformStamped &transform) {
00111                 Eigen::Affine3d tr;
00112                 // TODO: later, when tf2 5.12 will be released need to revisit and replace this to
00113                 // tf2::convert()
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 };      // namespace mavplugin
00127 
00128 PLUGINLIB_EXPORT_CLASS(mavplugin::SetpointPositionPlugin, mavplugin::MavRosPlugin)


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