descriptor_surface_based_recognition.h

Go to the documentation of this file.

#ifndef DESCRIPTOR_SURFACED_BASED_RECOGNITION_H
#define DESCRIPTOR_SURFACED_BASED_RECOGNITION_H

#include <ros/ros.h>
#include <dynamic_reconfigure/server.h>
#include <asr_descriptor_surface_based_recognition/DescriptorSurfaceBasedRecognitionConfig.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <visualization_msgs/MarkerArray.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>

#include <Eigen/Dense>

#include <asr_msgs/AsrObject.h>
#include <asr_object_database/ObjectMetaData.h>
#include <asr_object_database/RecognizerListMeshes.h>

#include <asr_descriptor_surface_based_recognition/GetRecognizer.h>
#include <asr_descriptor_surface_based_recognition/ReleaseRecognizer.h>
#include <asr_descriptor_surface_based_recognition/GetObjectList.h>
#include <asr_descriptor_surface_based_recognition/ClearAllRecognizers.h>

#include "threadpool/threadpool.hpp"
#include "pose_validation.h"
#include "object_descriptor.h"
#include "pose_recognition.h"
#include "object_2D_positions.h"
#include "util.h"


namespace descriptor_surface_based_recognition {

using namespace asr_descriptor_surface_based_recognition;

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

const static std::string GET_RECOGNIZER_SERVICE_NAME("get_recognizer");
const static std::string RELEASE_RECOGNIZER_SERVICE_NAME("release_recognizer");
const static std::string GET_OBJECT_LIST_SERVICE_NAME("get_object_list");
const static std::string CLEAR_ALL_RECOGNIZERS_SERVICE_NAME("clear_all_recognizers");
const static std::string OBJECT_DB_SERVICE_OBJECT_TYPE ("/asr_object_database/object_meta_data");
const static std::string OBJECT_DB_SERVICE_OBJECT_MESHES("/asr_object_database/recognizer_list_meshes");

const static std::string OUTPUT_EVALUATION_DIR("eval");
const static std::string OUTPUT_EVALUATION_FILE_TIME("global_times.txt");
const static std::string OUTPUT_EVALUATION_FILE_POSES("global_poses.txt");

const static std::string OBJECT_DATABASE_CATEGORY("descriptor");

extern std::string PACKAGE_PATH;



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


class DescriptorSurfaceBasedRecognition {

private:

    std::string image_color_topic_;

    std::string image_mono_topic_;

    std::string point_cloud_topic_;

    std::string output_objects_topic_;

    std::string output_marker_topic_;

    std::string output_marker_bounding_box_topic_;

    std::string output_cloud_topic_;

    std::string output_image_topic_;


    ros::NodeHandle nh_;

    dynamic_reconfigure::Server<DescriptorSurfaceBasedRecognitionConfig> reconfigure_server_;

    DescriptorSurfaceBasedRecognitionConfig config_;

    message_filters::Subscriber<sensor_msgs::Image> image_sub_;
    message_filters::Subscriber<sensor_msgs::Image> image_mono_sub_;
    message_filters::Subscriber<sensor_msgs::PointCloud2> pc_with_guppy_sub_;

    ros::ServiceServer get_recognizer_service_;
    ros::ServiceServer release_recognizer_service_;
    ros::ServiceServer get_object_list_service_;
    ros::ServiceServer clear_all_recognizers_service_;

    ros::ServiceClient object_db_service_client_;
    ros::ServiceClient object_db_meshes_service_client_;

    boost::shared_ptr<ApproximateSync> sync_policy_;

    ros::Publisher objects_pub_;
    ros::Publisher marker_pub_;
    ros::Publisher boxes_pub_;
    ros::Publisher cloud_pub_;
    ros::Publisher image_pub_;


    visualization_msgs::MarkerArrayPtr msgs_marker_array_;
    visualization_msgs::MarkerArrayPtr msgs_box_marker_array_;

    boost::threadpool::pool thread_pool_;

    std::vector<ObjectDescriptor> objects_;

    std::vector<Ogre::MeshPtr> meshes_;

    std::vector<Object2DPositions> last_frame_positions_;

    PoseValidation pose_val_;

    std::ofstream outstream_times_;

    std::ofstream outstream_poses_;

    int frame_counter_;





    void rosCallback(const sensor_msgs::ImageConstPtr& input_image_guppy, const sensor_msgs::ImageConstPtr& input_image_guppy_mono, const sensor_msgs::PointCloud2ConstPtr& input_point_cloud_with_guppy);

    void configCallback(DescriptorSurfaceBasedRecognitionConfig &config_, uint32_t level);

    void threadTask(PoseRecognition *pose_rec);

    asr_msgs::AsrObjectPtr createAsrMessage(PoseRecognition *pose_rec, int results_index, std_msgs::Header header);

    void createMarker(asr_msgs::AsrObjectPtr &object, std::string &mesh_path);

    void clearMarkers();

    void createBoxMarker(RecognitionResult *result, std_msgs::Header header, rgb color, bool drawCompleteBoxes);

    void overlaySceneWith2DResults(PoseRecognition *pose_rec, HalconCpp::HImage *scene_image);

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr createVisualizationCloud(std::vector<PoseRecognition*> &pose_recs, std_msgs::Header header);

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

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

    bool processGetObjectListRequest(GetObjectList::Request &req, GetObjectList::Response &res);

    bool processClearAllRecognizersRequest(ClearAllRecognizers::Request &req, ClearAllRecognizers::Response &res);

    void initializeMeshes();

    bool startObjectRecognition(std::string name, int count, bool use_pose_val);

    void stopObjectRecognition(std::string name);

public:

    DescriptorSurfaceBasedRecognition();

};

}

#endif