CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-27 05:29:32 +00:00
Code Issues Packages Projects Releases Wiki Activity

~reset_vpe option to reset VPE only once (enabled by default if MOCAP package is used)

Browse Source
This commit is contained in:
Oleg Kalachev
2017-12-28 04:02:19 +03:00
parent ecddf122bf
commit 69f8b6e545
1 changed files with 4 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
clever/src/aruco_vpe.cpp
View File

@@ -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<string>("aruco_orientation", aruco_orientation_, "local_origin");
bool use_mocap;
nh_priv.param<bool>("use_mocap", use_mocap, false);
nh_priv.param<bool>("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_);