landing_target.cpp
Go to the documentation of this file.
1 
9 /*
10  * Copyright 2015,2017,2019 Nuno Marques.
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/utils.h>
18 #include <mavros/mavros_plugin.h>
19 #include <mavros/setpoint_mixin.h>
22 
23 #include <geometry_msgs/PoseStamped.h>
24 #include <geometry_msgs/Vector3Stamped.h>
25 #include <mavros_msgs/LandingTarget.h>
26 
27 namespace mavros {
28 namespace extra_plugins {
29 using mavlink::common::MAV_FRAME;
30 using mavlink::common::LANDING_TARGET_TYPE;
31 
39  private plugin::TF2ListenerMixin<LandingTargetPlugin> {
40 public:
42  nh("~landing_target"),
43  tf_rate(10.0),
44  send_tf(true),
45  listen_tf(false),
46  listen_lt(false),
47  mav_frame("LOCAL_NED"),
48  target_size_x(1.0),
49  target_size_y(1.0),
50  image_width(640),
51  image_height(480),
52  fov_x(2.0071286398),
53  fov_y(2.0071286398),
54  focal_length(2.8),
55  land_target_type("VISION_FIDUCIAL")
56  { }
57 
58  void initialize(UAS &uas_) override
59  {
61 
62  // general params
63  nh.param<std::string>("frame_id", frame_id, "landing_target_1");
64  nh.param("listen_lt", listen_lt, false); // subscribe to raw LadingTarget msg?
65  nh.param<std::string>("mav_frame", mav_frame, "LOCAL_NED");
66  frame = utils::mav_frame_from_str(mav_frame); // MAV_FRAME index based on given frame name (If unknown, defaults to GENERIC)
67 
68  nh.param<std::string>("land_target_type", land_target_type, "VISION_FIDUCIAL");
69  type = utils::landing_target_type_from_str(land_target_type); // LANDING_TARGET_TYPE index based on given type name (If unknown, defaults to LIGHT_BEACON)
70 
71  // target size
72  nh.param("target_size/x", target_size_x, 1.0); // [meters]
73  nh.param("target_size/y", target_size_y, 1.0);
74 
75  // image size
76  nh.param<int>("image/width", image_width, 640); // [pixels]
77  nh.param<int>("image/height", image_height, 480);
78 
79  // camera field-of-view -> should be precised using the calibrated camera intrinsics
80  nh.param("camera/fov_x", fov_x, 2.0071286398); // default: 115 [degrees]
81  nh.param("camera/fov_y", fov_y, 2.0071286398);
82  // camera focal length
83  nh.param("camera/focal_length", focal_length, 2.8); //ex: OpenMV Cam M7: 2.8 [mm]
84 
85  // tf subsection
86  nh.param("tf/send", send_tf, true);
87  nh.param("tf/listen", listen_tf, false);
88  nh.param<std::string>("tf/frame_id", tf_frame_id, frame_id);
89  nh.param<std::string>("tf/child_frame_id", tf_child_frame_id, "camera_center");
90  nh.param("tf/rate_limit", tf_rate, 50.0);
91 
92  land_target_pub = nh.advertise<geometry_msgs::PoseStamped>("pose_in", 10);
93  lt_marker_pub = nh.advertise<geometry_msgs::Vector3Stamped>("lt_marker", 10);
94 
95  if (listen_tf) { // Listen to transform
96  ROS_INFO_STREAM_NAMED("landing_target", "Listen to landing_target transform " << tf_frame_id
97  << " -> " << tf_child_frame_id);
98  tf2_start("LandingTargetTF", &LandingTargetPlugin::transform_cb);
99  }
100  else if (listen_lt) { // Subscribe to LandingTarget msg
102  }
103  else { // Subscribe to PoseStamped msg
104  pose_sub = nh.subscribe("pose", 10, &LandingTargetPlugin::pose_cb, this);
105  }
106  }
107 
109  {
110  return {
112  };
113  }
114 
115 private:
116  friend class TF2ListenerMixin;
118 
119  bool send_tf;
120  bool listen_tf;
121  double tf_rate;
123 
124  bool listen_lt;
125 
126  std::string frame_id;
127  std::string tf_frame_id;
128  std::string tf_child_frame_id;
129 
134 
136  double fov_x, fov_y;
137  double focal_length;
139 
141  std::string mav_frame;
142 
143  LANDING_TARGET_TYPE type;
144  std::string land_target_type;
145 
146  /* -*- low-level send -*- */
147  void landing_target(uint64_t time_usec,
148  uint8_t target_num,
149  uint8_t frame,
150  Eigen::Vector2f angle,
151  float distance,
152  Eigen::Vector2f size,
153  Eigen::Vector3d pos,
154  Eigen::Quaterniond q,
155  uint8_t type,
156  uint8_t position_valid)
157  {
158  mavlink::common::msg::LANDING_TARGET lt {};
159 
160  lt.time_usec = time_usec;
161  lt.target_num = target_num;
162  lt.frame = frame;
163  lt.distance = distance;
164  lt.type = type;
165  lt.position_valid = position_valid;
166  lt.angle_x = angle.x();
167  lt.angle_y = angle.y();
168  lt.size_x = size.x();
169  lt.size_y = size.y();
170  lt.x = pos.x();
171  lt.y = pos.y();
172  lt.z = pos.z();
173 
175 
176  UAS_FCU(m_uas)->send_message_ignore_drop(lt);
177  }
178 
179  /* -*- mid-level helpers -*- */
197  void inline cartesian_to_displacement(const Eigen::Vector3d &pos, Eigen::Vector2f &angle) {
198  float angle_rad = atan(pos.y() / pos.x()) * (M_PI / 180.0);
199 
200  if (pos.x() > 0 && pos.y() > 0) {
201  angle.x() = angle_rad;
202  angle.y() = -angle_rad;
203  }
204  else if (pos.x() < 0 && pos.y() > 0) {
205  angle.x() = M_PI - angle_rad;
206  angle.y() = angle_rad;
207  }
208  else if (pos.x() < 0 && pos.y() < 0) {
209  angle.x() = M_PI + angle_rad;
210  angle.y() = M_PI - angle_rad;
211  }
212  else if (pos.x() > 0 && pos.y() < 0) {
213  angle.x() = -angle_rad;
214  angle.y() = M_PI + angle_rad;
215  }
216  }
217 
221  void send_landing_target(const ros::Time &stamp, const Eigen::Affine3d &tr) {
227  auto pos = ftf::transform_frame_enu_ned(Eigen::Vector3d(tr.translation()));
228 
231  ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation())));
232 
233  Eigen::Vector2f angle;
234  Eigen::Vector2f size_rad;
235  Eigen::Vector2f fov;
236 
237  // the norm of the position vector is considered the distance to the landing target
238  float distance = pos.norm();
239 
240  // if the landing target type is a vision type, compute the angular offsets
241  if (land_target_type.find("VISION")) {
248  angle.x() = (pos.x() - image_width / 2.0) * fov.x() / image_width;
249  angle.y() = (pos.y() - image_height / 2.0) * fov.y() / image_height;
255  size_rad = {2 * (M_PI / 180.0) * atan(target_size_x / (2 * focal_length)),
256  2 * (M_PI / 180.0) * atan(target_size_y / (2 * focal_length))};
257  }
258  // else, the same values are computed considering the displacement relative to X and Y axes of the camera frame reference
259  else {
261  size_rad = {2 * (M_PI / 180.0) * atan(target_size_x / (2 * distance)),
262  2 * (M_PI / 180.0) * atan(target_size_y / (2 * distance))};
263  }
264 
265  if (last_transform_stamp == stamp) {
266  ROS_DEBUG_THROTTLE_NAMED(10, "landing_target", "LT: Same transform as last one, dropped.");
267  return;
268  }
269  last_transform_stamp = stamp;
270 
271  auto rpy = ftf::quaternion_to_rpy(q);
272 
273  // the last char of frame_id is considered the number of the target
274  uint8_t id = static_cast<uint8_t>(frame_id.back());
275 
276  ROS_DEBUG_THROTTLE_NAMED(10, "landing_target", "Tx landing target: "
277  "ID: %d frame: %s angular offset: X:%1.3frad, Y:%1.3frad) "
278  "distance: %1.3fm position: X:%1.3fm, Y:%1.3fm, Z:%1.3fm) "
279  "orientation: roll:%1.4frad pitch:%1.4frad yaw:%1.4frad "
280  "size: X:%1.3frad by Y:%1.3frad type: %s",
281  id, utils::to_string(static_cast<MAV_FRAME>(frame)).c_str(),
282  angle.x(), angle.y(), distance, pos.x(), pos.y(), pos.z(),
283  rpy.x(), rpy.y(), rpy.z(), size_rad.x(), size_rad.y(),
284  utils::to_string(static_cast<LANDING_TARGET_TYPE>(type)).c_str());
285 
286  landing_target(stamp.toNSec() / 1000,
287  id,
288  utils::enum_value(frame), // by default, in LOCAL_NED
289  angle,
290  distance,
291  size_rad,
292  pos,
293  q,
294  utils::enum_value(type),
295  1); // position is valid from the first received msg
296  }
297 
301  void handle_landing_target(const mavlink::mavlink_message_t *msg, mavlink::common::msg::LANDING_TARGET &land_target) {
303  auto position = ftf::transform_frame_ned_enu(Eigen::Vector3d(land_target.x, land_target.y, land_target.z));
306  Eigen::Quaterniond(land_target.q[0], land_target.q[1], land_target.q[2], land_target.q[3])));
307 
308  auto rpy = ftf::quaternion_to_rpy(orientation);
309 
310  ROS_DEBUG_THROTTLE_NAMED(10, "landing_target", "Rx landing target: "
311  "ID: %d frame: %s angular offset: X:%1.3frad, Y:%1.3frad) "
312  "distance: %1.3fm position: X:%1.3fm, Y:%1.3fm, Z:%1.3fm) "
313  "orientation: roll:%1.4frad pitch:%1.4frad yaw:%1.4frad "
314  "size: X:%1.3frad by Y:%1.3frad type: %s",
315  land_target.target_num, utils::to_string(static_cast<MAV_FRAME>(land_target.frame)).c_str(),
316  land_target.angle_x, land_target.angle_y, land_target.distance,
317  position.x(), position.y(), position.z(), rpy.x(), rpy.y(), rpy.z(), land_target.size_x, land_target.size_y,
318  utils::to_string(static_cast<LANDING_TARGET_TYPE>(land_target.type)).c_str());
319 
320  auto pose = boost::make_shared<geometry_msgs::PoseStamped>();
321  pose->header = m_uas->synchronized_header(frame_id, land_target.time_usec);
322 
323  tf::pointEigenToMsg(position, pose->pose.position);
324  tf::quaternionEigenToMsg(orientation, pose->pose.orientation);
325 
326  land_target_pub.publish(pose);
327 
328  if (send_tf) {
329  geometry_msgs::TransformStamped transform;
330 
331  transform.header.stamp = pose->header.stamp;
332  transform.header.frame_id = "landing_target_" + boost::lexical_cast<std::string>(land_target.target_num);
333  transform.child_frame_id = tf_child_frame_id;
334 
335  transform.transform.rotation = pose->pose.orientation;
336  tf::vectorEigenToMsg(position, transform.transform.translation);
337 
338  m_uas->tf2_broadcaster.sendTransform(transform);
339  }
340 
341  auto tg_size_msg = boost::make_shared<geometry_msgs::Vector3Stamped>();
342  Eigen::Vector3d target_size(target_size_x, target_size_y, 0.0);
343 
344  tf::vectorEigenToMsg(target_size, tg_size_msg->vector);
345 
346  lt_marker_pub.publish(tg_size_msg);
347  }
348 
349  /* -*- callbacks -*- */
353  void transform_cb(const geometry_msgs::TransformStamped &transform) {
354  Eigen::Affine3d tr;
355  tf::transformMsgToEigen(transform.transform, tr);
356 
357  send_landing_target(transform.header.stamp, tr);
358  }
359 
364  Eigen::Affine3d tr;
365  tf::poseMsgToEigen(req->pose, tr);
366 
367  send_landing_target(req->header.stamp, tr);
368  }
369 
375  Eigen::Affine3d tr;
376  tf::poseMsgToEigen(req->pose, tr);
377 
379  auto position = ftf::transform_frame_enu_ned(Eigen::Vector3d(tr.translation()));
380  auto orientation = ftf::transform_orientation_enu_ned(
381  ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation())));
382 
383  landing_target( req->header.stamp.toNSec() / 1000,
384  req->target_num,
385  req->frame, // by default, in LOCAL_NED
386  Eigen::Vector2f(req->angle[0], req->angle[1]),
387  req->distance,
388  Eigen::Vector2f(req->size[0], req->size[1]),
389  position,
390  orientation,
391  req->type,
392  1); // position is valid from the first received msg
393  }
394 };
395 } // namespace extra_plugins
396 } // namespace mavros
397 
void send_landing_target(const ros::Time &stamp, const Eigen::Affine3d &tr)
Send landing target transform to FCU.
std::shared_ptr< MAVConnInterface const > ConstPtr
std_msgs::Header synchronized_header(const std::string &frame_id, const T time_stamp)
mavlink::common::MAV_FRAME mav_frame_from_str(const std::string &mav_frame)
void publish(const boost::shared_ptr< M > &message) const
ssize_t pos
void quaternionEigenToMsg(const Eigen::Quaterniond &e, geometry_msgs::Quaternion &m)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
#define ROS_DEBUG_THROTTLE_NAMED(rate, name,...)
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
HandlerInfo make_handler(const mavlink::msgid_t id, void(_C::*fn)(const mavlink::mavlink_message_t *msg, const mavconn::Framing framing))
void tf2_start(const char *_thd_name, void(LandingTargetPlugin::*cbp)(const geometry_msgs::TransformStamped &))
void quaternion_to_mavlink(const Eigen::Quaternion< _Scalar > &q, std::array< float, 4 > &qmsg)
void landing_target(uint64_t time_usec, uint8_t target_num, uint8_t frame, Eigen::Vector2f angle, float distance, Eigen::Vector2f size, Eigen::Vector3d pos, Eigen::Quaterniond q, uint8_t type, uint8_t position_valid)
#define ROS_INFO_STREAM_NAMED(name, args)
TFSIMD_FORCE_INLINE tfScalar distance(const Vector3 &v) const
T transform_orientation_enu_ned(const T &in)
tf2_ros::TransformBroadcaster tf2_broadcaster
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
void handle_landing_target(const mavlink::mavlink_message_t *msg, mavlink::common::msg::LANDING_TARGET &land_target)
Receive landing target from FCU.
void transform_cb(const geometry_msgs::TransformStamped &transform)
callback for TF2 listener
INLINE Rall1d< T, V, S > atan(const Rall1d< T, V, S > &x)
Eigen::Vector3d quaternion_to_rpy(const Eigen::Quaterniond &q)
void vectorEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Vector3 &m)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
#define UAS_FCU(uasobjptr)
std::string to_string(timesync_mode e)
uint64_t toNSec() const
T transform_frame_ned_enu(const T &in)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void sendTransform(const geometry_msgs::TransformStamped &transform)
uint8_t size
void pointEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Point &m)
T transform_frame_enu_ned(const T &in)
void transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Affine3d &e)
std::vector< HandlerInfo > Subscriptions
ftf::StaticTF transform
T transform_orientation_ned_enu(const T &in)
T transform_orientation_baselink_aircraft(const T &in)
void initialize(UAS &uas_) override
T transform_orientation_aircraft_baselink(const T &in)
void cartesian_to_displacement(const Eigen::Vector3d &pos, Eigen::Vector2f &angle)
Displacement: (not to be mixed with angular displacement)
mavlink::common::LANDING_TARGET_TYPE landing_target_type_from_str(const std::string &landing_target_type)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
void landtarget_cb(const mavros_msgs::LandingTarget::ConstPtr &req)
callback for raw LandingTarget msgs topic - useful if one has the data processed in another node ...
void pose_cb(const geometry_msgs::PoseStamped::ConstPtr &req)
callback for PoseStamped msgs topic
constexpr std::underlying_type< _T >::type enum_value(_T e)


mavros_extras
Author(s): Vladimir Ermakov , Amilcar Lucas
autogenerated on Tue Jun 1 2021 02:36:36