px4flow.cpp
Go to the documentation of this file.
1 
10 /*
11  * Copyright 2014 M.H.Kabir.
12  * Copyright 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>
20 
21 #include <mavros_msgs/OpticalFlowRad.h>
22 #include <sensor_msgs/Temperature.h>
23 #include <sensor_msgs/Range.h>
24 
25 namespace mavros {
26 namespace extra_plugins{
33 public:
35  flow_nh("~px4flow"),
36  ranger_fov(0.0),
37  ranger_min_range(0.3),
38  ranger_max_range(5.0)
39  { }
40 
41  void initialize(UAS &uas_) override
42  {
44 
45  flow_nh.param<std::string>("frame_id", frame_id, "px4flow");
46 
53  flow_nh.param("ranger_fov", ranger_fov, 0.119428926);
54 
55  flow_nh.param("ranger_min_range", ranger_min_range, 0.3);
56  flow_nh.param("ranger_max_range", ranger_max_range, 5.0);
57 
58  flow_rad_pub = flow_nh.advertise<mavros_msgs::OpticalFlowRad>("raw/optical_flow_rad", 10);
59  range_pub = flow_nh.advertise<sensor_msgs::Range>("ground_distance", 10);
60  temp_pub = flow_nh.advertise<sensor_msgs::Temperature>("temperature", 10);
61 
62  flow_rad_sub = flow_nh.subscribe("raw/send", 1, &PX4FlowPlugin::send_cb, this);
63  }
64 
66  {
67  return {
69  };
70  }
71 
72 private:
74 
75  std::string frame_id;
76 
77  double ranger_fov;
80 
85 
86  void handle_optical_flow_rad(const mavlink::mavlink_message_t *msg, mavlink::common::msg::OPTICAL_FLOW_RAD &flow_rad)
87  {
88  auto header = m_uas->synchronized_header(frame_id, flow_rad.time_usec);
89 
97  Eigen::Vector3d(
98  flow_rad.integrated_x,
99  flow_rad.integrated_y,
100  0.0));
102  Eigen::Vector3d(
103  flow_rad.integrated_xgyro,
104  flow_rad.integrated_ygyro,
105  flow_rad.integrated_zgyro));
106 
107  auto flow_rad_msg = boost::make_shared<mavros_msgs::OpticalFlowRad>();
108 
109  flow_rad_msg->header = header;
110  flow_rad_msg->integration_time_us = flow_rad.integration_time_us;
111 
112  flow_rad_msg->integrated_x = int_xy.x();
113  flow_rad_msg->integrated_y = int_xy.y();
114 
115  flow_rad_msg->integrated_xgyro = int_gyro.x();
116  flow_rad_msg->integrated_ygyro = int_gyro.y();
117  flow_rad_msg->integrated_zgyro = int_gyro.z();
118 
119  flow_rad_msg->temperature = flow_rad.temperature / 100.0f; // in degrees celsius
120  flow_rad_msg->time_delta_distance_us = flow_rad.time_delta_distance_us;
121  flow_rad_msg->distance = flow_rad.distance;
122  flow_rad_msg->quality = flow_rad.quality;
123 
124  flow_rad_pub.publish(flow_rad_msg);
125 
126  // Temperature
127  auto temp_msg = boost::make_shared<sensor_msgs::Temperature>();
128 
129  temp_msg->header = header;
130  temp_msg->temperature = flow_rad_msg->temperature;
131 
132  temp_pub.publish(temp_msg);
133 
134  // Rangefinder
143  auto range_msg = boost::make_shared<sensor_msgs::Range>();
144 
145  range_msg->header = header;
146 
147  range_msg->radiation_type = sensor_msgs::Range::ULTRASOUND;
148  range_msg->field_of_view = ranger_fov;
149  range_msg->min_range = ranger_min_range;
150  range_msg->max_range = ranger_max_range;
151  range_msg->range = flow_rad.distance;
152 
153  range_pub.publish(range_msg);
154  }
155 
157  {
158  mavlink::common::msg::OPTICAL_FLOW_RAD flow_rad_msg = {};
159 
161  Eigen::Vector3d(
162  msg->integrated_x,
163  msg->integrated_y,
164  0.0));
166  Eigen::Vector3d(
167  msg->integrated_xgyro,
168  msg->integrated_ygyro,
169  msg->integrated_zgyro));
170 
171  flow_rad_msg.time_usec = msg->header.stamp.toNSec() / 1000;
172  flow_rad_msg.sensor_id = 0;
173  flow_rad_msg.integration_time_us = msg->integration_time_us;
174  flow_rad_msg.integrated_x = int_xy.x();
175  flow_rad_msg.integrated_y = int_xy.y();
176  flow_rad_msg.integrated_xgyro = int_gyro.x();
177  flow_rad_msg.integrated_ygyro = int_gyro.y();
178  flow_rad_msg.integrated_zgyro = int_gyro.z();
179  flow_rad_msg.temperature = msg->temperature * 100.0f; // temperature in centi-degrees Celsius
180  flow_rad_msg.quality = msg->quality;
181  flow_rad_msg.time_delta_distance_us = msg->time_delta_distance_us;
182  flow_rad_msg.distance = msg->distance;
183 
184  UAS_FCU(m_uas)->send_message_ignore_drop(flow_rad_msg);
185  }
186 };
187 } // namespace extra_plugins
188 } // namespace mavros
189 
std::shared_ptr< MAVConnInterface const > ConstPtr
std_msgs::Header synchronized_header(const std::string &frame_id, const T time_stamp)
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
HandlerInfo make_handler(const mavlink::msgid_t id, void(_C::*fn)(const mavlink::mavlink_message_t *msg, const mavconn::Framing framing))
Subscriptions get_subscriptions() override
Definition: px4flow.cpp:65
T transform_frame_aircraft_baselink(const T &in)
void handle_optical_flow_rad(const mavlink::mavlink_message_t *msg, mavlink::common::msg::OPTICAL_FLOW_RAD &flow_rad)
Definition: px4flow.cpp:86
bool param(const std::string &param_name, T &param_val, const T &default_val) const
#define UAS_FCU(uasobjptr)
T transform_frame_baselink_aircraft(const T &in)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
std::vector< HandlerInfo > Subscriptions
void send_cb(const mavros_msgs::OpticalFlowRad::ConstPtr msg)
Definition: px4flow.cpp:156
PX4 Optical Flow plugin.
Definition: px4flow.cpp:32
void initialize(UAS &uas_) override
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
PayloadHeader * header()
void initialize(UAS &uas_) override
Definition: px4flow.cpp:41


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