Go to the documentation of this file.00001
00019 #ifndef ARUCO_MARKER_RECOGNITION_H
00020 #define ARUCO_MARKER_RECOGNITION_H
00021
00022 #include <ros/ros.h>
00023 #include <sensor_msgs/Image.h>
00024 #include <message_filters/subscriber.h>
00025 #include <message_filters/synchronizer.h>
00026 #include <message_filters/sync_policies/approximate_time.h>
00027 #include <opencv2/highgui/highgui.hpp>
00028 #include <ivt_bridge/ivt_calibration.h>
00029 #include <visualization_msgs/MarkerArray.h>
00030 #include <asr_aruco_marker_recognition/GetRecognizer.h>
00031 #include <asr_aruco_marker_recognition/ReleaseRecognizer.h>
00032 #include <asr_object_database/ObjectMetaData.h>
00033 #include <asr_msgs/AsrObject.h>
00034 #include <dynamic_reconfigure/server.h>
00035 #include <asr_aruco_marker_recognition/ArucoMarkerRecognitionConfig.h>
00036
00037 #include "marker_detection.h"
00038
00039 namespace aruco_marker_recognition {
00040
00041 using namespace asr_aruco_marker_recognition;
00042
00044 const static std::string NODE_NAME("asr_aruco_marker_recognition");
00045
00047 const static double DEFAULT_MARKER_SIZE(0.05);
00048
00050 const static std::string GET_RECOGNIZER_SERVICE_NAME("get_recognizer");
00051 const static std::string RELEASE_RECOGNIZER_SERVICE_NAME("release_recognizer");
00052
00054 const static std::string OBJECT_DB_SERVICE_META_DATA("/asr_object_database/object_meta_data");
00055
00056 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> ApproximatePolicy;
00057 typedef message_filters::Synchronizer<ApproximatePolicy> ApproximateSync;
00058
00062 class ArucoMarkerRecognition {
00063
00064
00065 private:
00066
00067 bool use_stereo_;
00068
00070 std::string image_left_topic_;
00071
00073 std::string image_right_topic_;
00074
00076 std::string image_left_cam_info_topic_;
00077
00079 std::string image_right_cam_info_topic_;
00080
00082 ros::NodeHandle nh_;
00083
00085 double marker_size_;
00086
00088 message_filters::Subscriber<sensor_msgs::Image> image_left_sub_;
00089 message_filters::Subscriber<sensor_msgs::Image> image_right_sub_;
00090 message_filters::Subscriber<sensor_msgs::CameraInfo> image_left_param_sub_;
00091 message_filters::Subscriber<sensor_msgs::CameraInfo> image_right_param_sub_;
00092
00094 ros::ServiceServer get_recognizer_service_;
00095 ros::ServiceServer release_recognizer_service_;
00096
00098 dynamic_reconfigure::Server<ArucoMarkerRecognitionConfig> reconfigure_server_;
00099
00101 ArucoMarkerRecognitionConfig config_;
00102
00104 ros::ServiceClient object_mesh_service_client_;
00105
00107 boost::shared_ptr<ApproximateSync> sync_policy_;
00108
00110 MarkerDetection detector_;
00111
00113 ros::Publisher left_markers_img_pub_;
00114 ros::Publisher right_markers_img_pub_;
00115 ros::Publisher vis_markers_pub_;
00116 ros::Publisher asr_objects_pub_;
00117
00119 std::string recognizer_name_;
00120
00122 bool recognition_paused_;
00123
00124
00131 bool processGetRecognizerRequest(GetRecognizer::Request &req, GetRecognizer::Response &res);
00132
00139 bool processReleaseRecognizerRequest(ReleaseRecognizer::Request &req, ReleaseRecognizer::Response &res);
00140
00148 void imageCallback(const sensor_msgs::ImageConstPtr& input_img_left, const sensor_msgs::ImageConstPtr& input_img_right, const sensor_msgs::CameraInfoConstPtr& cam_info_left, const sensor_msgs::CameraInfoConstPtr& cam_info_right);
00149
00156 void configCallback(ArucoMarkerRecognitionConfig &config_, uint32_t level);
00157
00164 cv::Mat drawMarkers(const cv::Mat &image, const std::vector<aruco::Marker> &markers);
00165
00166 std::map<int, geometry_msgs::Pose> getMarkerPoses(const std::vector<aruco::Marker> &left_markers);
00167
00175 std::map<int, geometry_msgs::Pose> getMarkerPoses(const std::vector<aruco::Marker> &left_markers, const std::vector<aruco::Marker> &right_markers, const ivt_bridge::IvtStereoCalibration &ivtCalib);
00176
00177
00185 visualization_msgs::Marker createSquareMarker(int id, const geometry_msgs::Pose &pose, const string &frame_id);
00186
00195 visualization_msgs::Marker createArrowMarker(int id, const geometry_msgs::Pose &pose, const string &frame_id, bool isNsX = false);
00196
00205 visualization_msgs::Marker createMeshMarker(int id, const geometry_msgs::Pose &pose, const std::string &mesh_res, const string &frame_id);
00206
00215 asr_msgs::AsrObject createAsrObject(const geometry_msgs::Pose &pose, const std::string &object_type, const string &frame_id, const std::string &mesh_res = "");
00216
00217
00218
00219 public:
00220
00221
00225 ArucoMarkerRecognition();
00226 };
00227
00228 }
00229
00230
00231 #endif
00232