vision_speed_estimate.cpp
Go to the documentation of this file.
1 
10 /*
11  * Copyright 2014, 2018 Nuno Marques.
12  *
13  * This file is part of the mavros package and subject to the license terms
14  * in the top-level LICENSE file of the mavros repository.
15  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
16  */
17 
18 #include <mavros/mavros_plugin.h>
19 
20 #include <geometry_msgs/TwistStamped.h>
21 #include <geometry_msgs/TwistWithCovarianceStamped.h>
22 
23 #include <geometry_msgs/Vector3Stamped.h>
24 
25 namespace mavros {
26 namespace extra_plugins {
34 public:
36  sp_nh("~vision_speed"),
37  listen_twist(true),
38  twist_cov(true)
39  { }
40 
41  void initialize(UAS &uas_) override
42  {
44 
45  sp_nh.param("listen_twist", listen_twist, true);
46  sp_nh.param("twist_cov", twist_cov, true);
47 
48  if (listen_twist) {
49  if (twist_cov)
51  else
53  }
54  else
56  }
57 
59  {
60  return { /* Rx disabled */ };
61  }
62 
63 private:
65 
66  bool listen_twist;
67  bool twist_cov;
68 
72 
73  /* -*- low-level send -*- */
82  void send_vision_speed_estimate(const uint64_t usec, const Eigen::Vector3d &v, const ftf::Covariance3d &cov)
83  {
84  mavlink::common::msg::VISION_SPEED_ESTIMATE vs {};
85 
86  vs.usec = usec;
87 
88  // [[[cog:
89  // for f in "xyz":
90  // cog.outl("vs.%s = v.%s();" % (f, f))
91  // ]]]
92  vs.x = v.x();
93  vs.y = v.y();
94  vs.z = v.z();
95  // [[[end]]] (checksum: aee3cc9a73a2e736b7bc6c83ea93abdb)
96 
97  ftf::covariance_to_mavlink(cov, vs.covariance);
98 
99  UAS_FCU(m_uas)->send_message_ignore_drop(vs);
100  }
101 
102  /* -*- mid-level helpers -*- */
110  void convert_vision_speed(const ros::Time &stamp, const Eigen::Vector3d &vel_enu, const ftf::Covariance3d &cov_enu)
111  {
112  // Send transformed data from local ENU to NED frame
113  send_vision_speed_estimate(stamp.toNSec() / 1000,
116  }
117 
118  /* -*- callbacks -*- */
125  ftf::Covariance3d cov {}; // zero initialized
126 
127  convert_vision_speed(req->header.stamp, ftf::to_eigen(req->twist.linear), cov);
128  }
129 
136  ftf::Covariance3d cov3d {}; // zero initialized
137 
138  ftf::EigenMapCovariance3d cov3d_map(cov3d.data());
139  ftf::EigenMapConstCovariance6d cov6d_map(req->twist.covariance.data());
140 
141  // only the linear velocity will be sent
142  cov3d_map = cov6d_map.block<3, 3>(0, 0);
143 
144  convert_vision_speed(req->header.stamp, ftf::to_eigen(req->twist.twist.linear), cov3d);
145  }
146 
153  ftf::Covariance3d cov {}; // zero initialized
154 
155  convert_vision_speed(req->header.stamp, ftf::to_eigen(req->vector), cov);
156  }
157 };
158 } // namespace extra_plugins
159 } // namespace mavros
160 
std::shared_ptr< MAVConnInterface const > ConstPtr
void convert_vision_speed(const ros::Time &stamp, const Eigen::Vector3d &vel_enu, const ftf::Covariance3d &cov_enu)
Convert vector and covariance from local ENU to local NED frame.
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ros::Subscriber vision_twist_cov_sub
Subscriber to geometry_msgs/TwistWithCovarianceStamped msgs.
bool twist_cov
If True, listen to TwistWithCovariance data topic.
ros::Subscriber vision_vector_sub
Subscriber to geometry_msgs/Vector3Stamped msgs.
bool param(const std::string &param_name, T &param_val, const T &default_val) const
#define UAS_FCU(uasobjptr)
void send_vision_speed_estimate(const uint64_t usec, const Eigen::Vector3d &v, const ftf::Covariance3d &cov)
Send vision speed estimate on local NED frame to the FCU.
uint64_t toNSec() const
Eigen::Vector3d to_eigen(const geometry_msgs::Point r)
ros::Subscriber vision_twist_sub
Subscriber to geometry_msgs/TwistStamped msgs.
void twist_cb(const geometry_msgs::TwistStamped::ConstPtr &req)
Callback to geometry_msgs/TwistStamped msgs.
Eigen::Map< Eigen::Matrix< double, 3, 3, Eigen::RowMajor > > EigenMapCovariance3d
T transform_frame_enu_ned(const T &in)
sensor_msgs::Imu::_angular_velocity_covariance_type Covariance3d
std::vector< HandlerInfo > Subscriptions
void vector_cb(const geometry_msgs::Vector3Stamped::ConstPtr &req)
Callback to geometry_msgs/Vector3Stamped msgs.
bool listen_twist
If True, listen to Twist data topics.
void covariance_to_mavlink(const T &cov, std::array< float, SIZE > &covmsg)
void initialize(UAS &uas_) override
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
Eigen::Map< const Eigen::Matrix< double, 6, 6, Eigen::RowMajor > > EigenMapConstCovariance6d
void twist_cov_cb(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &req)
Callback to geometry_msgs/TwistWithCovarianceStamped msgs.


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