fake_gps.cpp
Go to the documentation of this file.
1 
10 /*
11  * Copyright 2015 Christoph Tobler.
12  * Copyright 2017 Nuno Marques.
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>
20 #include <mavros/setpoint_mixin.h>
22 #include <GeographicLib/Geocentric.hpp>
23 #include <GeographicLib/Geoid.hpp>
24 
25 #include <geometry_msgs/PoseStamped.h>
26 #include <geometry_msgs/TransformStamped.h>
27 
28 namespace mavros {
29 namespace extra_plugins {
30 using mavlink::common::GPS_FIX_TYPE;
31 
41  private plugin::TF2ListenerMixin<FakeGPSPlugin> {
42 public:
43  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
44 
46  fp_nh("~fake_gps"),
47  gps_rate(5.0),
48  use_mocap(true),
49  map_origin(0.0, 0.0, 0.0),
50  mocap_transform(true),
51  use_vision(false),
52  tf_listen(false),
53  tf_rate(10.0),
54  eph(2.0),
55  epv(2.0),
57  fix_type(GPS_FIX_TYPE::NO_GPS),
58  // WGS-84 ellipsoid (a - equatorial radius, f - flattening of ellipsoid)
59  earth(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f())
60  { }
61 
62  void initialize(UAS &uas_)
63  {
65 
66  double _gps_rate;
67  double origin_lat, origin_lon, origin_alt;
68 
69  last_pos_time = ros::Time(0.0);
70 
71  // general params
72  int ft_i;
73  fp_nh.param<int>("fix_type", ft_i, utils::enum_value(GPS_FIX_TYPE::NO_GPS));
74  fix_type = static_cast<GPS_FIX_TYPE>(ft_i);
75  fp_nh.param("gps_rate", _gps_rate, 5.0); // GPS data rate of 5hz
76  gps_rate: _gps_rate;
77  fp_nh.param("eph", eph, 2.0);
78  fp_nh.param("epv", epv, 2.0);
79  fp_nh.param<int>("satellites_visible", satellites_visible, 5);
80 
81  // default origin/starting point: Z├╝rich geodetic coordinates
82  fp_nh.param("geo_origin/lat", origin_lat, 47.3667); // [degrees]
83  fp_nh.param("geo_origin/lon", origin_lon, 8.5500); // [degrees]
84  fp_nh.param("geo_origin/alt", origin_alt, 408.0); // [meters - height over the WGS-84 ellipsoid]
85 
86  // init map origin with geodetic coordinates
87  map_origin = {origin_lat, origin_lon, origin_alt};
88 
89  try {
94  earth.Forward(map_origin.x(), map_origin.y(), map_origin.z(),
95  ecef_origin.x(), ecef_origin.y(), ecef_origin.z());
96  }
97  catch (const std::exception& e) {
98  ROS_INFO_STREAM("FGPS: Caught exception: " << e.what() << std::endl);
99  }
100 
101  // source set params
102  fp_nh.param("use_mocap", use_mocap, true); // listen to MoCap source
103  fp_nh.param("mocap_transform", mocap_transform, true); // listen to MoCap source (TransformStamped if true; PoseStamped if false)
104  fp_nh.param("tf/listen", tf_listen, false); // listen to TF source
105  fp_nh.param("use_vision", use_vision, false); // listen to Vision source
106 
107  // tf params
108  fp_nh.param<std::string>("tf/frame_id", tf_frame_id, "map");
109  fp_nh.param<std::string>("tf/child_frame_id", tf_child_frame_id, "fix");
110  fp_nh.param("tf/rate_limit", tf_rate, 10.0);
111 
112  if (use_mocap) {
113  if (mocap_transform) { // MoCap data in TransformStamped msg
114  mocap_tf_sub = fp_nh.subscribe("mocap/tf", 10, &FakeGPSPlugin::mocap_tf_cb, this);
115  }
116  else { // MoCap data in PoseStamped msg
117  mocap_pose_sub = fp_nh.subscribe("mocap/pose", 10, &FakeGPSPlugin::mocap_pose_cb, this);
118  }
119  }
120  else if (use_vision) { // Vision data in PoseStamped msg
122  }
123  else if (tf_listen) { // Pose aquired from TF Listener
124  ROS_INFO_STREAM_NAMED("fake_gps", "Listen to transform " << tf_frame_id
125  << " -> " << tf_child_frame_id);
126  tf2_start("FakeGPSVisionTF", &FakeGPSPlugin::transform_cb);
127  }
128  else {
129  ROS_ERROR_NAMED("fake_gps", "No pose source!");
130  }
131  }
132 
134  {
135  return { /* Rx disabled */ };
136  }
137 
138 private:
139  friend class TF2ListenerMixin;
141 
144 
145  // Constructor for a ellipsoid
146  GeographicLib::Geocentric earth;
147 
151 
152  bool use_mocap;
153  bool use_vision;
155  bool tf_listen;
156 
157  double eph, epv;
159  GPS_FIX_TYPE fix_type;
160 
161  double tf_rate;
162  std::string tf_frame_id;
163  std::string tf_child_frame_id;
165 
166  Eigen::Vector3d map_origin;
167  Eigen::Vector3d ecef_origin;
168  Eigen::Vector3d old_ecef;
169  double old_stamp;
170 
171  /* -*- mid-level helpers and low-level send -*- */
172 
176  void send_fake_gps(const ros::Time &stamp, const Eigen::Vector3d &ecef_offset) {
177  // Throttle incoming messages to 5hz
178  if ((ros::Time::now() - last_pos_time) < ros::Duration(gps_rate)) {
179  return;
180  }
181  last_pos_time = ros::Time::now();
182 
188  mavlink::common::msg::HIL_GPS fix {};
189 
190  Eigen::Vector3d geodetic;
191  Eigen::Vector3d current_ecef(ecef_origin.x() + ecef_offset.x(),
192  ecef_origin.y() + ecef_offset.y(),
193  ecef_origin.z() + ecef_offset.z());
194 
195  try {
196  earth.Reverse(current_ecef.x(), current_ecef.y(), current_ecef.z(),
197  geodetic.x(), geodetic.y(), geodetic.z());
198  }
199  catch (const std::exception& e) {
200  ROS_INFO_STREAM("FGPS: Caught exception: " << e.what() << std::endl);
201  }
202 
203  Eigen::Vector3d vel = ((old_ecef - current_ecef) / (stamp.toSec() - old_stamp)) * 1e2;// [cm/s]
204 
205  // compute course over ground
206  double cog;
207  if (vel.x() == 0 && vel.y() == 0) {
208  cog = 0;
209  }
210  else if (vel.x() >= 0 && vel.y() < 0) {
211  cog = M_PI * 5 / 2 - atan2(vel.x(), vel.y());
212  }
213  else {
214  cog = M_PI / 2 - atan2(vel.x(), vel.y());
215  }
216 
217  // Fill in and send message
218  fix.time_usec = stamp.toNSec() / 1000; // [useconds]
219  fix.lat = geodetic.x() * 1e7; // [degrees * 1e7]
220  fix.lon = geodetic.y() * 1e7; // [degrees * 1e7]
221  fix.alt = (geodetic.z() + GeographicLib::Geoid::ELLIPSOIDTOGEOID *
222  (*m_uas->egm96_5)(geodetic.x(), geodetic.y())) * 1e3; // [meters * 1e3]
223  fix.vel = vel.block<2, 1>(0, 0).norm(); // [cm/s]
224  fix.vn = vel.x(); // [cm/s]
225  fix.ve = vel.y(); // [cm/s]
226  fix.vd = vel.z(); // [cm/s]
227  fix.cog = cog * 1e2; // [degrees * 1e2]
228  fix.eph = eph * 1e2; // [cm]
229  fix.epv = epv * 1e2; // [cm]
230  fix.fix_type = utils::enum_value(fix_type);;
231  fix.satellites_visible = satellites_visible;
232 
233  // store old values
234  old_stamp = stamp.toSec();
235  old_ecef = current_ecef;
236 
237  UAS_FCU(m_uas)->send_message_ignore_drop(fix);
238  }
239 
240  /* -*- callbacks -*- */
242  {
243  Eigen::Affine3d pos_enu;
244  tf::transformMsgToEigen(trans->transform, pos_enu);
245 
246  send_fake_gps(trans->header.stamp, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation()), map_origin));
247  }
248 
250  {
251  Eigen::Affine3d pos_enu;
252  tf::poseMsgToEigen(req->pose, pos_enu);
253 
254  send_fake_gps(req->header.stamp, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation()), map_origin));
255  }
256 
258  {
259  Eigen::Affine3d pos_enu;
260  tf::poseMsgToEigen(req->pose, pos_enu);
261 
262  send_fake_gps(req->header.stamp, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation()), map_origin));
263  }
264 
265  void transform_cb(const geometry_msgs::TransformStamped &trans)
266  {
267  Eigen::Affine3d pos_enu;
268 
269  tf::transformMsgToEigen(trans.transform, pos_enu);
270 
271  send_fake_gps(trans.header.stamp, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation()), map_origin));
272  }
273 };
274 } // namespace extra_plugins
275 } // namespace mavros
276 
void mocap_pose_cb(const geometry_msgs::PoseStamped::ConstPtr &req)
Definition: fake_gps.cpp:249
std::shared_ptr< MAVConnInterface const > ConstPtr
GeographicLib::Geocentric earth
Definition: fake_gps.cpp:146
void mocap_tf_cb(const geometry_msgs::TransformStamped::ConstPtr &trans)
Definition: fake_gps.cpp:241
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
void send_fake_gps(const ros::Time &stamp, const Eigen::Vector3d &ecef_offset)
Send fake GPS coordinates through HIL_GPS Mavlink msg.
Definition: fake_gps.cpp:176
void tf2_start(const char *_thd_name, void(FakeGPSPlugin::*cbp)(const geometry_msgs::TransformStamped &))
#define ROS_INFO_STREAM_NAMED(name, args)
bool mocap_transform
set use of mocap data (TransformStamped msg)
Definition: fake_gps.cpp:154
Eigen::Vector3d map_origin
geodetic origin [lla]
Definition: fake_gps.cpp:166
bool param(const std::string &param_name, T &param_val, const T &default_val) const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW FakeGPSPlugin()
Definition: fake_gps.cpp:45
std::shared_ptr< GeographicLib::Geoid > egm96_5
#define UAS_FCU(uasobjptr)
double old_stamp
previous stamp [s]
Definition: fake_gps.cpp:169
uint64_t toNSec() const
void transform_cb(const geometry_msgs::TransformStamped &trans)
Definition: fake_gps.cpp:265
T transform_frame_enu_ecef(const T &in, const T &map_origin)
Eigen::Vector3d old_ecef
previous geocentric position [m]
Definition: fake_gps.cpp:168
bool tf_listen
set use of TF Listener data
Definition: fake_gps.cpp:155
void transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Affine3d &e)
#define ROS_INFO_STREAM(args)
std::vector< HandlerInfo > Subscriptions
void initialize(UAS &uas_)
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
Eigen::Vector3d ecef_origin
geocentric origin [m]
Definition: fake_gps.cpp:167
#define ROS_ERROR_NAMED(name,...)
static Time now()
void vision_cb(const geometry_msgs::PoseStamped::ConstPtr &req)
Definition: fake_gps.cpp:257
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
bool use_vision
set use of vision data
Definition: fake_gps.cpp:153
constexpr std::underlying_type< _T >::type enum_value(_T e)
bool use_mocap
set use of mocap data (PoseStamped msg)
Definition: fake_gps.cpp:152


mavros_extras
Author(s): Vladimir Ermakov
autogenerated on Mon Jul 8 2019 03:20:18