uas_data.cpp
Go to the documentation of this file.
1 
6 /*
7  * Copyright 2014,2015 Vladimir Ermakov.
8  *
9  * This file is part of the mavros package and subject to the license terms
10  * in the top-level LICENSE file of the mavros repository.
11  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
12  */
13 
14 #include <array>
15 #include <unordered_map>
16 #include <stdexcept>
17 #include <mavros/mavros_uas.h>
18 #include <mavros/utils.h>
19 #include <mavros/px4_custom_mode.h>
20 
21 using namespace mavros;
22 using utils::enum_value;
23 
25  tf2_listener(tf2_buffer, true),
26  type(enum_value(MAV_TYPE::GENERIC)),
27  autopilot(enum_value(MAV_AUTOPILOT::GENERIC)),
28  base_mode(0),
29  target_system(1),
30  target_component(1),
31  connected(false),
32  gps_eph(NAN),
33  gps_epv(NAN),
34  gps_fix_type(0),
35  gps_satellites_visible(0),
36  time_offset(0),
37  tsync_mode(UAS::timesync_mode::NONE),
38  fcu_caps_known(false),
39  fcu_capabilities(0)
40 {
41  try {
42  // Using smallest dataset with 5' grid,
43  // From default location,
44  // Use cubic interpolation, Thread safe
45  egm96_5 = std::make_shared<GeographicLib::Geoid>("egm96-5", "", true, true);
46  }
47  catch (const std::exception &e) {
48  // catch exception and shutdown node
49  ROS_FATAL_STREAM("UAS: GeographicLib exception: " << e.what() <<
50  " | Run install_geographiclib_dataset.sh script in order to install Geoid Model dataset!");
51  ros::shutdown();
52  }
53 
54  // Publish helper TFs used for frame transformation in the odometry plugin
55  std::vector<geometry_msgs::TransformStamped> transform_vector;
56  add_static_transform("map", "map_ned", Eigen::Affine3d(ftf::quaternion_from_rpy(M_PI, 0, M_PI_2)),transform_vector);
57  add_static_transform("odom", "odom_ned", Eigen::Affine3d(ftf::quaternion_from_rpy(M_PI, 0, M_PI_2)),transform_vector);
58  add_static_transform("base_link", "base_link_frd", Eigen::Affine3d(ftf::quaternion_from_rpy(M_PI, 0, 0)),transform_vector);
59 
60  tf2_static_broadcaster.sendTransform(transform_vector);
61 }
62 
63 /* -*- heartbeat handlers -*- */
64 
65 void UAS::update_heartbeat(uint8_t type_, uint8_t autopilot_, uint8_t base_mode_)
66 {
67  type = type_;
68  autopilot = autopilot_;
69  base_mode = base_mode_;
70 }
71 
73 {
74  if (conn_ != connected) {
75  connected = conn_;
76 
77  // call all change cb's
78  for (auto &cb : connection_cb_vec)
79  cb(conn_);
80  }
81 }
82 
84 {
85  lock_guard lock(mutex);
86  connection_cb_vec.push_back(cb);
87 }
88 
89 /* -*- autopilot version -*- */
90 
91 static uint64_t get_default_caps(UAS::MAV_AUTOPILOT ap_type)
92 {
93  // TODO: return default caps mask for known FCU's
94  return 0;
95 }
96 
98 {
99  if (fcu_caps_known) {
100  uint64_t caps = fcu_capabilities;
101  return caps;
102  }
103  else {
105  }
106 }
107 
108 // This function may need a mutex now
109 void UAS::update_capabilities(bool known, uint64_t caps)
110 {
111  bool process_cb_queue = false;
112 
113  if (known != fcu_caps_known) {
114  if (!fcu_caps_known) {
115  process_cb_queue = true;
116  }
117  fcu_caps_known = known;
118  } else if (fcu_caps_known) { // Implies fcu_caps_known == known
119  if (caps != fcu_capabilities) {
120  process_cb_queue = true;
121  }
122  }
123  else {} // Capabilities werent known before and arent known after update
124 
125  if (process_cb_queue) {
126  fcu_capabilities = caps;
127  for (auto &cb : capabilities_cb_vec) {
128  cb(static_cast<MAV_CAP>(caps));
129  }
130  }
131 }
132 
134 {
135  lock_guard lock(mutex);
136  capabilities_cb_vec.push_back(cb);
137 }
138 
139 /* -*- IMU data -*- */
140 
142 {
143  lock_guard lock(mutex);
144  imu_enu_data = imu;
145 }
146 
148 {
149  lock_guard lock(mutex);
150  imu_ned_data = imu;
151 }
152 
154 {
155  lock_guard lock(mutex);
156  return imu_enu_data;
157 }
158 
160 {
161  lock_guard lock(mutex);
162  return imu_ned_data;
163 }
164 
165 geometry_msgs::Quaternion UAS::get_attitude_orientation_enu()
166 {
167  lock_guard lock(mutex);
168  if (imu_enu_data)
169  return imu_enu_data->orientation;
170  else {
171  // fallback - return identity
172  geometry_msgs::Quaternion q;
173  q.w = 1.0; q.x = q.y = q.z = 0.0;
174  return q;
175  }
176 }
177 
178 geometry_msgs::Quaternion UAS::get_attitude_orientation_ned()
179 {
180  lock_guard lock(mutex);
181  if (imu_ned_data)
182  return imu_ned_data->orientation;
183  else {
184  // fallback - return identity
185  geometry_msgs::Quaternion q;
186  q.w = 1.0; q.x = q.y = q.z = 0.0;
187  return q;
188  }
189 }
190 
192 {
193  lock_guard lock(mutex);
194  if (imu_enu_data)
195  return imu_enu_data->angular_velocity;
196  else {
197  // fallback
198  geometry_msgs::Vector3 v;
199  v.x = v.y = v.z = 0.0;
200  return v;
201  }
202 }
203 
205 {
206  lock_guard lock(mutex);
207  if (imu_ned_data)
208  return imu_ned_data->angular_velocity;
209  else {
210  // fallback
211  geometry_msgs::Vector3 v;
212  v.x = v.y = v.z = 0.0;
213  return v;
214  }
215 }
216 
217 
218 /* -*- GPS data -*- */
219 
221  float eph, float epv,
222  int fix_type, int satellites_visible)
223 {
224  lock_guard lock(mutex);
225 
226  gps_fix = fix;
227  gps_eph = eph;
228  gps_epv = epv;
229  gps_fix_type = fix_type;
230  gps_satellites_visible = satellites_visible;
231 }
232 
234 void UAS::get_gps_epts(float &eph, float &epv, int &fix_type, int &satellites_visible)
235 {
236  lock_guard lock(mutex);
237 
238  eph = gps_eph;
239  epv = gps_epv;
240  fix_type = gps_fix_type;
241  satellites_visible = gps_satellites_visible;
242 }
243 
246 {
247  lock_guard lock(mutex);
248  return gps_fix;
249 }
250 
251 /* -*- transform -*- */
252 
254 void UAS::add_static_transform(const std::string &frame_id, const std::string &child_id, const Eigen::Affine3d &tr, std::vector<geometry_msgs::TransformStamped>& vector)
255 {
256  geometry_msgs::TransformStamped static_transform;
257 
258  static_transform.header.stamp = ros::Time::now();
259  static_transform.header.frame_id = frame_id;
260  static_transform.child_frame_id = child_id;
261  tf::transformEigenToMsg(tr, static_transform.transform);
262 
263  vector.emplace_back(static_transform);
264 }
265 
267 void UAS::publish_static_transform(const std::string &frame_id, const std::string &child_id, const Eigen::Affine3d &tr)
268 {
269  geometry_msgs::TransformStamped static_transformStamped;
270 
271  static_transformStamped.header.stamp = ros::Time::now();
272  static_transformStamped.header.frame_id = frame_id;
273  static_transformStamped.child_frame_id = child_id;
274  tf::transformEigenToMsg(tr, static_transformStamped.transform);
275 
276  tf2_static_broadcaster.sendTransform(static_transformStamped);
277 }
std::atomic< uint8_t > autopilot
Definition: mavros_uas.h:451
mavlink::minimal::MAV_TYPE MAV_TYPE
Definition: mavros_uas.h:70
sensor_msgs::NavSatFix::Ptr gps_fix
Definition: mavros_uas.h:464
void add_capabilities_change_handler(CapabilitiesCb cb)
Adds a function to the capabilities callback queue.
Definition: uas_data.cpp:133
void publish_static_transform(const std::string &frame_id, const std::string &child_id, const Eigen::Affine3d &tr)
Publishes static transform.
Definition: uas_data.cpp:267
geometry_msgs::Quaternion get_attitude_orientation_enu()
Get Attitude orientation quaternion.
Definition: uas_data.cpp:165
MAV_AUTOPILOT get_autopilot()
Returns autopilot type.
Definition: mavros_uas.h:131
void update_connection_status(bool conn_)
Definition: uas_data.cpp:72
float gps_epv
Definition: mavros_uas.h:466
void transformEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Transform &m)
sensor_msgs::Imu::Ptr imu_enu_data
Definition: mavros_uas.h:461
tf2_ros::StaticTransformBroadcaster tf2_static_broadcaster
Definition: mavros_uas.h:276
std::function< void(bool)> ConnectionCb
Definition: mavros_uas.h:78
geometry_msgs::Quaternion get_attitude_orientation_ned()
Get Attitude orientation quaternion.
Definition: uas_data.cpp:178
void update_capabilities(bool known, uint64_t caps=0)
Update the capabilities if they&#39;ve changed every VERSION/timeout.
Definition: uas_data.cpp:109
void update_attitude_imu_enu(sensor_msgs::Imu::Ptr &imu)
Store IMU data [ENU].
Definition: uas_data.cpp:141
UAS for plugins.
Definition: mavros_uas.h:67
std::atomic< uint8_t > type
Definition: mavros_uas.h:450
#define ROS_FATAL_STREAM(args)
std::shared_ptr< GeographicLib::Geoid > egm96_5
Geoid dataset used to convert between AMSL and WGS-84.
Definition: mavros_uas.h:243
void add_connection_change_handler(ConnectionCb cb)
Add connection change handler callback.
Definition: uas_data.cpp:83
geometry_msgs::Vector3 get_attitude_angular_velocity_ned()
Get angular velocity from IMU data.
Definition: uas_data.cpp:204
std::lock_guard< std::recursive_mutex > lock_guard
Definition: mavros_uas.h:80
std::atomic< bool > connected
Definition: mavros_uas.h:457
static uint64_t get_default_caps(UAS::MAV_AUTOPILOT ap_type)
Definition: uas_data.cpp:91
std::atomic< bool > fcu_caps_known
Definition: mavros_uas.h:473
std::vector< CapabilitiesCb > capabilities_cb_vec
Definition: mavros_uas.h:459
MAVROS Plugin context.
std::function< void(MAV_CAP)> CapabilitiesCb
Definition: mavros_uas.h:79
sensor_msgs::Imu::Ptr get_attitude_imu_ned()
Get IMU data [NED].
Definition: uas_data.cpp:159
std::recursive_mutex mutex
Definition: mavros_uas.h:448
void update_heartbeat(uint8_t type_, uint8_t autopilot_, uint8_t base_mode_)
Definition: uas_data.cpp:65
Eigen::Quaterniond quaternion_from_rpy(const Eigen::Vector3d &rpy)
Convert euler angles to quaternion.
int gps_fix_type
Definition: mavros_uas.h:467
sensor_msgs::NavSatFix::Ptr get_gps_fix()
Retunrs last GPS RAW message.
Definition: uas_data.cpp:245
mavlink::minimal::MAV_AUTOPILOT MAV_AUTOPILOT
Definition: mavros_uas.h:71
float gps_eph
Definition: mavros_uas.h:465
uint64_t get_capabilities()
Definition: uas_data.cpp:97
PX4 custom mode constantsPX4 custom flight modes.
std::atomic< uint8_t > base_mode
Definition: mavros_uas.h:452
static Time now()
ROSCPP_DECL void shutdown()
sensor_msgs::Imu::Ptr imu_ned_data
Definition: mavros_uas.h:462
void update_attitude_imu_ned(sensor_msgs::Imu::Ptr &imu)
Store IMU data [NED].
Definition: uas_data.cpp:147
void sendTransform(const geometry_msgs::TransformStamped &transform)
sensor_msgs::Imu::Ptr get_attitude_imu_enu()
Get IMU data [ENU].
Definition: uas_data.cpp:153
void update_gps_fix_epts(sensor_msgs::NavSatFix::Ptr &fix, float eph, float epv, int fix_type, int satellites_visible)
Store GPS RAW data.
Definition: uas_data.cpp:220
std::atomic< uint64_t > fcu_capabilities
Definition: mavros_uas.h:474
geometry_msgs::Vector3 get_attitude_angular_velocity_enu()
Get angular velocity from IMU data.
Definition: uas_data.cpp:191
void add_static_transform(const std::string &frame_id, const std::string &child_id, const Eigen::Affine3d &tr, std::vector< geometry_msgs::TransformStamped > &vector)
Add static transform. To publish all static transforms at once, we stack them in a std::vector...
Definition: uas_data.cpp:254
int gps_satellites_visible
Definition: mavros_uas.h:468
std::vector< ConnectionCb > connection_cb_vec
Definition: mavros_uas.h:458
constexpr std::underlying_type< _T >::type enum_value(_T e)
Definition: utils.h:55
void get_gps_epts(float &eph, float &epv, int &fix_type, int &satellites_visible)
Returns EPH, EPV, Fix type and satellites visible.
Definition: uas_data.cpp:234
std::shared_ptr< MAVConnInterface > Ptr


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