extras fix #392: add additional subscription for PoseWithCovarianceSt…

…amped
mavlink · Sep 27, 2015 · 52d4a73 · 52d4a73
1 parent 7ded0f7
commit 52d4a73
Showing 1 changed file with 9 additions and 0 deletions.
diff --git a/mavros_extras/src/plugins/vision_pose_estimate.cpp b/mavros_extras/src/plugins/vision_pose_estimate.cpp
@@ -59,6 +59,7 @@ class VisionPoseEstimatePlugin : public MavRosPlugin,
 		}
 		else {
 			vision_sub = sp_nh.subscribe("pose", 10, &VisionPoseEstimatePlugin::vision_cb, this);
+			vision_cov_sub = sp_nh.subscribe("pose_cov", 10, &VisionPoseEstimatePlugin::vision_cov_cb, this);
 		}
 	}
 
@@ -72,6 +73,7 @@ class VisionPoseEstimatePlugin : public MavRosPlugin,
 	UAS *uas;
 
 	ros::Subscriber vision_sub;
+	ros::Subscriber vision_cov_sub;
 
 	std::string tf_frame_id;
 	std::string tf_child_frame_id;
@@ -137,6 +139,13 @@ class VisionPoseEstimatePlugin : public MavRosPlugin,
 
 		send_vision_estimate(req->header.stamp, tr);
 	}
+
+	void vision_cov_cb(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &req) {
+		Eigen::Affine3d tr;
+		tf::poseMsgToEigen(req->pose.pose, tr);
+
+		send_vision_estimate(req->header.stamp, tr);
+	}
 };
 };	// namespace mavplugin