aruco_marker_recognition.h

Go to the documentation of this file.

#ifndef ARUCO_MARKER_RECOGNITION_H
#define ARUCO_MARKER_RECOGNITION_H

#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <opencv2/highgui/highgui.hpp>
#include <ivt_bridge/ivt_calibration.h>
#include <visualization_msgs/MarkerArray.h>
#include <asr_aruco_marker_recognition/GetRecognizer.h>
#include <asr_aruco_marker_recognition/ReleaseRecognizer.h>
#include <asr_object_database/ObjectMetaData.h>
#include <asr_msgs/AsrObject.h>
#include <dynamic_reconfigure/server.h>
#include <asr_aruco_marker_recognition/ArucoMarkerRecognitionConfig.h>

#include "marker_detection.h"

namespace aruco_marker_recognition {

using namespace asr_aruco_marker_recognition;

const static std::string NODE_NAME("asr_aruco_marker_recognition");

const static double DEFAULT_MARKER_SIZE(0.05);

const static std::string GET_RECOGNIZER_SERVICE_NAME("get_recognizer");
const static std::string RELEASE_RECOGNIZER_SERVICE_NAME("release_recognizer");

const static std::string OBJECT_DB_SERVICE_META_DATA("/asr_object_database/object_meta_data");

typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> ApproximatePolicy;
typedef message_filters::Synchronizer<ApproximatePolicy> ApproximateSync;

class ArucoMarkerRecognition {


private:

    bool use_stereo_;

    std::string image_left_topic_;

    std::string image_right_topic_;

    std::string image_left_cam_info_topic_;

    std::string image_right_cam_info_topic_;

    ros::NodeHandle nh_;

    double marker_size_;

    message_filters::Subscriber<sensor_msgs::Image> image_left_sub_;
    message_filters::Subscriber<sensor_msgs::Image> image_right_sub_;
    message_filters::Subscriber<sensor_msgs::CameraInfo> image_left_param_sub_;
    message_filters::Subscriber<sensor_msgs::CameraInfo> image_right_param_sub_;

    ros::ServiceServer get_recognizer_service_;
    ros::ServiceServer release_recognizer_service_;

    dynamic_reconfigure::Server<ArucoMarkerRecognitionConfig> reconfigure_server_;

    ArucoMarkerRecognitionConfig config_;

    ros::ServiceClient object_mesh_service_client_;

    boost::shared_ptr<ApproximateSync> sync_policy_;

    MarkerDetection detector_;

    ros::Publisher left_markers_img_pub_;
    ros::Publisher right_markers_img_pub_;
    ros::Publisher vis_markers_pub_;
    ros::Publisher asr_objects_pub_;

    std::string recognizer_name_;

    bool recognition_paused_;


    bool processGetRecognizerRequest(GetRecognizer::Request &req, GetRecognizer::Response &res);

    bool processReleaseRecognizerRequest(ReleaseRecognizer::Request &req, ReleaseRecognizer::Response &res);

    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);

    void configCallback(ArucoMarkerRecognitionConfig &config_, uint32_t level);

    cv::Mat drawMarkers(const cv::Mat &image, const std::vector<aruco::Marker> &markers);

    std::map<int, geometry_msgs::Pose> getMarkerPoses(const std::vector<aruco::Marker> &left_markers);

    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);


    visualization_msgs::Marker createSquareMarker(int id, const geometry_msgs::Pose &pose, const string &frame_id);

    visualization_msgs::Marker createArrowMarker(int id, const geometry_msgs::Pose &pose, const string &frame_id, bool isNsX = false);

    visualization_msgs::Marker createMeshMarker(int id, const geometry_msgs::Pose &pose, const std::string &mesh_res, const string &frame_id);

    asr_msgs::AsrObject createAsrObject(const geometry_msgs::Pose &pose, const std::string &object_type, const string &frame_id, const std::string &mesh_res = "");



public:


    ArucoMarkerRecognition();
};

}


#endif