local_position.cpp
Go to the documentation of this file.
1 
11 /*
12  * Copyright 2014,2016 Vladimir Ermakov.
13  *
14  * This file is part of the mavros package and subject to the license terms
15  * in the top-level LICENSE file of the mavros repository.
16  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
17  */
18 
19 #include <mavros/mavros_plugin.h>
21 
22 #include <geometry_msgs/AccelWithCovarianceStamped.h>
23 #include <geometry_msgs/PoseStamped.h>
24 #include <geometry_msgs/PoseWithCovarianceStamped.h>
25 #include <geometry_msgs/TwistStamped.h>
26 #include <geometry_msgs/TwistWithCovarianceStamped.h>
27 #include <geometry_msgs/TransformStamped.h>
28 
29 #include <nav_msgs/Odometry.h>
30 
31 namespace mavros {
32 namespace std_plugins {
39 public:
41  lp_nh("~local_position"),
42  tf_send(false),
45  { }
46 
47  void initialize(UAS &uas_) override
48  {
49  PluginBase::initialize(uas_);
50 
51  // header frame_id.
52  // default to map (world-fixed,ENU as per REP-105).
53  lp_nh.param<std::string>("frame_id", frame_id, "map");
54  // Important tf subsection
55  // Report the transform from world to base_link here.
56  lp_nh.param("tf/send", tf_send, false);
57  lp_nh.param<std::string>("tf/frame_id", tf_frame_id, "map");
58  lp_nh.param<std::string>("tf/child_frame_id", tf_child_frame_id, "base_link");
59 
60  local_position = lp_nh.advertise<geometry_msgs::PoseStamped>("pose", 10);
61  local_position_cov = lp_nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose_cov", 10);
62  local_velocity_local = lp_nh.advertise<geometry_msgs::TwistStamped>("velocity_local", 10);
63  local_velocity_body = lp_nh.advertise<geometry_msgs::TwistStamped>("velocity_body", 10);
64  local_velocity_cov = lp_nh.advertise<geometry_msgs::TwistWithCovarianceStamped>("velocity_body_cov", 10);
65  local_accel = lp_nh.advertise<geometry_msgs::AccelWithCovarianceStamped>("accel", 10);
66  local_odom = lp_nh.advertise<nav_msgs::Odometry>("odom",10);
67  }
68 
70  return {
73  };
74  }
75 
76 private:
78 
86 
87  std::string frame_id;
88  std::string tf_frame_id;
89  std::string tf_child_frame_id;
90  bool tf_send;
93 
95  {
96  if (tf_send) {
97  geometry_msgs::TransformStamped transform;
98  transform.header.stamp = odom->header.stamp;
99  transform.header.frame_id = tf_frame_id;
100  transform.child_frame_id = tf_child_frame_id;
101  transform.transform.translation.x = odom->pose.pose.position.x;
102  transform.transform.translation.y = odom->pose.pose.position.y;
103  transform.transform.translation.z = odom->pose.pose.position.z;
104  transform.transform.rotation = odom->pose.pose.orientation;
105  m_uas->tf2_broadcaster.sendTransform(transform);
106  }
107  }
108 
109  void handle_local_position_ned(const mavlink::mavlink_message_t *msg, mavlink::common::msg::LOCAL_POSITION_NED &pos_ned)
110  {
111  has_local_position_ned = true;
112 
113  //--------------- Transform FCU position and Velocity Data ---------------//
114  auto enu_position = ftf::transform_frame_ned_enu(Eigen::Vector3d(pos_ned.x, pos_ned.y, pos_ned.z));
115  auto enu_velocity = ftf::transform_frame_ned_enu(Eigen::Vector3d(pos_ned.vx, pos_ned.vy, pos_ned.vz));
116 
117  //--------------- Get Odom Information ---------------//
118  // Note this orientation describes baselink->ENU transform
119  auto enu_orientation_msg = m_uas->get_attitude_orientation_enu();
120  auto baselink_angular_msg = m_uas->get_attitude_angular_velocity_enu();
121  Eigen::Quaterniond enu_orientation;
122  tf::quaternionMsgToEigen(enu_orientation_msg, enu_orientation);
123  auto baselink_linear = ftf::transform_frame_enu_baselink(enu_velocity, enu_orientation.inverse());
124 
125  auto odom = boost::make_shared<nav_msgs::Odometry>();
126  odom->header = m_uas->synchronized_header(frame_id, pos_ned.time_boot_ms);
127  odom->child_frame_id = tf_child_frame_id;
128 
129  tf::pointEigenToMsg(enu_position, odom->pose.pose.position);
130  odom->pose.pose.orientation = enu_orientation_msg;
131  tf::vectorEigenToMsg(baselink_linear, odom->twist.twist.linear);
132  odom->twist.twist.angular = baselink_angular_msg;
133 
134  // publish odom if we don't have LOCAL_POSITION_NED_COV
135  if (!has_local_position_ned_cov) {
136  local_odom.publish(odom);
137  }
138 
139  // publish pose always
140  auto pose = boost::make_shared<geometry_msgs::PoseStamped>();
141  pose->header = odom->header;
142  pose->pose = odom->pose.pose;
143  local_position.publish(pose);
144 
145  // publish velocity always
146  // velocity in the body frame
147  auto twist_body = boost::make_shared<geometry_msgs::TwistStamped>();
148  twist_body->header.stamp = odom->header.stamp;
149  twist_body->header.frame_id = tf_child_frame_id;
150  twist_body->twist.linear = odom->twist.twist.linear;
151  twist_body->twist.angular = baselink_angular_msg;
152  local_velocity_body.publish(twist_body);
153 
154  // velocity in the local frame
155  auto twist_local = boost::make_shared<geometry_msgs::TwistStamped>();
156  twist_local->header.stamp = twist_body->header.stamp;
157  twist_local->header.frame_id = tf_child_frame_id;
158  tf::vectorEigenToMsg(enu_velocity, twist_local->twist.linear);
159  tf::vectorEigenToMsg(ftf::transform_frame_baselink_enu(ftf::to_eigen(baselink_angular_msg), enu_orientation),
160  twist_local->twist.angular);
161 
162  local_velocity_local.publish(twist_local);
163 
164  // publish tf
165  publish_tf(odom);
166  }
167 
168  void handle_local_position_ned_cov(const mavlink::mavlink_message_t *msg, mavlink::common::msg::LOCAL_POSITION_NED_COV &pos_ned)
169  {
170  has_local_position_ned_cov = true;
171 
172  auto enu_position = ftf::transform_frame_ned_enu(Eigen::Vector3d(pos_ned.x, pos_ned.y, pos_ned.z));
173  auto enu_velocity = ftf::transform_frame_ned_enu(Eigen::Vector3d(pos_ned.vx, pos_ned.vy, pos_ned.vz));
174 
175  auto enu_orientation_msg = m_uas->get_attitude_orientation_enu();
176  auto baselink_angular_msg = m_uas->get_attitude_angular_velocity_enu();
177  Eigen::Quaterniond enu_orientation;
178  tf::quaternionMsgToEigen(enu_orientation_msg, enu_orientation);
179  auto baselink_linear = ftf::transform_frame_enu_baselink(enu_velocity, enu_orientation.inverse());
180 
181  auto odom = boost::make_shared<nav_msgs::Odometry>();
182  odom->header = m_uas->synchronized_header(frame_id, pos_ned.time_usec);
183  odom->child_frame_id = tf_child_frame_id;
184 
185  tf::pointEigenToMsg(enu_position, odom->pose.pose.position);
186  odom->pose.pose.orientation = enu_orientation_msg;
187  tf::vectorEigenToMsg(baselink_linear, odom->twist.twist.linear);
188  odom->twist.twist.angular = baselink_angular_msg;
189 
190  odom->pose.covariance[0] = pos_ned.covariance[0]; // x
191  odom->pose.covariance[7] = pos_ned.covariance[9]; // y
192  odom->pose.covariance[14] = pos_ned.covariance[17]; // z
193 
194  odom->twist.covariance[0] = pos_ned.covariance[24]; // vx
195  odom->twist.covariance[7] = pos_ned.covariance[30]; // vy
196  odom->twist.covariance[14] = pos_ned.covariance[35]; // vz
197  // TODO: orientation + angular velocity covariances from ATTITUDE_QUATERION_COV
198 
199  // publish odom always
200  local_odom.publish(odom);
201 
202  // publish pose_cov always
203  auto pose_cov = boost::make_shared<geometry_msgs::PoseWithCovarianceStamped>();
204  pose_cov->header = odom->header;
205  pose_cov->pose = odom->pose;
206  local_position_cov.publish(pose_cov);
207 
208  // publish velocity_cov always
209  auto twist_cov = boost::make_shared<geometry_msgs::TwistWithCovarianceStamped>();
210  twist_cov->header.stamp = odom->header.stamp;
211  twist_cov->header.frame_id = odom->child_frame_id;
212  twist_cov->twist = odom->twist;
213  local_velocity_cov.publish(twist_cov);
214 
215  // publish pose, velocity, tf if we don't have LOCAL_POSITION_NED
216  if (!has_local_position_ned) {
217  auto pose = boost::make_shared<geometry_msgs::PoseStamped>();
218  pose->header = odom->header;
219  pose->pose = odom->pose.pose;
220  local_position.publish(pose);
221 
222  auto twist = boost::make_shared<geometry_msgs::TwistStamped>();
223  twist->header.stamp = odom->header.stamp;
224  twist->header.frame_id = odom->child_frame_id;
225  twist->twist = odom->twist.twist;
226  local_velocity_body.publish(twist);
227 
228  // publish tf
229  publish_tf(odom);
230  }
231 
232  // publish accelerations
233  auto accel = boost::make_shared<geometry_msgs::AccelWithCovarianceStamped>();
234  accel->header = odom->header;
235 
236  auto enu_accel = ftf::transform_frame_ned_enu(Eigen::Vector3d(pos_ned.ax, pos_ned.ay, pos_ned.az));
237  tf::vectorEigenToMsg(enu_accel, accel->accel.accel.linear);
238 
239  accel->accel.covariance[0] = pos_ned.covariance[39]; // ax
240  accel->accel.covariance[7] = pos_ned.covariance[42]; // ay
241  accel->accel.covariance[14] = pos_ned.covariance[44]; // az
242 
243  local_accel.publish(accel);
244  }
245 };
246 } // namespace std_plugins
247 } // namespace mavros
248 
MAVROS Plugin base class.
Definition: mavros_plugin.h:36
std_msgs::Header synchronized_header(const std::string &frame_id, const T time_stamp)
Create message header from time_boot_ms or time_usec stamps and frame_id.
Definition: mavros_uas.h:375
void publish(const boost::shared_ptr< M > &message) const
geometry_msgs::Quaternion get_attitude_orientation_enu()
Get Attitude orientation quaternion.
Definition: uas_data.cpp:165
void handle_local_position_ned(const mavlink::mavlink_message_t *msg, mavlink::common::msg::LOCAL_POSITION_NED &pos_ned)
MAVROS Plugin base.
HandlerInfo make_handler(const mavlink::msgid_t id, void(_C::*fn)(const mavlink::mavlink_message_t *msg, const mavconn::Framing framing))
Definition: mavros_plugin.h:87
void initialize(UAS &uas_) override
Plugin initializer.
tf2_ros::TransformBroadcaster tf2_broadcaster
Definition: mavros_uas.h:275
void handle_local_position_ned_cov(const mavlink::mavlink_message_t *msg, mavlink::common::msg::LOCAL_POSITION_NED_COV &pos_ned)
T transform_frame_enu_baselink(const T &in, const Eigen::Quaterniond &q)
Transform data expressed in ENU to base_link frame. Assumes quaternion represents rotation from ENU t...
Definition: frame_tf.h:311
PluginBase()
Plugin constructor Should not do anything before initialize()
Definition: mavros_plugin.h:74
Local position plugin. Publish local position to TF, PositionStamped, TwistStamped and Odometry...
UAS for plugins.
Definition: mavros_uas.h:67
Subscriptions get_subscriptions() override
Return vector of MAVLink message subscriptions (handlers)
std::string tf_child_frame_id
frame for TF
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
T transform_frame_ned_enu(const T &in)
Transform data expressed in NED to ENU frame.
Definition: frame_tf.h:215
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
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void sendTransform(const geometry_msgs::TransformStamped &transform)
void pointEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Point &m)
std::vector< HandlerInfo > Subscriptions
Subscriptions vector.
Definition: mavros_plugin.h:47
void quaternionMsgToEigen(const geometry_msgs::Quaternion &m, Eigen::Quaterniond &e)
void publish_tf(boost::shared_ptr< nav_msgs::Odometry > &odom)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
geometry_msgs::Vector3 get_attitude_angular_velocity_enu()
Get angular velocity from IMU data.
Definition: uas_data.cpp:191
T transform_frame_baselink_enu(const T &in, const Eigen::Quaterniond &q)
Transform data expressed in baselink to ENU frame. Assumes quaternion represents rotation from basel_...
Definition: frame_tf.h:320


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