comthread.cpp

Go to the documentation of this file.

#include "comthread.h"

#include <ros/ros.h>
#include <re_vision/SearchFor.h>
#include <std_msgs/String.h>
#include <sensor_msgs/Image.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>

#include <QDebug>

#include <re_vision/SearchFor.h>

ComThread::ComThread(QObject *parent) :
    QThread(parent), image_transport(nh)
{
    zaragoza_client = nh.serviceClient<re_vision::SearchFor>("/re_vision/search_for");
    zaragoza_model_dir_pub = nh.advertise<std_msgs::String>("/re_vslam/new_model", 1);

    kinect_model_dir_pub = nh.advertise<std_msgs::String>("/re_kinect/model_path", 1);

    image_sub = image_transport.subscribe("/camera/rgb/image_color", 1, &ComThread::imageCb, this);
    kinect_detected_objects_sub = nh.subscribe<re_kinect_object_detector::DetectionResult>("/re_kinect/detection_results", 10, boost::bind(&ComThread::kinect_detected_objectCb, this, _1));
}

void ComThread::run()
{
    ros::spin();
}

void ComThread::publishModelDir(QString model_dir, QString model_type, QString model_name) {
    std_msgs::String msg;
    msg.data = model_dir.toStdString();
    if ((model_type == "3D") && zaragoza_client.exists()) {
        zaragoza_model_dir_pub.publish(msg);
        if (std::find(zaragoza_model_names.begin(), zaragoza_model_names.end(),
                      model_name.toStdString()) == zaragoza_model_names.end()) {
            zaragoza_model_names.push_back(model_name.toStdString());
        }
    }
    else if (model_type == "kinect_pcl") {
        kinect_model_dir_pub.publish(msg);
    }

}

QImage cvMatToQImage(const cv::Mat& image)
{
    QImage qImage(
        image.data,
        image.size().width,
        image.size().height,
        image.step,
        QImage::Format_RGB888
    );

    return qImage.rgbSwapped();
}

void ComThread::kinect_detected_objectCb(const re_kinect_object_detector::DetectionResultConstPtr &msg) {
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
        cv_ptr = cv_bridge::toCvCopy(msg->Image,"bgr8");
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

     for(size_t i=0; i<msg->Detections.size(); i++) {
         const re_msgs::DetectedObject&  detectedObject = msg->Detections.at(i);

         for (size_t j=0; j < detectedObject.points2d.size(); j++) {
             if (detectedObject.points2d.at(j).y < cv_ptr->image.size().height &&
                     detectedObject.points2d.at(j).y > 0 &&
                     detectedObject.points2d.at(j).x < cv_ptr->image.size().width &&
                     detectedObject.points2d.at(j).x > 0)
                 cv_ptr->image.at<cv::Vec3b>(detectedObject.points2d.at(j).y, detectedObject.points2d.at(j).x) = cv::Vec3b(0, 0, 255);
         }
     }
     emit updateKinectDetectionImg(cvMatToQImage(cv_ptr->image));
}

void ComThread::imageCb(const sensor_msgs::ImageConstPtr &msg) {
    if (zaragoza_model_names.empty())
        return;

    re_vision::SearchForRequest req;
    req.Image = *msg;
    req.Objects = zaragoza_model_names;
    req.MaxPointsPerObject = -1;

    re_vision::SearchForResponse resp;
    zaragoza_client.call(req, resp);


//    qDebug() << "detections length: " << resp.Detections.size();
    if (resp.Detections.size() == 0)
        return;

    cv_bridge::CvImagePtr cv_ptr;
    try
    {
        cv_ptr = cv_bridge::toCvCopy(msg,"bgr8");
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    // mark object pixels
    bool marked_one = false;
    for(int i=0; i<resp.Detections.size(); i++) {
        const re_msgs::DetectedObject&  detectedObject = resp.Detections.at(i);

        for (int j=0; j < detectedObject.points2d.size(); j++) {
            marked_one = true;
            cv_ptr->image.at<cv::Vec3b>(detectedObject.points2d.at(j).y, detectedObject.points2d.at(j).x) = cv::Vec3b(0, 0, 255);
        }
    }

    if (marked_one)
        emit updateZaragozaDetectionImg(cvMatToQImage(cv_ptr->image));
}