FaceContourRequestHandler.cpp

Go to the documentation of this file.

#include <face_contour_detector/FaceContourRequestHandler.h>

//
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/CvBridge.h>
#include <image_transport/image_transport.h>

//service definitions
#include <face_contour_detector/GetFilters.h>
#include <face_contour_detector/ApplyFilters.h>
#include <face_contour_detector/autoselect_result.h>
#include <face_contour_detector/ContourDetectorGUI.h>
#include <face_contour_detector/experience_request.h>
#include <face_contour_detector/filters/MaskBlur.h>

#include <portrait_robot_msgs/alubsc_node_instr.h>
#include <portrait_robot_msgs/alubsc_status_msg.h>

//tinyxml
#include <tinyxml/tinyxml.h>

//
#include <face_contour_detector/AutoParameterExplorer.h>
#include <boost/function.hpp>
#include <face_contour_detector/AreaParameterChooserWorker.h>

namespace face_contour_detector {

FaceContourRequestHandler::FaceContourRequestHandler(ros::NodeHandle& node,
                std::string exploreConfigPath) :
                node(node), exploreConfigPath(exploreConfigPath) {
}
FaceContourRequestHandler::~FaceContourRequestHandler() {
        while (!requests.empty()) {
                delete requests.front().image;
                delete requests.front().mask;
                requests.pop();
        }
}

void FaceContourRequestHandler::SolveOneRequest() {
        if (!requests.empty()) {
                //load latest request

                cv::Mat* currentImg = requests.front().image;
                cv::Mat* currentMask = requests.front().mask;

                requests.pop();

                M_SendStatusMessage("Blurring around masked areas");

                //Apply MaskBlur filter
                cv::Mat* maskedImage = new cv::Mat(currentImg->rows, currentImg->cols,
                                CV_8UC3);
                {
                        //setting the clearcolor to white
                        face_contour_detector::Bgr8 clearcolor;
                        clearcolor.b = 255;
                        clearcolor.g = 255;
                        clearcolor.r = 255;
                        face_contour_detector::filters::MaskBlur mb =
                                        face_contour_detector::filters::MaskBlur(10, clearcolor);
                        //mask to gray scale image
                        cv::Mat bwMask = cv::Mat(currentMask->rows, currentMask->cols,
                                        CV_8UC1);
                        cv::cvtColor(*currentMask, bwMask, CV_BGR2GRAY);
                        mb.SetMask(bwMask, 0, 255);
                        //apply the BLUUUURIIIING
                        mb.Apply(*currentImg, *maskedImage);
                        //cleaning up and setting the pointer
                        delete currentImg;
                        currentImg = maskedImage;
                        cv::imwrite("parameter_autoselect_bluredImage.png", *currentImg);
                }

                M_SendStatusMessage("starting exploration for good parameters...");

                //save the results here
                std::vector<face_contour_detector::autoselect_result> result;

                //worker 1
                std::vector<WorkerSetting> settings;
                M_LoadExploreSettings(settings);

                std::vector<WorkerSetting>::iterator setIt;
                int i = 1;
                for (setIt = settings.begin(); setIt != settings.end(); setIt++) {
                        face_contour_detector::AreaParameterChooserWorker worker(node,
                                        *currentImg, *currentMask, setIt->targetValues);
                        worker.CallculateParameters(setIt->numResults, setIt->steps,
                                        result);

                        if (result.size() < setIt->numResults) {
                                ROS_ERROR("did not get enough results for the request");
                                M_SendStatusMessage(
                                                "did not get enough results for the request, please try to send a better image");
                                delete currentImg;
                                delete currentMask;
                                return;
                        } else {
                                std::stringstream sstr;
                                sstr << "found " << result.size()
                                                << " parameter combination(s) by exploring with worker "
                                                << i;
                                M_SendStatusMessage(sstr.str());
                        }
                        i++;
                }

                /*int steps = 15;
                 std::map<std::string, std::map<std::string, double> > targetValue;
                 targetValue["LeftEye"]["graphmass"] = 0.35;
                 targetValue["RightEye"]["graphmass"] = 0.35;
                 targetValue["Nose"]["graphmass"] = 0.35;
                 targetValue["Mouth"]["graphmass"] = 0.35;
                 targetValue["Complete Face"]["graphmass"] = 0.25;

                 */

                M_SendStatusMessage(
                                "try to find parameters by searching rated results...");

                //sending to the next selector
                ros::ServiceClient exprClient = node.serviceClient<
                                face_contour_detector::experience_request>(
                                "experience_autoselector");
                face_contour_detector::experience_request exprSrv;

                cv::Mat imgCopy = currentImg->clone();
                cv::Mat maskCopy = currentMask->clone();

                try {
                        sensor_msgs::CvBridge cvBridge;
                        IplImage ipl = imgCopy;
                        exprSrv.request.image = *(cvBridge.cvToImgMsg(&ipl));

                } catch (sensor_msgs::CvBridgeException& error) {
                        ROS_ERROR(
                                        "could not convert the image with the cvBridge to a sensor_msgs/Image");
                        return;
                }

                try {
                        sensor_msgs::CvBridge cvBridge;
                        IplImage ipl = maskCopy;
                        exprSrv.request.mask = *(cvBridge.cvToImgMsg(&ipl));

                } catch (sensor_msgs::CvBridgeException& error) {
                        ROS_ERROR(
                                        "could not convert the image with the cvBridge to a sensor_msgs/Image");
                        return;
                }

                if (exprClient.call(exprSrv)) {
                        std::vector<face_contour_detector::autoselect_result>::iterator it;
                        for (it = exprSrv.response.proposals.begin();
                                        it != exprSrv.response.proposals.end(); it++) {
                                result.push_back(*it);
                        }
                        M_SendStatusMessage(
                                        "received experience based parameter combinations");
                } else {
                        ROS_WARN(
                                        "Could not reach the experience based autoparameter chooser");
                }

                //worker 2
                /*std::map<std::string, std::map<std::string, double> > targetValue2;
                 targetValue2["Complete Face"]["graphmass"] = 0.4;

                 AreaParameterChooserWorker worker2(node, *currentImg, *currentMask, targetValue2);
                 worker2.CallculateParameters(2, steps, result);

                 if (result.size() < 4) {
                 ROS_ERROR("did not get enough results for the request (worker2)");
                 M_SendStatusMessage("did not get enough results for the request, please try to make another image");
                 delete currentImg;
                 delete currentMask;
                 return;
                 }*/

                //generate gui request
                std::string topic = "display_on_gui";
                ros::ServiceClient client = node.serviceClient<
                                face_contour_detector::ContourDetectorGUI>(topic);
                face_contour_detector::ContourDetectorGUI srv;
                srv.request.proposals = result;
                ROS_INFO("gui service request send");
                M_SendStatusMessage("opening gui...");
                if (client.call(srv)) {
                        ROS_INFO("gui respondet");

                        //ok lets send a anwser to glados
                        /*ros::ServiceClient resClient = node.serviceClient<portrait_robot_msgs::alubsc_node_instr>("/alubsc/edge_image");
                         portrait_robot_msgs::alubsc_node_instr resSrv;
                         resSrv.request.images.push_back(srv.response.result);
                         resSrv.request.abort = false;

                         if (resClient.call(resSrv)) {
                         ROS_INFO("Response send");
                         } else  {
                         ROS_INFO("could not reach response service");
                         }*/

                } else {
                        ROS_ERROR("Failed to call service %s", topic.c_str());
                }
                M_SendStatusMessage("done");

                //ImageMaskPair dummyP;
                //dummyP.image = currentImg;
                //dummyP.mask = currentMask;

                //StaticResponse(dummyP);

                //delete image data
                delete currentImg;
                delete currentMask;

                ROS_INFO("solved a request");
        }
}

bool FaceContourRequestHandler::HandleAlubscRequest(
                portrait_robot_msgs::alubsc_node_instr::Request &req,
                portrait_robot_msgs::alubsc_node_instr::Response &res) {
        if (req.abort) {
                M_SendStatusMessage("received abort");
        } else {
                std::vector<sensor_msgs::Image>::const_iterator it;
                ImageMaskPair p;
                p.image = 0;
                p.mask = 0;
                int i = 0;
                for (it = req.images.begin(); it != req.images.end(); it++) {
                        if (i > 1) {
                                ROS_ERROR("More than two images in the message");
                                res.success = false;
                                return false;
                        }
                        try {

                                sensor_msgs::CvBridge cvBridge;
                                if (!cvBridge.fromImage(*it, "bgr8"))
                                        throw sensor_msgs::CvBridgeException(
                                                        "Conversion to OpenCV image failed");
                                ;ROS_INFO(
                                                "got image with widht=%i and height=%i", it->width, it->height);
                                if (i == 0) {
                                        p.image = new cv::Mat(cvBridge.toIpl(), true);
                                } else if (i == 1) {
                                        p.mask = new cv::Mat(cvBridge.toIpl(), true);
                                }
                                //cv::imwrite("filter_services_req_in.png", inputImage);
                        } catch (sensor_msgs::CvBridgeException& error) {
                                ROS_ERROR(
                                                "could not convert the image with the cvBridge to a cv::Mat");
                                res.success = false;
                                return false;
                        }
                        i++;
                }
                if (p.image == 0) {
                        ROS_ERROR("no image received");
                        res.success = false;
                        return false;
                }
                if (p.mask == 0) {
                        ROS_ERROR("no mask received");
                        res.success = false;
                        return false;
                }
                requests.push(p);
                res.success = true;
        }
        return true;
}

void FaceContourRequestHandler::M_SendStatusMessage(
                const std::string& message) {
        ros::ServiceClient client = node.serviceClient<
                        portrait_robot_msgs::alubsc_status_msg>("status_msg");
        portrait_robot_msgs::alubsc_status_msg srv;
        srv.request.ID = 2;
        srv.request.message = message;
        if (!client.call(srv)) {
                ROS_WARN(
                                "could not reach status message service \"%s\"", client.getService().c_str());
                ROS_INFO("%s", message.c_str());
        }
}

void FaceContourRequestHandler::M_LoadExploreSettings(
                std::vector<WorkerSetting>& settings) {
        TiXmlDocument doc(exploreConfigPath.c_str());
        if (!doc.LoadFile()) {
                ROS_ERROR("Could not load the explore_settings xml config file");
                M_SendStatusMessage(
                                "Could not load the explore_settings xml config file");
                return;
        }
        TiXmlElement* root = doc.RootElement();
        for (TiXmlElement* worker = root->FirstChildElement("worker"); worker;
                        worker = worker->NextSiblingElement("worker")) {
                //check tag name
                WorkerSetting setting;
                if (worker->Attribute("steps")) {
                        setting.steps = atoi(worker->Attribute("steps"));
                } else {
                        setting.steps = 15;
                }
                if (worker->Attribute("numresults")) {
                        setting.numResults = atoi(worker->Attribute("numresults"));
                } else {
                        setting.numResults = 2;
                }
                for (TiXmlElement* area = worker->FirstChildElement("area"); area;
                                area = area->NextSiblingElement("area")) {
                        if (area->Attribute("name")) {
                                setting.targetValues[area->Attribute("name")] = std::map<
                                                std::string, double>();
                                for (TiXmlElement* targetValue = area->FirstChildElement(
                                                "target_value");
                                                targetValue;
                                                targetValue = targetValue->NextSiblingElement(
                                                                "target_value")) {
                                        if (targetValue->Attribute("name")
                                                        && targetValue->Attribute("value")) {
                                                setting.targetValues[area->Attribute("name")][targetValue->Attribute(
                                                                "name")] = atof(
                                                                targetValue->Attribute("value"));
                                        }
                                }
                        }
                }
                settings.push_back(setting);
        }
}

}