RosServices.cpp

Go to the documentation of this file.

#include <face_contour_detector/filters/RosServices.h>

#include <ros/ros.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>

//filters
#include <face_contour_detector/filters/Filter.h>
#include <face_contour_detector/filters/List.h>

//std
#include <string>
#include <vector>
#include <map>

//for atoi and atof
#include <stdio.h>
#include <stdlib.h>

//forest
#include <face_contour_detector/ImageForest.h>

namespace face_contour_detector {

namespace filters {
    
    RosServices::RosServices() {
        
    }
    RosServices::~RosServices() {
        std::map<std::string, Filter*>::iterator it;
        for (it = m_filters.begin(); it != m_filters.end(); it++) {
            delete it->second;
        }
    }
    void RosServices::AddFilter(std::string name, Filter* filter) {
        m_filters[name] = filter;
    }
    bool RosServices::HandleApplyRequest(face_contour_detector::ApplyFilters::Request& request, face_contour_detector::ApplyFilters::Response& response) {
        
        response.success = true;
        cv::Mat inputImage;
        //extract image
        try {
            
            sensor_msgs::CvBridge cvBridge;
            if (!cvBridge.fromImage(request.input_image, "mono8")) {
                throw sensor_msgs::CvBridgeException("Conversion to OpenCV image failed");
            }
            ROS_INFO("Image widht=%i height=%i", request.input_image.width, request.input_image.height);
            //inputImage.create(request.input_image.height, request.input_image.width, CV_8UC1);
            inputImage = 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");
            response.success = false;
            return false;
        }
        
        int rows = inputImage.rows;
        int cols = inputImage.cols;
        
        cv::Mat resultImage(rows, cols, CV_8UC1);
        
        //cv::imwrite("filter_services_req_in_aftercreateresult.png", inputImage);
        
        //Don't use copyTo BECAUSE IT IS BUGGED LIKE HELL (ok maybe it is not)
        for (int y = 0; y < rows; y++) {
            for (int x = 0; x < cols; x++) {
                ROS_ASSERT(resultImage.cols > x && resultImage.rows > y);
                //resultImage.at<unsigned char>(y,x) = inputImage.at<unsigned char>(y,x);
                resultImage.at<unsigned char>(y,x) = 0;
            }
        }
        //cv::imwrite("filter_services_req_in_aftercopy.png", inputImage);
        
        //iteratore through the areas
        //ROS_INFO("Starting to iterate through the areas");
        int areaI = 0;
        std::vector<face_contour_detector::filter_area_setup>::iterator areaIt;
        for (areaIt = request.areas.begin(); areaIt != request.areas.end(); areaIt++) {
            
                //ROS_INFO("at area %i name=%s x=%i y=%i width=%i height=%i", areaI, areaIt->area.name.c_str(), areaIt->area.x, areaIt->area.y, areaIt->area.width, areaIt->area.height);

            //check if the area is valid
            if (areaIt->area.x < 0 || areaIt->area.x >= inputImage.cols ||
                areaIt->area.y < 0 || areaIt->area.y >= inputImage.rows ||
                areaIt->area.width <= 0 || areaIt->area.height <= 0 ||
                areaIt->area.x + areaIt->area.width > inputImage.cols ||
                areaIt->area.y + areaIt->area.height > inputImage.rows
                ) {
                ROS_ERROR("The received area (name=%s, x=%i y=%i width=%i height=%i ) is not valid", areaIt->area.name.c_str(), areaIt->area.x, areaIt->area.y, areaIt->area.width, areaIt->area.height);
                response.success = false;
                return false;
            }
            
            //generate area response message
            face_contour_detector::filter_area_result areaResponse;
            areaResponse.area = areaIt->area;
            
            //copy out the area
            //ROS_INFO("copying out area");
            cv::Mat areaImage(areaIt->area.height, areaIt->area.width, CV_8UC1); 
            
            for (int y = 0; y < areaIt->area.height; y++) {
                for (int x = 0; x < areaIt->area.width; x++) {
                        ROS_ASSERT(inputImage.cols > x);
                        ROS_ASSERT(inputImage.rows > y);
                    areaImage.at<unsigned char>(y,x) = inputImage.at<unsigned char>(areaIt->area.y + y, areaIt->area.x + x); 
                }
            }
            
            //run the filters
            int filterI = 0;
            //ROS_INFO("copying out area to result cv::Mat");
            cv::Mat areaImageResult = areaImage.clone();

            std::vector<face_contour_detector::filter_setup>::iterator filterIt;
            for (filterIt = areaIt->filters.begin(); filterIt != areaIt->filters.end(); filterIt++) {
                Filter* currFilter = 0;
                //ROS_INFO("starting filter setup");
                if (M_SetupFilter(*filterIt, currFilter)) {
                        //ROS_INFO("applying filter");
                    currFilter->Apply(areaImage, areaImageResult);
                    //ROS_INFO("copy result");
                    //DONT USE COPY TO IT IS FUCKING BUGGED
                    //areaImageResult.copyTo(areaImage);
                    areaImage = areaImageResult.clone();
                    /*for (int y = 0; y < areaImageResult.rows; y++) {
                        for (int x = 0; x < areaImageResult.cols; x++) {
                            ROS_ASSERT(y < areaImage.rows);
                            ROS_ASSERT(x < areaImage.cols);
                            //ROS_INFO("Reading");
                            unsigned char data = areaImageResult.at<unsigned char>(y, x);
                            //ROS_INFO("Writing");
                            areaImage.at<unsigned char>(y,x) =  data;
                        }
                    }*/
                    
                    //std::stringstream s;
                    //s<<"ros_filter_service_debug_a"<<areaI<<"_f"<<filterI<<".png";
                    //cv::imwrite(s.str().c_str(), areaImage);
                } else  {
                    response.success = false;
                    return false;
                }
                filterI++;
            }
            
            //overwrite following areas
            {
                std::vector<face_contour_detector::filter_area_setup>::iterator areaIt2;
                bool following = false;
                for (areaIt2 = request.areas.begin(); areaIt2 != request.areas.end(); areaIt2++) {
                    if (following) {
                        for (int y = 0; y < areaIt2->area.height; y++) {
                            for (int x = 0; x < areaIt2->area.width; x++) {
                                int realX = areaIt2->area.x + x - areaIt->area.x;
                                int realY = areaIt2->area.y + y - areaIt->area.y;
                                if (realX >= 0 && realX < areaIt->area.width &&
                                                realY >= 0 && realY < areaIt->area.height) {
                                        ROS_ASSERT(areaImageResult.rows > realY && areaImageResult.cols > realX);
                                        areaImageResult.at<unsigned char>(realY,realX) = (unsigned char)0;
                                }
                            }
                        }
                    } else if (areaIt2 == areaIt) {
                        following = true;
                    }
                }
            }
            
            //read out the result properties for this area and push it onto the stack
            M_ExtractProperties(areaImageResult, areaResponse.result_properties);
            response.area_results.push_back(areaResponse);
            
            //paste the result into the bigger resultpicture
            for (int y = 0; y < areaIt->area.height; y++) {
                for (int x = 0; x < areaIt->area.width; x++) {
                        ROS_ASSERT(areaIt->area.y + y >= 0 && areaIt->area.y + y < resultImage.rows);
                        ROS_ASSERT(areaIt->area.x + x >= 0 && areaIt->area.x + x < resultImage.cols);
                    resultImage.at<unsigned char>(areaIt->area.y + y, areaIt->area.x + x) = (unsigned char)areaImageResult.at<unsigned char>(y, x); 
                }
            }
            
            areaI++;
        }
        
        //Extract the properties for the hole image
        M_ExtractProperties(resultImage, response.result_properties);
        
        //Putting the result image into the response
        try {
            
            sensor_msgs::CvBridge cvBridge;
            //cv::imwrite("filter_services_req_out.bmp", resultImage);
            IplImage ipl = resultImage;
            response.result_image = *(cvBridge.cvToImgMsg(&ipl));
        } catch (sensor_msgs::CvBridgeException error) {
            ROS_ERROR("could not convert the image with the cvBridge to a sensor_msgs/Image");
            response.success = false;
            return false;
        }
        return true;
    }
    bool RosServices::HandleGetRequest(face_contour_detector::GetFilters::Request& request, face_contour_detector::GetFilters::Response& response) {
        
        std::map<std::string, Filter*>::iterator it;
        for (it = m_filters.begin(); it != m_filters.end(); it++) {
            face_contour_detector::filter f;
            f.name = it->first;
            std::vector<Parameter> params = it->second->GetParameters();
            std::vector<Parameter>::iterator paramIt;
            for (paramIt = params.begin(); paramIt != params.end(); paramIt++) {
                face_contour_detector::parameter p;
                p.name = paramIt->GetName();
                if (paramIt->GetType() == face_contour_detector::filters::TBOOL) {
                    p.type = "bool";
                } else if (paramIt->GetType() == face_contour_detector::filters::TINT) {
                    p.type = "int";
                } else if (paramIt->GetType() == face_contour_detector::filters::TDOUBLE) {
                    p.type = "float";
                } else if (paramIt->GetType() == face_contour_detector::filters::TSTRING) {
                    p.type = "string";
                }
                p.ruler_minimum = paramIt->GetRulerMinimum();
                p.ruler_maximum = paramIt->GetRulerMaximum();
                f.parameters.push_back(p);
            }
            response.filters.push_back(f);
        }
        return true;
    }
    
