detection_tracker_node.h

Go to the documentation of this file.

#ifndef _DETECTION_TRACKER_
#define _DETECTION_TRACKER_

//##################
//#### includes ####

// standard includes
//--

// ROS includes
#include <ros/ros.h>
#include <ros/package.h>

// ROS message includes
#include <sensor_msgs/Image.h>
//#include <sensor_msgs/PointCloud2.h>
#include <cob_perception_msgs/DetectionArray.h>

// services
//#include <cob_people_detection/DetectPeople.h>

// topics
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>

// opencv
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>

// boost
#include <boost/bind.hpp>
#include <boost/thread/mutex.hpp>

// external includes
#include "cob_vision_utils/GlobalDefines.h"

#include <sstream>
#include <string>
#include <vector>

namespace ipa_PeopleDetector
{

class DetectionTrackerNode
{
protected:
        image_transport::ImageTransport* it_;
        image_transport::SubscriberFilter people_segmentation_image_sub_; 

        message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<cob_perception_msgs::DetectionArray, sensor_msgs::Image> >* sync_input_2_;
        message_filters::Subscriber<cob_perception_msgs::DetectionArray> face_position_subscriber_; 
        ros::Publisher face_position_publisher_; 

        ros::NodeHandle node_handle_; 

        std::vector<cob_perception_msgs::Detection> face_position_accumulator_; 
        boost::timed_mutex face_position_accumulator_mutex_; 
        std::vector<std::map<std::string, double> > face_identification_votes_; 

        // parameters
        bool debug_; 
        bool use_people_segmentation_; 
        double face_redetection_time_; 
        ros::Duration publish_currently_not_visible_detections_timespan_; 
        double min_segmented_people_ratio_face_; 
        double min_segmented_people_ratio_head_; 
        double tracking_range_m_; 
        double face_identification_score_decay_rate_; 
        double min_face_identification_score_to_publish_; 
        bool fall_back_to_unknown_identification_; 
        bool display_timing_;

        bool rosbag_mode_; 

public:

        DetectionTrackerNode(ros::NodeHandle nh);

        ~DetectionTrackerNode();

        unsigned long convertColorImageMessageToMat(const sensor_msgs::Image::ConstPtr& image_msg, cv_bridge::CvImageConstPtr& image_ptr, cv::Mat& image);

        unsigned long copyDetection(const cob_perception_msgs::Detection& src, cob_perception_msgs::Detection& dest, bool update = false,
                        unsigned int updateIndex = UINT_MAX);

        double computeFacePositionDistanceTrackingRange(const cob_perception_msgs::Detection& previous_detection, const cob_perception_msgs::Detection& current_detection);

        double computeFacePositionDistance(const cob_perception_msgs::Detection& previous_detection, const cob_perception_msgs::Detection& current_detection);

        unsigned long removeMultipleInstancesOfLabel();

        unsigned long prepareFacePositionMessage(cob_perception_msgs::DetectionArray& face_position_msg_out, ros::Time image_recording_time, std::string frame_id);

        void inputCallback(const cob_perception_msgs::DetectionArray::ConstPtr& face_position_msg_in, const sensor_msgs::Image::ConstPtr& people_segmentation_image_msg);

};

}
;

#endif // _DETECTION_TRACKER_