From deaba007cd450c1df93fe3f32879c59635e794fc Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Fri, 22 Dec 2017 02:56:04 +0300
Subject: [PATCH] `aruco_orientation`, `use_mocap` params for aruco_vpe

---
 clever/src/aruco_vpe.cpp | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/clever/src/aruco_vpe.cpp b/clever/src/aruco_vpe.cpp
index 547be33b..acd66add 100644
--- a/clever/src/aruco_vpe.cpp
+++ b/clever/src/aruco_vpe.cpp
@@ -37,9 +37,13 @@ private:
         ros::NodeHandle& nh = getNodeHandle();
         ros::NodeHandle& nh_priv = getPrivateNodeHandle();
 
+        nh_priv.param<string>("aruco_orientation", aruco_orientation_, "local_origin");
+        bool use_mocap;
+        nh_priv.param<bool>("use_mocap", use_mocap, false);
+
         static ros::Subscriber pose_sub = nh.subscribe("aruco_pose/pose", 1, &ArucoVPE::handleArucoPose, this);
-        vision_position_pub_ = nh.advertise<PoseStamped>("mavros/vision_pose/pose", 1);
-        nh_priv.param<std::string>("aruco_orientation", aruco_orientation_, "local_origin");
+
+        vision_position_pub_ = nh.advertise<PoseStamped>(use_mocap ? "mavros/mocap/pose" : "mavros/vision_pose/pose", 1);
 
         ROS_INFO("aruco orientation frame: %s", aruco_orientation_.c_str());