vision_pose_estimate.cpp
Go to the documentation of this file.
1 
10 /*
11  * Copyright 2014 M.H.Kabir.
12  *
13  * This file is part of the mavros package and subject to the license terms
14  * in the top-level LICENSE file of the mavros repository.
15  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
16  */
17 
18 #include <mavros/mavros_plugin.h>
19 #include <mavros/setpoint_mixin.h>
21 
22 #include <geometry_msgs/PoseStamped.h>
23 #include <geometry_msgs/PoseWithCovarianceStamped.h>
24 
25 namespace mavros {
26 namespace extra_plugins{
35  private plugin::TF2ListenerMixin<VisionPoseEstimatePlugin> {
36 public:
38  sp_nh("~vision_pose"),
39  tf_rate(10.0)
40  { }
41 
42  void initialize(UAS &uas_) override
43  {
45 
46  bool tf_listen;
47 
48  // tf params
49  sp_nh.param("tf/listen", tf_listen, false);
50  sp_nh.param<std::string>("tf/frame_id", tf_frame_id, "map");
51  sp_nh.param<std::string>("tf/child_frame_id", tf_child_frame_id, "vision_estimate");
52  sp_nh.param("tf/rate_limit", tf_rate, 10.0);
53 
54  if (tf_listen) {
55  ROS_INFO_STREAM_NAMED("vision_pose", "Listen to vision transform " << tf_frame_id
56  << " -> " << tf_child_frame_id);
58  }
59  else {
62  }
63  }
64 
66  {
67  return { /* Rx disabled */ };
68  }
69 
70 private:
71  friend class TF2ListenerMixin;
73 
76 
77  std::string tf_frame_id;
78  std::string tf_child_frame_id;
79  double tf_rate;
81 
82  /* -*- low-level send -*- */
86  void send_vision_estimate(const ros::Time &stamp, const Eigen::Affine3d &tr, const geometry_msgs::PoseWithCovariance::_covariance_type &cov)
87  {
92  if (last_transform_stamp == stamp) {
93  ROS_DEBUG_THROTTLE_NAMED(10, "vision_pose", "Vision: Same transform as last one, dropped.");
94  return;
95  }
96  last_transform_stamp = stamp;
97 
98  auto position = ftf::transform_frame_enu_ned(Eigen::Vector3d(tr.translation()));
99  auto rpy = ftf::quaternion_to_rpy(
101  ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation()))));
102 
103  auto cov_ned = ftf::transform_frame_enu_ned(cov);
104  ftf::EigenMapConstCovariance6d cov_map(cov_ned.data());
105 
106  auto urt_view = Eigen::Matrix<double, 6, 6>(cov_map.triangularView<Eigen::Upper>());
107  ROS_DEBUG_STREAM_NAMED("vision_pose", "Vision: Covariance URT: " << std::endl << urt_view);
108 
109  mavlink::common::msg::VISION_POSITION_ESTIMATE vp{};
110 
111  vp.usec = stamp.toNSec() / 1000;
112  // [[[cog:
113  // for f in "xyz":
114  // cog.outl("vp.%s = position.%s();" % (f, f))
115  // for a, b in zip("xyz", ('roll', 'pitch', 'yaw')):
116  // cog.outl("vp.%s = rpy.%s();" % (b, a))
117  // ]]]
118  vp.x = position.x();
119  vp.y = position.y();
120  vp.z = position.z();
121  vp.roll = rpy.x();
122  vp.pitch = rpy.y();
123  vp.yaw = rpy.z();
124  // [[[end]]] (checksum: 2048daf411780847e77f08fe5a0b9dd3)
125 
126  // just the URT of the 6x6 Pose Covariance Matrix, given
127  // that the matrix is symmetric
128  ftf::covariance_urt_to_mavlink(cov_map, vp.covariance);
129 
130  UAS_FCU(m_uas)->send_message_ignore_drop(vp);
131  }
132 
133  /* -*- callbacks -*- */
134 
135  /* common TF listener moved to mixin */
136 
137  void transform_cb(const geometry_msgs::TransformStamped &transform)
138  {
139  Eigen::Affine3d tr;
140  tf::transformMsgToEigen(transform.transform, tr);
141  ftf::Covariance6d cov {}; // zero initialized
142 
143  send_vision_estimate(transform.header.stamp, tr, cov);
144  }
145 
147  {
148  Eigen::Affine3d tr;
149  tf::poseMsgToEigen(req->pose, tr);
150  ftf::Covariance6d cov {}; // zero initialized
151 
152  send_vision_estimate(req->header.stamp, tr, cov);
153  }
154 
156  {
157  Eigen::Affine3d tr;
158  tf::poseMsgToEigen(req->pose.pose, tr);
159 
160  send_vision_estimate(req->header.stamp, tr, req->pose.covariance);
161  }
162 };
163 } // namespace extra_plugins
164 } // namespace mavros
165 
std::shared_ptr< MAVConnInterface const > ConstPtr
void vision_cov_cb(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &req)
#define ROS_DEBUG_STREAM_NAMED(name, args)
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)
void tf2_start(const char *_thd_name, void(VisionPoseEstimatePlugin::*cbp)(const geometry_msgs::TransformStamped &))
#define ROS_INFO_STREAM_NAMED(name, args)
T transform_orientation_enu_ned(const T &in)
bool tf_listen
Eigen::Vector3d quaternion_to_rpy(const Eigen::Quaterniond &q)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
#define UAS_FCU(uasobjptr)
uint64_t toNSec() const
void covariance_urt_to_mavlink(const T &covmap, std::array< float, ARR_SIZE > &covmsg)
geometry_msgs::PoseWithCovariance::_covariance_type Covariance6d
void vision_cb(const geometry_msgs::PoseStamped::ConstPtr &req)
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
void transform_cb(const geometry_msgs::TransformStamped &transform)
void send_vision_estimate(const ros::Time &stamp, const Eigen::Affine3d &tr, const geometry_msgs::PoseWithCovariance::_covariance_type &cov)
Send vision estimate transform to FCU position controller.
T transform_orientation_baselink_aircraft(const T &in)
void initialize(UAS &uas_) override
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
Eigen::Map< const Eigen::Matrix< double, 6, 6, Eigen::RowMajor > > EigenMapConstCovariance6d


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