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 
mavros::ftf::transform_frame_enu_ned
T transform_frame_enu_ned(const T &in)
mavros::plugin::PluginBase::m_uas
UAS * m_uas
mavros::ftf::Covariance3d
sensor_msgs::Imu::_angular_velocity_covariance_type Covariance3d
mavros::extra_plugins::VisionSpeedEstimatePlugin::send_vision_speed_estimate
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.
Definition: vision_speed_estimate.cpp:82
mavros::plugin::PluginBase::Subscriptions
std::vector< HandlerInfo > Subscriptions
mavros::extra_plugins::VisionSpeedEstimatePlugin::vector_cb
void vector_cb(const geometry_msgs::Vector3Stamped::ConstPtr &req)
Callback to geometry_msgs/Vector3Stamped msgs.
Definition: vision_speed_estimate.cpp:152
mavros::plugin::PluginBase::PluginBase
PluginBase()
UAS_FCU
#define UAS_FCU(uasobjptr)
mavros::UAS
ConstPtr
std::shared_ptr< MAVConnInterface const > ConstPtr
mavros::ftf::EigenMapConstCovariance6d
Eigen::Map< const Eigen::Matrix< double, 6, 6, Eigen::RowMajor > > EigenMapConstCovariance6d
mavros::extra_plugins::VisionSpeedEstimatePlugin::twist_cov_cb
void twist_cov_cb(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &req)
Callback to geometry_msgs/TwistWithCovarianceStamped msgs.
Definition: vision_speed_estimate.cpp:135
mavros::extra_plugins::VisionSpeedEstimatePlugin::vision_twist_cov_sub
ros::Subscriber vision_twist_cov_sub
Subscriber to geometry_msgs/TwistWithCovarianceStamped msgs.
Definition: vision_speed_estimate.cpp:70
mavros::ftf::to_eigen
Eigen::Vector3d to_eigen(const geometry_msgs::Point r)
mavros::extra_plugins::VisionSpeedEstimatePlugin::vision_vector_sub
ros::Subscriber vision_vector_sub
Subscriber to geometry_msgs/Vector3Stamped msgs.
Definition: vision_speed_estimate.cpp:71
mavros::extra_plugins::VisionSpeedEstimatePlugin::vision_twist_sub
ros::Subscriber vision_twist_sub
Subscriber to geometry_msgs/TwistStamped msgs.
Definition: vision_speed_estimate.cpp:69
mavros::extra_plugins::VisionSpeedEstimatePlugin::listen_twist
bool listen_twist
If True, listen to Twist data topics.
Definition: vision_speed_estimate.cpp:66
PLUGINLIB_EXPORT_CLASS
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
mavros_plugin.h
mavros::extra_plugins::VisionSpeedEstimatePlugin::VisionSpeedEstimatePlugin
VisionSpeedEstimatePlugin()
Definition: vision_speed_estimate.cpp:35
mavros::extra_plugins::VisionSpeedEstimatePlugin::sp_nh
ros::NodeHandle sp_nh
Definition: vision_speed_estimate.cpp:64
mavros::ftf::covariance_to_mavlink
void covariance_to_mavlink(const T &cov, std::array< float, SIZE > &covmsg)
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
mavros::extra_plugins::VisionSpeedEstimatePlugin::get_subscriptions
Subscriptions get_subscriptions() override
Definition: vision_speed_estimate.cpp:58
mavros
mavros::extra_plugins::VisionSpeedEstimatePlugin
Vision speed estimate plugin.
Definition: vision_speed_estimate.cpp:33
ros::Time
mavros::extra_plugins::VisionSpeedEstimatePlugin::convert_vision_speed
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.
Definition: vision_speed_estimate.cpp:110
initialize
virtual void initialize(UAS &uas)
mavros::plugin::PluginBase
class_list_macros.hpp
TimeBase< Time, Duration >::toNSec
uint64_t toNSec() const
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
mavros::extra_plugins::VisionSpeedEstimatePlugin::initialize
void initialize(UAS &uas_) override
Definition: vision_speed_estimate.cpp:41
mavros::ftf::EigenMapCovariance3d
Eigen::Map< Eigen::Matrix< double, 3, 3, Eigen::RowMajor > > EigenMapCovariance3d
mavros::extra_plugins::VisionSpeedEstimatePlugin::twist_cb
void twist_cb(const geometry_msgs::TwistStamped::ConstPtr &req)
Callback to geometry_msgs/TwistStamped msgs.
Definition: vision_speed_estimate.cpp:124
mavros::extra_plugins::VisionSpeedEstimatePlugin::twist_cov
bool twist_cov
If True, listen to TwistWithCovariance data topic.
Definition: vision_speed_estimate.cpp:67
ros::NodeHandle
ros::Subscriber


mavros_extras
Author(s): Vladimir Ermakov , Amilcar Lucas
autogenerated on Tue May 6 2025 02:34:08