wind_estimation.cpp
Go to the documentation of this file.
1 
9 /*
10  * Copyright 2018 Thomas Stastny <thomas.stastny@mavt.ethz.ch>
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/mavros_plugin.h>
18 
19 #include <angles/angles.h>
21 
22 #include <geometry_msgs/TwistWithCovarianceStamped.h>
23 
24 namespace mavros {
25 namespace std_plugins {
30 public:
32  nh("~")
33  { }
34 
38  void initialize(UAS &uas_) override
39  {
40  PluginBase::initialize(uas_);
41 
42  wind_pub = nh.advertise<geometry_msgs::TwistWithCovarianceStamped>("wind_estimation", 10);
43  }
44 
46  {
47  return {
50  };
51  }
52 
53 private:
55 
57 
61  void handle_apm_wind(const mavlink::mavlink_message_t *msg, mavlink::ardupilotmega::msg::WIND &wind)
62  {
63  const double speed = wind.speed;
64  const double course = angles::from_degrees(wind.direction) + M_PI; // direction "from" -> direction "to"
65 
66  auto twist_cov = boost::make_shared<geometry_msgs::TwistWithCovarianceStamped>();
67  twist_cov->header.stamp = ros::Time::now();
68  twist_cov->twist.twist.linear.x = speed * std::sin(course); // E
69  twist_cov->twist.twist.linear.y = speed * std::cos(course); // N
70  twist_cov->twist.twist.linear.z = -wind.speed_z;// D -> U
71 
72  // covariance matrix unknown in APM msg
73  ftf::EigenMapCovariance6d cov_map(twist_cov->twist.covariance.data());
74  cov_map.setZero();
75  cov_map(0, 0) = -1.0;
76 
77  wind_pub.publish(twist_cov);
78  }
79 
83  void handle_px4_wind(const mavlink::mavlink_message_t *msg, mavlink::common::msg::WIND_COV &wind)
84  {
85  auto twist_cov = boost::make_shared<geometry_msgs::TwistWithCovarianceStamped>();
86  twist_cov->header.stamp = m_uas->synchronise_stamp(wind.time_usec);
87 
88  tf::vectorEigenToMsg(ftf::transform_frame_ned_enu(Eigen::Vector3d(wind.wind_x, wind.wind_y, wind.wind_z)),
89  twist_cov->twist.twist.linear);
90 
91  // fill available covariance elements
92  ftf::EigenMapCovariance6d cov_map(twist_cov->twist.covariance.data());
93  cov_map.setZero();
94  cov_map(0, 0) = wind.var_horiz; // NOTE: this is a summed covariance for both x and y horizontal wind components
95  cov_map(2, 2) = wind.var_vert;
96 
97  wind_pub.publish(twist_cov);
98  }
99 };
100 } // namespace std_plugins
101 } // namespace mavros
102 
MAVROS Plugin base class.
Definition: mavros_plugin.h:36
void publish(const boost::shared_ptr< M > &message) const
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
Eigen::Map< Eigen::Matrix< double, 6, 6, Eigen::RowMajor > > EigenMapCovariance6d
Eigen::Map for Covariance6d.
Definition: frame_tf.h:50
PluginBase()
Plugin constructor Should not do anything before initialize()
Definition: mavros_plugin.h:74
UAS for plugins.
Definition: mavros_uas.h:67
static double from_degrees(double degrees)
void vectorEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Vector3 &m)
ros::Time synchronise_stamp(uint32_t time_boot_ms)
Compute FCU message time from time_boot_ms or time_usec field.
T transform_frame_ned_enu(const T &in)
Transform data expressed in NED to ENU frame.
Definition: frame_tf.h:215
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
std::vector< HandlerInfo > Subscriptions
Subscriptions vector.
Definition: mavros_plugin.h:47
static Time now()
Subscriptions get_subscriptions() override
Return vector of MAVLink message subscriptions (handlers)
void handle_px4_wind(const mavlink::mavlink_message_t *msg, mavlink::common::msg::WIND_COV &wind)
void handle_apm_wind(const mavlink::mavlink_message_t *msg, mavlink::ardupilotmega::msg::WIND &wind)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)


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