    void RosServices::M_ExtractProperties(cv::Mat& image, std::vector<face_contour_detector::name_value_pair>& result) {
        
        ROS_ASSERT(image.channels() == 1);
        
        //creating forest out of image
        face_contour_detector::ImageForest forest(image, 200);
        std::map<int, face_contour_detector::ImageForest::Graph> graphs = forest.GetGraphs();
        
        //num of graphs
        {
            face_contour_detector::name_value_pair entry;
            std::stringstream valuestream;
            entry.type = "int";
            entry.name = "numGraphs";
            valuestream<<graphs.size();
            entry.value = valuestream.str().c_str();
            result.push_back(entry);
        }
        
        //num of connectingpoints
        {
            face_contour_detector::name_value_pair entry;
            std::stringstream valuestream;
            int count = 0;
            std::map<int, face_contour_detector::ImageForest::Graph>::iterator it;
            for (it = graphs.begin(); it != graphs.end(); it++) {
                count += it->second.connectingPoints.size();
            }
            entry.type = "int";
            entry.name = "numGraphConnectingPoints";
            valuestream<<count;
            entry.value = valuestream.str().c_str();
            result.push_back(entry);
        }
        
        //num of endpoints
        {
            face_contour_detector::name_value_pair entry;
            std::stringstream valuestream;
            int count = 0;
            std::map<int, face_contour_detector::ImageForest::Graph>::iterator it;
            for (it = graphs.begin(); it != graphs.end(); it++) {
                count += it->second.endPoints.size();
            }
            entry.type = "int";
            entry.name = "numGraphEndpoints";
            valuestream<<count;
            entry.value = valuestream.str().c_str();
            result.push_back(entry);
        }
        //edgyness
        /*{
            face_contour_detector::name_value_pair entry;
            std::stringstream valuestream;
            entry.type = "float";
            entry.name = "graphEdgyness";
            valuestream<<forest.CallculateEdgyness();
            entry.value = valuestream.str().c_str();
            result.push_back(entry);
        }*/
        
        //black to whitepixel ratio (num blackpixels/num whitepixels)
        {
            face_contour_detector::name_value_pair entry;
            std::stringstream valuestream;
            entry.type = "float";
            entry.name = "graphmass";
            double graphMass = forest.CallculateGraphMass();
            valuestream<<graphMass;
            entry.value = valuestream.str().c_str();
            result.push_back(entry);
        }
    }
    
