extras: add covariance parsing to vision_speed_estimate (#996)

mavros/launch/apm_config.yaml

@@ -198,7 +198,8 @@ vision_pose:
 
 # vision_speed_estimate
 vision_speed:
-  listen_twist: false
+  listen_twist: true    # enable listen to twist topic, else listen to vec3d topic
+  twist_cov: true       # enable listen to twist with covariance topic
 
 # vibration
 vibration:

mavros/launch/px4_config.yaml

@@ -209,7 +209,8 @@ vision_pose:
 
 # vision_speed_estimate
 vision_speed:
-  listen_twist: false
+  listen_twist: true    # enable listen to twist topic, else listen to vec3d topic
+  twist_cov: true       # enable listen to twist with covariance topic
 
 # vibration
 vibration:

mavros_extras/src/plugins/vision_speed_estimate.cpp

@@ -8,46 +8,50 @@
  * @{
  */
 /*
- * Copyright 2014 Nuno Marques.
+ * Copyright 2014, 2018 Nuno Marques.
  *
  * This file is part of the mavros package and subject to the license terms
  * in the top-level LICENSE file of the mavros repository.
  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
  */
 
 #include <mavros/mavros_plugin.h>
-#include <eigen_conversions/eigen_msg.h>
 
 #include <geometry_msgs/TwistStamped.h>
+#include <geometry_msgs/TwistWithCovarianceStamped.h>
+
 #include <geometry_msgs/Vector3Stamped.h>
 
 namespace mavros {
-namespace extra_plugins{
+namespace extra_plugins {
 /**
  * @brief Vision speed estimate plugin
  *
  * Send velocity estimation from various vision estimators
  * to FCU position and attitude estimators.
- *
  */
 class VisionSpeedEstimatePlugin : public plugin::PluginBase {
 public:
 	VisionSpeedEstimatePlugin() : PluginBase(),
-		sp_nh("~vision_speed")
+		sp_nh("~vision_speed"),
+		listen_twist(true),
+		twist_cov(true)
 	{ }
 
 	void initialize(UAS &uas_)
 	{
 		PluginBase::initialize(uas_);
 
-		bool listen_twist;
-
-		sp_nh.param("listen_twist", listen_twist, false);
+		sp_nh.param("listen_twist", listen_twist, true);
+		sp_nh.param("twist_cov", twist_cov, true);
 
 		if (listen_twist)
-			vision_vel_sub = sp_nh.subscribe("speed_twist", 10, &VisionSpeedEstimatePlugin::vel_twist_cb, this);
+			if (twist_cov)
+				vision_twist_cov_sub = sp_nh.subscribe("speed_twist_cov", 10, &VisionSpeedEstimatePlugin::twist_cov_cb, this);
+			else
+				vision_twist_sub = sp_nh.subscribe("speed_twist", 10, &VisionSpeedEstimatePlugin::twist_cb, this);
 		else
-			vision_vel_sub = sp_nh.subscribe("speed_vector", 10, &VisionSpeedEstimatePlugin::vel_speed_cb, this);
+			vision_vector_sub = sp_nh.subscribe("speed_vector", 10, &VisionSpeedEstimatePlugin::vector_cb, this);
 	}
 
 	Subscriptions get_subscriptions()
@@ -58,13 +62,25 @@ class VisionSpeedEstimatePlugin : public plugin::PluginBase {
 private:
 	ros::NodeHandle sp_nh;
 
-	ros::Subscriber vision_vel_sub;
+	bool listen_twist;			//!< If True, listen to Twist data topics
+	bool twist_cov;				//!< If True, listen to TwistWithCovariance data topic
 
-	/* -*- low-level send -*- */
+	ros::Subscriber vision_twist_sub;	//!< Subscriber to geometry_msgs/TwistStamped msgs
+	ros::Subscriber vision_twist_cov_sub;	//!< Subscriber to geometry_msgs/TwistWithCovarianceStamped msgs
+	ros::Subscriber vision_vector_sub;	//!< Subscriber to geometry_msgs/Vector3Stamped msgs
 
-	void vision_speed_estimate(uint64_t usec, Eigen::Vector3d &v)
+	/* -*- low-level send -*- */
+	/**
+	 * @brief Send vision speed estimate on local NED frame to the FCU.
+	 *
+	 * Message specification: https://mavlink.io/en/messages/common.html#VISION_SPEED_ESTIMATE
+	 * @param usec	Timestamp (microseconds, synced to UNIX time or since system boot) (us)
+	 * @param v	Velocity/speed vector in the local NED frame (meters)
+	 * @param cov	Linear velocity covariance matrix (local NED frame)
+	 */
+	void send_vision_speed_estimate(const uint64_t usec, const Eigen::Vector3d &v, const ftf::Covariance3d &cov)
 	{
-		mavlink::common::msg::VISION_SPEED_ESTIMATE vs{};
+		mavlink::common::msg::VISION_SPEED_ESTIMATE vs {};
 
 		vs.usec = usec;
 
@@ -77,37 +93,65 @@ class VisionSpeedEstimatePlugin : public plugin::PluginBase {
 		vs.z = v.z();
 		// [[[end]]] (checksum: aee3cc9a73a2e736b7bc6c83ea93abdb)
 
+		ftf::covariance_to_mavlink(cov, vs.covariance);
+
 		UAS_FCU(m_uas)->send_message_ignore_drop(vs);
 	}
 
+	/* -*- mid-level helpers -*- */
 	/**
-	 * @todo Suggest modification on PX4 firmware to MAVLINK VISION_SPEED_ESTIMATE
-	 * msg name, which should be called instead VISION_VELOCITY_ESTIMATE
+	 * @brief Convert vector and covariance from local ENU to local NED frame
+	 *
+	 * @param stamp		ROS timestamp of the message
+	 * @param vel_enu	Velocity/speed vector in the ENU frame
+	 * @param cov_enu	Linear velocity/speed in the ENU frame
 	 */
+	void convert_vision_speed(const ros::Time &stamp, const Eigen::Vector3d &vel_enu, const ftf::Covariance3d &cov_enu)
+	{
+		// Send transformed data from local ENU to NED frame
+		send_vision_speed_estimate(stamp.toNSec() / 1000,
+					ftf::transform_frame_enu_ned(vel_enu),
+					ftf::transform_frame_enu_ned(cov_enu));
+	}
 
-	/* -*- mid-level helpers -*- */
-
+	/* -*- callbacks -*- */
 	/**
-	 * @brief Send vision speed estimate to FCU velocity controller
+	 * @brief Callback to geometry_msgs/TwistStamped msgs
+	 *
+	 * @param req	received geometry_msgs/TwistStamped msg
 	 */
-	void send_vision_speed(const geometry_msgs::Vector3 &vel_enu, const ros::Time &stamp)
-	{
-		Eigen::Vector3d vel_;
-		tf::vectorMsgToEigen(vel_enu, vel_);
-		//Transform from ENU to NED frame
-		auto vel = ftf::transform_frame_enu_ned(vel_);
+	void twist_cb(const geometry_msgs::TwistStamped::ConstPtr &req) {
+		ftf::Covariance3d cov {};	// zero initialized
 
-		vision_speed_estimate(stamp.toNSec() / 1000, vel);
+		convert_vision_speed(req->header.stamp, ftf::to_eigen(req->twist.linear), cov);
 	}
 
-	/* -*- callbacks -*- */
+	/**
+	 * @brief Callback to geometry_msgs/TwistWithCovarianceStamped msgs
+	 *
+	 * @param req	received geometry_msgs/TwistWithCovarianceStamped msg
+	 */
+	void twist_cov_cb(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &req) {
+		ftf::Covariance3d cov3d {};	// zero initialized
 
-	void vel_twist_cb(const geometry_msgs::TwistStamped::ConstPtr &req) {
-		send_vision_speed(req->twist.linear, req->header.stamp);
+		ftf::EigenMapCovariance3d cov3d_map(cov3d.data());
+		ftf::EigenMapConstCovariance6d cov6d_map(req->twist.covariance.data());
+
+		// only the linear velocity will be sent
+		cov3d_map = cov6d_map.block<3, 3>(0, 0);
+
+		convert_vision_speed(req->header.stamp, ftf::to_eigen(req->twist.twist.linear), cov3d);
 	}
 
-	void vel_speed_cb(const geometry_msgs::Vector3Stamped::ConstPtr &req) {
-		send_vision_speed(req->vector, req->header.stamp);
+	/**
+	 * @brief Callback to geometry_msgs/Vector3Stamped msgs
+	 *
+	 * @param req	received geometry_msgs/Vector3Stamped msg
+	 */
+	void vector_cb(const geometry_msgs::Vector3Stamped::ConstPtr &req) {
+		ftf::Covariance3d cov {};	// zero initialized
+
+		convert_vision_speed(req->header.stamp, ftf::to_eigen(req->vector), cov);
 	}
 };
 }	// namespace extra_plugins