00001 00002 00003 #ifndef TARGET_OBJECT_RECOGNIZER_H 00004 #define TARGET_OBJECT_RECOGNIZER_H 00005 00006 #include <iostream> 00007 #include <ros/ros.h> 00008 #include <geometry_msgs/Pose.h> 00009 #include <geometry_msgs/PoseStamped.h> 00010 #include <geometry_msgs/PoseWithCovarianceStamped.h> 00011 #include <jsk_recognition_msgs/BoundingBoxArray.h> 00012 #include <tf/transform_broadcaster.h> 00013 #include <tf/transform_datatypes.h> 00014 #include <tf/transform_listener.h> 00015 #include <boost/thread.hpp> 00016 00017 class TargetObject 00018 { 00019 public: 00020 TargetObject(geometry_msgs::PoseStamped transed_pose, 00021 jsk_recognition_msgs::BoundingBox box); 00022 geometry_msgs::Pose getPose(); 00023 void addTargetObject(geometry_msgs::PoseStamped transed_pose, 00024 jsk_recognition_msgs::BoundingBox new_box); 00025 int getCounter(); 00026 void addCounter(); 00027 jsk_recognition_msgs::BoundingBox getBox(); 00028 ~TargetObject(); 00029 private: 00030 jsk_recognition_msgs::BoundingBox box_; 00031 geometry_msgs::Pose pose_; 00032 geometry_msgs::Vector3 dimensions_; 00033 int counter_; 00034 }; 00035 00036 class TargetObjectRecognizer 00037 { 00038 public: 00039 TargetObjectRecognizer(ros::NodeHandle nh); 00040 ~TargetObjectRecognizer(); 00041 void detectedCallback(const jsk_recognition_msgs::BoundingBoxArray::ConstPtr &detected_objects); 00042 void robotPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& amcl_pose); 00043 void run(); 00044 private: 00045 double calcDistance(geometry_msgs::Pose pose_1, geometry_msgs::Pose pose_2); 00046 ros::NodeHandle nh_; 00047 ros::Rate rate_; 00048 ros::Publisher recognized_pub_; 00049 ros::Subscriber detected_sub_; 00050 ros::Subscriber robotpose_sub_; 00051 tf::TransformListener tf_; 00052 geometry_msgs::PoseWithCovarianceStamped latest_robot_pose_; 00053 std::vector<TargetObject> target_object_candidates_; 00054 boost::mutex robot_pose_mutex_; 00055 }; 00056 00057 00058 #endif /* TARGET_OBJECT_RECOGNIZER_H */