Go to the documentation of this file.
20 #include <geometry_msgs/TwistStamped.h>
21 #include <geometry_msgs/TwistWithCovarianceStamped.h>
23 #include <geometry_msgs/Vector3Stamped.h>
26 namespace extra_plugins {
36 sp_nh(
"~vision_speed"),
84 mavlink::common::msg::VISION_SPEED_ESTIMATE vs {};
142 cov3d_map = cov6d_map.block<3, 3>(0, 0);
T transform_frame_enu_ned(const T &in)
sensor_msgs::Imu::_angular_velocity_covariance_type Covariance3d
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.
std::vector< HandlerInfo > Subscriptions
void vector_cb(const geometry_msgs::Vector3Stamped::ConstPtr &req)
Callback to geometry_msgs/Vector3Stamped msgs.
#define UAS_FCU(uasobjptr)
std::shared_ptr< MAVConnInterface const > ConstPtr
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.
ros::Subscriber vision_twist_cov_sub
Subscriber to geometry_msgs/TwistWithCovarianceStamped msgs.
Eigen::Vector3d to_eigen(const geometry_msgs::Point r)
ros::Subscriber vision_vector_sub
Subscriber to geometry_msgs/Vector3Stamped msgs.
ros::Subscriber vision_twist_sub
Subscriber to geometry_msgs/TwistStamped msgs.
bool listen_twist
If True, listen to Twist data topics.
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
VisionSpeedEstimatePlugin()
void covariance_to_mavlink(const T &cov, std::array< float, SIZE > &covmsg)
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())
Subscriptions get_subscriptions() override
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.
virtual void initialize(UAS &uas)
T param(const std::string ¶m_name, const T &default_val) const
void initialize(UAS &uas_) override
Eigen::Map< Eigen::Matrix< double, 3, 3, Eigen::RowMajor > > EigenMapCovariance3d
void twist_cb(const geometry_msgs::TwistStamped::ConstPtr &req)
Callback to geometry_msgs/TwistStamped msgs.
bool twist_cov
If True, listen to TwistWithCovariance data topic.
mavros_extras
Author(s): Vladimir Ermakov
, Amilcar Lucas
autogenerated on Tue May 6 2025 02:34:08