00001 #ifndef CHECKERBOARD_POSE_ESTIMATION_ROS_DETECTOR_H 00002 #define CHECKERBOARD_POSE_ESTIMATION_ROS_DETECTOR_H 00003 00004 #include <ros/ros.h> 00005 #include <image_transport/image_transport.h> 00006 #include <tf/tf.h> 00007 #include <tf/transform_broadcaster.h> 00008 #include <checkerboard_pose_estimation/detector.h> 00009 #include <checkerboard_pose_estimation/estimator.h> 00010 00011 namespace checkerboard_pose_estimation { 00012 00013 class RosDetector 00014 { 00015 public: 00016 RosDetector(const std::string& name = ros::this_node::getName()); 00017 00018 bool initFromParameters(const ros::NodeHandle& nh = ros::NodeHandle("~")); 00019 00020 Detector& getDetector() { return detector_; } 00021 const Detector& getDetector() const { return detector_; } 00022 00023 visual_pose_estimation::PoseEstimator& getPoseEstimator() { return pose_estimator_; } 00024 const visual_pose_estimation::PoseEstimator& getPoseEstimator() const { return pose_estimator_; } 00025 00026 // detected object = checkerboard, target = plug 00027 bool detectObject(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& info_msg, 00028 const tf::Stamped<tf::Pose>& target_prior, const tf::Transformer& transformer, 00029 tf::Stamped<tf::Pose>& target_pose); 00030 00031 void publishDisplayImage(const cv::Mat& source, const std::vector<cv::Point2f>& corners, bool success); 00032 00033 protected: 00034 ros::NodeHandle nh_; 00035 image_transport::ImageTransport it_; 00036 std::string name_; 00037 00038 // Publications 00039 tf::TransformBroadcaster tf_broadcaster_; 00040 image_transport::Publisher display_pub_; 00041 sensor_msgs::Image display_img_; 00042 cv::Mat display_img_cv_; 00043 ros::Publisher marker_pub_; 00044 00045 // Message processing 00046 image_geometry::PinholeCameraModel cam_model_; 00047 Detector detector_; 00048 visual_pose_estimation::PoseEstimator pose_estimator_; 00050 tf::Transform target_in_detected_object_; 00051 tf::Stamped<tf::Pose> target_prior_; 00052 }; 00053 00054 } //namespace checkerboard_pose_estimation 00055 00056 #endif