From ecddf122bffb42b5e542cf78d24e412e6eba3a6b Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Thu, 28 Dec 2017 04:01:52 +0300 Subject: [PATCH] Stop publishing zero vision positions when local position is obtained --- clever/src/aruco_vpe.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/clever/src/aruco_vpe.cpp b/clever/src/aruco_vpe.cpp index acd66add..890a9822 100644 --- a/clever/src/aruco_vpe.cpp +++ b/clever/src/aruco_vpe.cpp @@ -41,7 +41,8 @@ private: bool use_mocap; nh_priv.param("use_mocap", use_mocap, false); - static ros::Subscriber pose_sub = nh.subscribe("aruco_pose/pose", 1, &ArucoVPE::handleArucoPose, this); + static ros::Subscriber pose_sub = nh.subscribe("mavros/local_position/pose", 1, &ArucoVPE::handlePose, this); + static ros::Subscriber aruco_pose_sub = nh.subscribe("aruco_pose/pose", 1, &ArucoVPE::handleArucoPose, this); vision_position_pub_ = nh.advertise(use_mocap ? "mavros/mocap/pose" : "mavros/vision_pose/pose", 1); @@ -61,6 +62,13 @@ private: vision_position_pub_.publish(ps); } + void handlePose(const geometry_msgs::PoseStampedConstPtr& pose) + { + // local position is inited, stop posting dummy position + ROS_INFO_ONCE("Got local position, stop publishing zeroes"); + dummy_vision_timer_.stop(); + } + void handleArucoPose(const geometry_msgs::PoseStampedConstPtr& pose) { static TransformBroadcaster br;