setpoint_position.cpp
Go to the documentation of this file.
1 
9 /*
10  * Copyright 2014,2016 Vladimir Ermakov.
11  *
12  * This file is part of the mavros package and subject to the license terms
13  * in the top-level LICENSE file of the mavros repository.
14  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
15  */
16 
17 #include <mavros/mavros_plugin.h>
18 #include <mavros/setpoint_mixin.h>
20 
21 #include <geometry_msgs/PoseStamped.h>
22 
23 #include <mavros_msgs/SetMavFrame.h>
24 #include <geographic_msgs/GeoPoseStamped.h>
25 
26 #include <GeographicLib/Geocentric.hpp>
27 
28 namespace mavros {
29 namespace std_plugins {
30 using mavlink::common::MAV_FRAME;
37  private plugin::SetPositionTargetLocalNEDMixin<SetpointPositionPlugin>,
38  private plugin::SetPositionTargetGlobalIntMixin<SetpointPositionPlugin>,
39  private plugin::TF2ListenerMixin<SetpointPositionPlugin> {
40 public:
41  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
42 
44  sp_nh("~setpoint_position"),
45  spg_nh("~"),
46  tf_rate(50.0),
47  tf_listen(false)
48  { }
49 
50  void initialize(UAS &uas_) override
51  {
52  PluginBase::initialize(uas_);
53 
54  // tf params
55  sp_nh.param("tf/listen", tf_listen, false);
56  sp_nh.param<std::string>("tf/frame_id", tf_frame_id, "map");
57  sp_nh.param<std::string>("tf/child_frame_id", tf_child_frame_id, "target_position");
58  sp_nh.param("tf/rate_limit", tf_rate, 50.0);
59 
60  if (tf_listen) {
61  ROS_INFO_STREAM_NAMED("setpoint", "Listen to position setpoint transform "
62  << tf_frame_id << " -> " << tf_child_frame_id);
64  }
65  else {
67  // Subscriber for goal gps
69  // Subscriber for goal gps but will convert it to local coordinates
71  // subscriber for current gps state, mavros/global_position/global.
72  gps_sub = spg_nh.subscribe("global_position/global", 10, &SetpointPositionPlugin::gps_cb, this);
73  // Subscribe for current local ENU pose.
74  local_sub = spg_nh.subscribe("local_position/pose", 10, &SetpointPositionPlugin::local_cb, this);
75  }
77 
78  // mav_frame
79  std::string mav_frame_str;
80  if (!sp_nh.getParam("mav_frame", mav_frame_str)) {
81  mav_frame = MAV_FRAME::LOCAL_NED;
82  } else {
83  mav_frame = utils::mav_frame_from_str(mav_frame_str);
84  }
85  }
86 
88  {
89  return { /* Rx disabled */ };
90  }
91 
92 private:
95  friend class TF2ListenerMixin;
96 
105 
106  /* Stores current gps state. */
107  //sensor_msgs::NavSatFix current_gps_msg;
108  Eigen::Vector3d current_gps;
109  Eigen::Vector3d current_local_pos;
110  uint32_t old_gps_stamp = 0;
111 
112  std::string tf_frame_id;
113  std::string tf_child_frame_id;
114 
115  bool tf_listen;
116  double tf_rate;
117 
119 
120  /* -*- mid-level helpers -*- */
121 
127  void send_position_target(const ros::Time &stamp, const Eigen::Affine3d &tr)
128  {
129  using mavlink::common::MAV_FRAME;
130 
131  /* Documentation start from bit 1 instead 0;
132  * Ignore velocity and accel vectors, yaw rate.
133  *
134  * In past versions on PX4 there been bug described in #273.
135  * If you got similar issue please try update firmware first.
136  */
137  const uint16_t ignore_all_except_xyz_y = (1 << 11) | (7 << 6) | (7 << 3);
138 
139  auto p = [&] () {
140  if (static_cast<MAV_FRAME>(mav_frame) == MAV_FRAME::BODY_NED || static_cast<MAV_FRAME>(mav_frame) == MAV_FRAME::BODY_OFFSET_NED) {
141  return ftf::transform_frame_baselink_aircraft(Eigen::Vector3d(tr.translation()));
142  } else {
143  return ftf::transform_frame_enu_ned(Eigen::Vector3d(tr.translation()));
144  }
145  } ();
146 
147  auto q = [&] () {
148  if (mav_frame == MAV_FRAME::BODY_NED || mav_frame == MAV_FRAME::BODY_OFFSET_NED) {
149  return ftf::transform_orientation_absolute_frame_aircraft_baselink(Eigen::Quaterniond(tr.rotation()));
150  } else {
152  ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation())));
153  }
154  } ();
155 
156  set_position_target_local_ned(stamp.toNSec() / 1000000,
157  utils::enum_value(mav_frame),
158  ignore_all_except_xyz_y,
159  p,
160  Eigen::Vector3d::Zero(),
161  Eigen::Vector3d::Zero(),
162  ftf::quaternion_get_yaw(q), 0.0);
163  }
164 
165  /* -*- callbacks -*- */
166 
167  /* common TF listener moved to mixin */
168  void transform_cb(const geometry_msgs::TransformStamped &transform)
169  {
170  Eigen::Affine3d tr;
171  // TODO: later, when tf2 5.12 will be released need to revisit and replace this to
172  // tf2::convert()
173  tf::transformMsgToEigen(transform.transform, tr);
174 
175  send_position_target(transform.header.stamp, tr);
176  }
177 
179  {
180  Eigen::Affine3d tr;
181  tf::poseMsgToEigen(req->pose, tr);
182 
183  send_position_target(req->header.stamp, tr);
184  }
185 
190  {
191  using mavlink::common::POSITION_TARGET_TYPEMASK;
192 
193  uint16_t type_mask = uint16_t(POSITION_TARGET_TYPEMASK::VX_IGNORE)
194  | uint16_t(POSITION_TARGET_TYPEMASK::VY_IGNORE)
195  | uint16_t(POSITION_TARGET_TYPEMASK::VZ_IGNORE)
196  | uint16_t(POSITION_TARGET_TYPEMASK::AX_IGNORE)
197  | uint16_t(POSITION_TARGET_TYPEMASK::AY_IGNORE)
198  | uint16_t(POSITION_TARGET_TYPEMASK::AZ_IGNORE);
199 
200  Eigen::Quaterniond attitude;
201  tf::quaternionMsgToEigen(req->pose.orientation, attitude);
202  Eigen::Quaterniond q = ftf::transform_orientation_enu_ned(
204 
206  req->header.stamp.toNSec() / 1000000,
207  uint8_t(MAV_FRAME::GLOBAL_INT),
208  type_mask,
209  req->pose.position.latitude * 1e7,
210  req->pose.position.longitude * 1e7,
211  req->pose.position.altitude,
212  Eigen::Vector3d::Zero(),
213  Eigen::Vector3d::Zero(),
215  0);
216  }
217 
222  {
231  GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
232 
233  Eigen::Vector3d goal_gps(req->pose.position.latitude, req->pose.position.longitude, req->pose.position.altitude);
234 
235  // current gps -> curent ECEF
236  Eigen::Vector3d current_ecef;
237  earth.Forward(current_gps.x(), current_gps.y(), current_gps.z(),
238  current_ecef.x(), current_ecef.y(), current_ecef.z());
239 
240  // goal gps -> goal ECEF
241  Eigen::Vector3d goal_ecef;
242  earth.Forward(goal_gps.x(), goal_gps.y(), goal_gps.z(),
243  goal_ecef.x(), goal_ecef.y(), goal_ecef.z());
244 
245  // get ENU offset from ECEF offset
246  Eigen::Vector3d ecef_offset = goal_ecef - current_ecef;
247  Eigen::Vector3d enu_offset = ftf::transform_frame_ecef_enu(ecef_offset, current_gps);
248 
249  // prepare yaw angle
250  Eigen::Affine3d sp; // holds position setpoint
251  Eigen::Quaterniond q; // holds desired yaw
252 
253  tf::quaternionMsgToEigen(req->pose.orientation, q);
254 
255  // set position setpoint
256  sp.translation() = current_local_pos + enu_offset;
257  // set desired orientation
258  sp.linear() = q.toRotationMatrix();
259 
260  // Only send if current gps is updated, to avoid divergence
261  if ((req->header.stamp.toNSec() / 1000000) > old_gps_stamp) {
262  old_gps_stamp = req->header.stamp.toNSec() / 1000000;
263  send_position_target(req->header.stamp, sp);
264  }
265  else {
266  ROS_WARN_THROTTLE_NAMED(10, "spgp", "SPG: sp not sent.");
267  }
268  }
269 
274  {
275  current_gps = {msg->latitude, msg->longitude, msg->altitude};
276  }
277 
282  {
283  current_local_pos = ftf::to_eigen(msg->pose.position);
284  }
285 
286  bool set_mav_frame_cb(mavros_msgs::SetMavFrame::Request &req, mavros_msgs::SetMavFrame::Response &res)
287  {
288  mav_frame = static_cast<MAV_FRAME>(req.mav_frame);
289  const std::string mav_frame_str = utils::to_string(mav_frame);
290  sp_nh.setParam("mav_frame", mav_frame_str);
291  res.success = true;
292  return true;
293  }
294 };
295 } // namespace std_plugins
296 } // namespace mavros
297 
MAVROS Plugin base class.
Definition: mavros_plugin.h:36
std::shared_ptr< MAVConnInterface const > ConstPtr
#define ROS_WARN_THROTTLE_NAMED(rate, name,...)
ros::Subscriber setpointg_sub
Global setpoint.
mavlink::common::MAV_FRAME mav_frame_from_str(const std::string &mav_frame)
Retreive MAV_FRAME from name.
bool set_mav_frame_cb(mavros_msgs::SetMavFrame::Request &req, mavros_msgs::SetMavFrame::Response &res)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
MAVROS Plugin base.
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
This mixin adds TF2 listener thread to plugin.
uint32_t old_gps_stamp
old time gps time stamp in [ms], to check if new gps msg is received
void setpointg2l_cb(const geographic_msgs::GeoPoseStamped::ConstPtr &req)
void tf2_start(const char *_thd_name, void(SetpointPositionPlugin::*cbp)(const geometry_msgs::TransformStamped &))
start tf listener
#define ROS_INFO_STREAM_NAMED(name, args)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
void set_position_target_local_ned(uint32_t time_boot_ms, uint8_t coordinate_frame, uint16_t type_mask, Eigen::Vector3d p, Eigen::Vector3d v, Eigen::Vector3d af, float yaw, float yaw_rate)
Message specification: https://mavlink.io/en/messages/common.html#SET_POSITION_TARGET_LOCAL_NED.
void setpoint_cb(const geometry_msgs::PoseStamped::ConstPtr &req)
T transform_orientation_enu_ned(const T &in)
Transform from attitude represented WRT ENU frame to attitude represented WRT NED frame...
Definition: frame_tf.h:169
void local_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
Subscriptions get_subscriptions() override
Return vector of MAVLink message subscriptions (handlers)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SetpointPositionPlugin()
PluginBase()
Plugin constructor Should not do anything before initialize()
Definition: mavros_plugin.h:74
T transform_orientation_absolute_frame_aircraft_baselink(const T &in)
Transform from attitude represented WRT aircraft frame to attitude represented WRT base_link frame...
Definition: frame_tf.h:197
UAS for plugins.
Definition: mavros_uas.h:67
bool param(const std::string &param_name, T &param_val, const T &default_val) const
Eigen::Vector3d current_gps
geodetic coordinates LLA
Mixin for setpoint plugins.
std::string to_string(timesync_mode e)
uint64_t toNSec() const
Eigen::Vector3d to_eigen(const geometry_msgs::Point r)
Helper to convert common ROS geometry_msgs::Point to Eigen::Vector3d.
Definition: frame_tf.h:452
T transform_frame_baselink_aircraft(const T &in)
Transform data expressed in Baselink frame to Aircraft frame.
Definition: frame_tf.h:242
T transform_frame_enu_ned(const T &in)
Transform data expressed in ENU to NED frame.
Definition: frame_tf.h:224
void transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Affine3d &e)
void gps_cb(const sensor_msgs::NavSatFix::ConstPtr &msg)
void transform_cb(const geometry_msgs::TransformStamped &transform)
void setpointg_cb(const geographic_msgs::GeoPoseStamped::ConstPtr &req)
This mixin adds set_position_target_local_ned()
std::vector< HandlerInfo > Subscriptions
Subscriptions vector.
Definition: mavros_plugin.h:47
void set_position_target_global_int(uint32_t time_boot_ms, uint8_t coordinate_frame, uint16_t type_mask, int32_t lat_int, int32_t lon_int, float alt, Eigen::Vector3d v, Eigen::Vector3d af, float yaw, float yaw_rate)
Message specification: https://mavlink.io/en/messages/common.html#SET_POSITION_TARGET_GLOBAL_INT.
double quaternion_get_yaw(const Eigen::Quaterniond &q)
Get Yaw angle from quaternion.
bool getParam(const std::string &key, std::string &s) const
T transform_frame_ecef_enu(const T &in, const T &map_origin)
Transform data expressed in ECEF frame to ENU frame.
Definition: frame_tf.h:254
ros::Subscriber setpointg2l_sub
Global setpoint converted to local setpoint.
void quaternionMsgToEigen(const geometry_msgs::Quaternion &m, Eigen::Quaterniond &e)
T transform_orientation_baselink_aircraft(const T &in)
Transform from attitude represented WRT baselink frame to attitude represented WRT body frame...
Definition: frame_tf.h:187
void send_position_target(const ros::Time &stamp, const Eigen::Affine3d &tr)
Send setpoint to FCU position controller.
This mixin adds set_position_target_global_int()
void initialize(UAS &uas_) override
Plugin initializer.
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
ros::NodeHandle spg_nh
to get local position and gps coord which are not under sp_h()
ros::Subscriber local_sub
current local ENU
constexpr std::underlying_type< _T >::type enum_value(_T e)
Definition: utils.h:55
Eigen::Vector3d current_local_pos
Current local position in ENU.


mavros
Author(s): Vladimir Ermakov
autogenerated on Tue Jun 1 2021 02:36:26