00001 /* 00002 * ar_marker_processor.hpp 00003 * 00004 * LICENSE : BSD - https://raw.github.com/yujinrobot/yujin_ocs/license/LICENSE 00005 * 00006 * Created on: Apr 6, 2013 00007 * Author: jorge 00008 * Modified on: Dec, 2013 00009 * Jihoon Lee 00010 * 00011 */ 00012 00013 #ifndef AR_PAIR_TRACKING_HPP_ 00014 #define AR_PAIR_TRACKING_HPP_ 00015 00016 /***************************************************************************** 00017 ** Includes 00018 *****************************************************************************/ 00019 00020 #include <ros/ros.h> 00021 #include <tf/tf.h> 00022 #include <tf/transform_listener.h> 00023 #include <tf/transform_broadcaster.h> 00024 #include <yocs_math_toolkit/common.hpp> 00025 #include <yocs_math_toolkit/geometry.hpp> 00026 #include <ar_track_alvar_msgs/AlvarMarkers.h> 00027 #include <yocs_ar_marker_tracking/tracking.hpp> 00028 #include <yocs_msgs/ARPair.h> 00029 #include <yocs_msgs/ARPairList.h> 00030 00031 /***************************************************************************** 00032 ** Namespaces 00033 *****************************************************************************/ 00034 00035 namespace yocs 00036 { 00037 00038 namespace ARPairTrackingDefaultParams 00039 { 00040 const int AR_PAIR_LEFT_ID = 3; 00041 const int AR_PAIR_RIGHT_ID = 0; 00042 const double AR_PAIR_BASELINE = 0.26; 00043 const double AR_PAIR_TARGET_POSE_OFFSET = 0.40; 00044 00045 const bool PUBLISH_TRANSFORMS = true; 00046 const std::string PUB_RELATIVE_TARGET_POSE = "relative_target_pose"; 00047 const std::string PUB_INITIAL_POSE = "initial_pose"; 00048 const std::string PUB_SPOTTED_MARKERS = "spotted_markers"; 00049 const std::string SUB_UPDATE_AR_PAIRS = "update_ar_pairs"; 00050 const std::string GLOBAL_FRAME = "ar_global"; 00051 const std::string MARKER_FRAME = "camera_rgb_optical_frame"; 00052 const std::string BASE_FRAME = "base_footprint"; 00053 const std::string TARGET_POSE_FRAME = "target_pose"; 00054 } 00055 00056 /***************************************************************************** 00057 ** Classes 00058 *****************************************************************************/ 00059 00060 class ARPairTracking : public ARMarkerTracking 00061 { 00062 public: 00063 00064 ARPairTracking(); 00065 virtual ~ARPairTracking(); 00066 00067 bool init(); 00068 void addPair(const yocs_msgs::ARPair& p); 00069 00070 protected: 00071 // raw list of ar markers from ar_alvar_track pkg 00072 void customCB(const ar_track_alvar_msgs::AlvarMarkers& spotted_markers, const std::vector<TrackedMarker> &tracked_markers); 00073 bool spotMarkerPair(const ar_track_alvar_msgs::AlvarMarkers& spotted_markers, const yocs_msgs::ARPair& pair, ar_track_alvar_msgs::AlvarMarker& left, ar_track_alvar_msgs::AlvarMarker& right); 00074 void computeRelativeRobotPose(const yocs_msgs::ARPair& spotted_pair, const std::vector<TrackedMarker>& tracked_markers, const ar_track_alvar_msgs::AlvarMarker& left, const ar_track_alvar_msgs::AlvarMarker& right); 00075 00076 void updateARPairsCB(const yocs_msgs::ARPairList::ConstPtr& msg); 00077 00078 private: 00079 // Confidence evaluation attributes 00080 00081 ros::Publisher pub_relative_target_pose_, pub_initial_pose_, pub_spotted_markers_; 00082 ros::Subscriber sub_update_ar_pairs_; 00083 00084 tf::Transformer tf_internal_; 00085 tf::TransformListener tf_listener_; 00086 tf::TransformBroadcaster tf_brcaster_; 00087 // double tf_broadcast_freq_; /**< Allows enabling tf broadcasting; mostly for debug */ 00088 00089 std::vector<yocs_msgs::ARPair> ar_pairs_; 00090 bool publish_transforms; 00091 00092 std::string global_frame_; 00093 std::string marker_frame_; 00094 std::string base_frame_; 00095 std::string target_pose_frame_; 00096 }; 00097 00098 } /* namespace yocs */ 00099 00100 #endif /* AR_MARKERS_HPP_ */