From 9c9078d23ddd2989e377c791350f2f57bab361bf Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sun, 10 Feb 2019 02:51:48 +0300 Subject: [PATCH] Remove aruco_vpe nodelet --- clever/CMakeLists.txt | 8 --- clever/nodelet_plugins.xml | 5 -- clever/src/aruco_vpe.cpp | 137 ------------------------------------- 3 files changed, 150 deletions(-) delete mode 100644 clever/src/aruco_vpe.cpp diff --git a/clever/CMakeLists.txt b/clever/CMakeLists.txt index 3507a438..e0020d00 100644 --- a/clever/CMakeLists.txt +++ b/clever/CMakeLists.txt @@ -148,10 +148,6 @@ add_library(clever src/optical_flow.cpp ) -add_library(aruco_vpe - src/aruco_vpe.cpp -) - ## Add cmake target dependencies of the library ## as an example, code may need to be generated before libraries ## either from message generation or dynamic reconfigure @@ -196,10 +192,6 @@ target_link_libraries(clever ${catkin_LIBRARIES} ) -target_link_libraries(aruco_vpe - ${catkin_LIBRARIES} -) - ############# ## Install ## ############# diff --git a/clever/nodelet_plugins.xml b/clever/nodelet_plugins.xml index 656bc151..d9e2fc4d 100644 --- a/clever/nodelet_plugins.xml +++ b/clever/nodelet_plugins.xml @@ -3,8 +3,3 @@ - - - - - diff --git a/clever/src/aruco_vpe.cpp b/clever/src/aruco_vpe.cpp deleted file mode 100644 index a1009910..00000000 --- a/clever/src/aruco_vpe.cpp +++ /dev/null @@ -1,137 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "util.h" - -using namespace tf2_ros; -using geometry_msgs::PoseStamped; -using geometry_msgs::TransformStamped; -using std::string; - -class ArucoVPE : public nodelet::Nodelet -{ -public: - ArucoVPE() : - last_published_(0), - lookup_timeout_(0.05) - {} - -private: - ros::Time last_published_; - ros::Duration lookup_timeout_; - ros::Duration reset_timeout_; - ros::Publisher vision_position_pub_; - ros::Timer dummy_vision_timer_; - string aruco_orientation_; - bool reset_vpe_; - - void onInit() - { - ros::NodeHandle& nh = getNodeHandle(); - ros::NodeHandle& nh_priv = getPrivateNodeHandle(); - - nh_priv.param("aruco_orientation", aruco_orientation_, "map"); - bool use_mocap; - nh_priv.param("use_mocap", use_mocap, false); - nh_priv.param("reset_vpe", reset_vpe_, !use_mocap); - double reset_timeout; - nh_priv.param("reset_timeout", reset_timeout, 2.0); - reset_timeout_ = ros::Duration(reset_timeout); - - 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); - - ROS_INFO("aruco orientation frame: %s", aruco_orientation_.c_str()); - - dummy_vision_timer_ = nh.createTimer(ros::Duration(0.5), &ArucoVPE::publishDummy, this); - - ROS_INFO("Aruco VPE initialized"); - } - - void publishDummy(const ros::TimerEvent&) - { - // This is published to init FCU's position estimator - static PoseStamped ps; - ps.header.stamp = ros::Time::now(); - ps.pose.orientation.w = 1; - 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; - static Buffer tf_buffer; - static TransformListener tfListener(tf_buffer); - static StaticTransformBroadcaster static_br; - static PoseStamped ps, vpe_raw, vpe; - TransformStamped t; - - ros::Time stamp = pose->header.stamp; - double roll, pitch, yaw; - - try - { - // Refine aruco map pose - // Reference in local origin - t = tf_buffer.lookupTransform(aruco_orientation_, "aruco_map_reference", stamp, lookup_timeout_); - quaternionToEuler(t.transform.rotation, roll, pitch, yaw); - eulerToQuaternion(t.transform.rotation, 0, 0, yaw); - t.child_frame_id = "aruco_map_reference_horiz"; - br.sendTransform(t); - - // Aruco map in reference - t = tf_buffer.lookupTransform("aruco_map_reference", "aruco_map_raw", stamp, lookup_timeout_); - t.header.frame_id = "aruco_map_reference_horiz"; - t.child_frame_id = "aruco_map_vision"; - br.sendTransform(t); - - if (last_published_.toSec() == 0 || // no vpe has been posted - (reset_vpe_ && (ros::Time::now() - last_published_ > reset_timeout_))) // vpe origin outdated - { - ROS_INFO("Reset VPE"); - t = tf_buffer.lookupTransform("map", "aruco_map_vision", stamp, lookup_timeout_); - t.child_frame_id = "aruco_map"; - static_br.sendTransform(t); - } - - // Calculate VPE - ps.header.frame_id = "body"; - ps.header.stamp = stamp; - ps.pose.orientation.w = 1; - - tf_buffer.transform(ps, vpe_raw, "aruco_map_vision", lookup_timeout_); - - vpe_raw.header.frame_id = "aruco_map"; - tf_buffer.transform(vpe_raw, vpe, "map", lookup_timeout_); - - vision_position_pub_.publish(vpe); - - last_published_ = stamp; - dummy_vision_timer_.stop(); - } - catch (const tf2::TransformException& e) - { - ROS_WARN_THROTTLE(10, "Aruco VPE: failed to transform: %s", e.what()); - } - } -}; - -PLUGINLIB_EXPORT_CLASS(ArucoVPE, nodelet::Nodelet)