    bool RosServices::M_SetupFilter(face_contour_detector::filter_setup& filterSetup, Filter*& filter) {
        //get the filter
        if (m_filters.find(filterSetup.name) == m_filters.end()) {
            ROS_ERROR("Could not find a filter with the name \"%s\"", filterSetup.name.c_str());
            return false;
        }
        filter = m_filters[filterSetup.name];
        
        //setup the parameters
        filter->ResetParameters();
        std::vector<Parameter> params = filter->GetParameters();
        std::vector<Parameter>::iterator it;
        for (it = params.begin(); it != params.end(); it++) {
                std::vector<face_contour_detector::name_value_pair>::iterator found = filterSetup.parameters.end();
            std::vector<face_contour_detector::name_value_pair>::iterator nvIt;
            for (nvIt = filterSetup.parameters.begin(); nvIt != filterSetup.parameters.end(); nvIt++) {
                if (nvIt->name == it->GetName()) {
                    found = nvIt;
                    break;
                }
            }
            if (found != filterSetup.parameters.end()) {
                if (it->GetType() == face_contour_detector::filters::TBOOL) {
                    *(it->GetBoolValuePtr()) = (found->value == std::string("1") || found->value == std::string("true"));
                } else if (it->GetType() == face_contour_detector::filters::TINT) {
                    *(it->GetIntValuePtr()) = atoi(found->value.c_str());
                } else if (it->GetType() == face_contour_detector::filters::TDOUBLE) {
                    *(it->GetDoubleValuePtr()) = atof(found->value.c_str());
                } else if (it->GetType() == face_contour_detector::filters::TSTRING) {
                    *(it->GetStringValuePtr()) = found->value;
                }
            } else  {
                ROS_WARN("Could not find the parameter \"%s\" for the filter \"%s\"", it->GetName().c_str(), filterSetup.name.c_str());
            }
        }
        return true;
    }
    
} //namespace filters

} //namespace face_contour_detector