fake_object_recognition.h

Go to the documentation of this file.

#ifndef FAKE_OBJECT_RECOGNITION_H
#define FAKE_OBJECT_RECOGNITION_H

#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <object_config.h>
#include <asr_fake_object_recognition/GetRecognizer.h>
#include <asr_fake_object_recognition/ReleaseRecognizer.h>
#include <asr_fake_object_recognition/GetAllRecognizers.h>
#include <asr_fake_object_recognition/ClearAllRecognizers.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <asr_msgs/AsrObject.h>
#include <visualization_msgs/Marker.h>
#include <dynamic_reconfigure/server.h>
#include <asr_fake_object_recognition/FakeObjectRecognitionConfig.h>
#include <error_simulation.h>
#include <std_msgs/ColorRGBA.h>
#include <geometry_msgs/Point.h>
//Eigen
#include <Eigen/Geometry>

#include <visualization_msgs/MarkerArray.h>

namespace fake_object_recognition {

using namespace asr_fake_object_recognition;

const static std::string NODE_NAME("asr_fake_object_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_ALL_RECOGNIZERS_SERVICE_NAME("get_all_recognizers");

const static std::string CLEAR_ALL_RECOGNIZERS_SERVICE_NAME("clear_all_recognizers");

class FakeObjectRecognition {

private:
    ros::NodeHandle nh_;

    dynamic_reconfigure::Server<FakeObjectRecognitionConfig> reconfigure_server_;

    FakeObjectRecognitionConfig config_;

    bool config_changed_;

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

    ros::ServiceServer get_all_recognizers_service_;
    ros::ServiceServer clear_all_recognizers_service_;


    ros::Publisher recognized_objects_pub_;
    ros::Publisher recognized_objects_marker_pub_;
    ros::Publisher generated_constellation_marker_pub_;
    ros::Publisher object_normals_pub_;

    ros::Timer timer_;

    bool recognition_released_;

    double fovx_;
    double fovy_;
    double ncp_;
    double fcp_;

    std::string frame_world_;
    std::string frame_camera_left_;
    std::string frame_camera_right_;

    std::string config_file_path_;

    std::string output_rec_objects_topic_;
    std::string output_rec_markers_topic_;
    std::string output_constellation_marker_topic_;
    std::string output_normals_topic_;

    double rating_threshold_d_;
    double rating_threshold_x_;
    double rating_threshold_y_;

    double rating_threshold_;

    double timer_duration_;

    std::vector<ObjectConfig> objects_;

    std::vector<std::string> objects_to_rec_;

    tf::TransformListener listener_;

    ErrorSimulation err_sim_;

    std::map<std::string, std::vector<geometry_msgs::Point>> normals_;

    std::map<std::string, std::array<geometry_msgs::Point, 8>> bounding_box_corners_;

    ros::ServiceClient object_metadata_service_client_;

    std::string bb_corners_file_name_;

    std::string object_database_name_;



    void loadObjects();

    bool parsePoseString(std::string pose_in, geometry_msgs::Pose &pose_out, std::string delim, std::string angles);

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

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

    bool processGetAllRecognizersRequest(GetAllRecognizers::Request &req, GetAllRecognizers::Response &res);

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


    void timerCallback(const ros::TimerEvent &event);

    void configCallback(FakeObjectRecognitionConfig &config, uint32_t level);

    void doRecognition();

    bool objectIsVisible(const geometry_msgs::Pose &pose_left, const geometry_msgs::Pose &pose_right);
    
    bool objectIsVisible(const std::array<geometry_msgs::Point, 8> &bb_left, const std::array<geometry_msgs::Point, 8> &bb_right, const geometry_msgs::Pose &pose_left, const geometry_msgs::Pose &pose_right,
                         const std::vector<geometry_msgs::Point> &normals_left, const std::vector<geometry_msgs::Point> &normals_right);

    geometry_msgs::Pose transformFrame(const geometry_msgs::Pose &pose, const std::string &frame_from, const std::string &frame_to);

    asr_msgs::AsrObjectPtr createAsrMessage(const ObjectConfig &object_config, const geometry_msgs::Pose &pose, const std::string &frame_id);

    visualization_msgs::Marker createMarker(const asr_msgs::AsrObjectPtr &object, int id, int lifetime, bool use_col_init = false);

    static std_msgs::ColorRGBA getMeshColor(std::string observed_id);

    static std_msgs::ColorRGBA createColorRGBA(float red, float green, float blue, float alpha);

    std::vector<geometry_msgs::Point> transformPoints(std::vector<geometry_msgs::Point> points_list, Eigen::Quaterniond rotation, Eigen::Vector3d translation);

    std::array<geometry_msgs::Point, 8> transformPoints(std::array<geometry_msgs::Point, 8> points_list, Eigen::Quaterniond rotation, Eigen::Vector3d translation);

    visualization_msgs::MarkerArray::Ptr createNormalMarker(const ObjectConfig &object, int id, int lifetime);

    std::vector<geometry_msgs::Point> getNormals(const ObjectConfig &object);

     bool getBBfromFile(std::array<geometry_msgs::Point, 8> &result, std::string object_type);

    std::array<geometry_msgs::Point, 8> calculateBB(const ObjectConfig &object);

    geometry_msgs::Point createPoint(double x, double y, double z);

public:
    FakeObjectRecognition();

};

}


#endif /* FAKE_OBJECT_RECOGNITION_H */