From 69f8b6e5456876d20d3e6f1e9c7a21318d7ae4f2 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Thu, 28 Dec 2017 04:02:19 +0300 Subject: [PATCH] ~reset_vpe option to reset VPE only once (enabled by default if MOCAP package is used) --- clever/src/aruco_vpe.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/clever/src/aruco_vpe.cpp b/clever/src/aruco_vpe.cpp index 890a9822..122297c1 100644 --- a/clever/src/aruco_vpe.cpp +++ b/clever/src/aruco_vpe.cpp @@ -31,6 +31,7 @@ private: ros::Publisher vision_position_pub_; ros::Timer dummy_vision_timer_; string aruco_orientation_; + bool reset_vpe_; void onInit() { @@ -40,6 +41,7 @@ private: nh_priv.param("aruco_orientation", aruco_orientation_, "local_origin"); bool use_mocap; nh_priv.param("use_mocap", use_mocap, false); + nh_priv.param("reset_vpe", reset_vpe_, !use_mocap); 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); @@ -97,8 +99,8 @@ private: t.child_frame_id = "aruco_map_vision"; br.sendTransform(t); - // Reset VPE - if (!last_published_.isValid() or ros::Time::now() - last_published_ > ros::Duration(2)) + if (last_published_.toSec() == 0 || // no vpe has been posted + (reset_vpe_ && (ros::Time::now() - last_published_ > ros::Duration(2)))) // vpe origin outdated { ROS_INFO("Reset VPE"); t = tf_buffer.lookupTransform("local_origin", "aruco_map_vision", stamp, lookup_timeout_);