SendAutoselectRequest.cpp

Go to the documentation of this file.

#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 <portrait_robot_msgs/alubsc_node_instr.h>
#include <portrait_robot_msgs/alubsc_status_msg.h>

//std
#include <vector>

bool gotAnwser;

bool MessageResultRequest(portrait_robot_msgs::alubsc_status_msg::Request& req,
                portrait_robot_msgs::alubsc_status_msg::Response& res) {
        if (req.ID == 2) {
                ROS_INFO("Status: [%s]", req.message.c_str());
        }
        return true;
}

bool HandleResultReqest(portrait_robot_msgs::alubsc_node_instr::Request& req,
                portrait_robot_msgs::alubsc_node_instr::Response& res) {

        gotAnwser = true;
        res.success = true;

        try {

                if (req.images.size() != 1) {
                        ROS_ERROR(
                                        "The response request has %i images, but it should have only 1", int(req.images.size()));
                        res.success = false;
                        return false;
                }

                sensor_msgs::CvBridge cvBridge;
                if (!cvBridge.fromImage(req.images[0])) {
                        throw sensor_msgs::CvBridgeException(
                                        "Conversion to OpenCV image failed");
                }ROS_INFO(
                                "got image with widht=%i and height=%i", req.images[0].width, req.images[0].height);
                cv::Mat img = cv::Mat(cvBridge.toIpl(), true);
                cv::imwrite("send_autoselect_reques_result.png", img);
                ROS_INFO("saved response to send_autoselect_reques_result.png");
        } catch (sensor_msgs::CvBridgeException& error) {
                ROS_ERROR("could not convert the image with the cvBridge to a cv::Mat");
                res.success = false;
                return false;
        }

        return true;
}

int main(int argc, char* argv[]) {

        gotAnwser = false;

        ros::init(argc, argv, "send_autoselect_request");
        ros::NodeHandle n;

        if (argc < 3) {
                ROS_ERROR("there must be an image to transfer");

        }

        std::string topic = "/contour_detector/contour_detector";
        if (argc >= 4) {
                topic = argv[3];
        }

        //setting up the service for the anwser
        ros::ServiceServer resultService = n.advertiseService<>(
                        "/alubsc/edge_image", HandleResultReqest);

        //provide the service for the status messages
        ros::ServiceServer messageService = n.advertiseService<>(
                        "/alubsc/status_msg", MessageResultRequest);

        portrait_robot_msgs::alubsc_node_instr srv;
        srv.request.abort = false;

        //load image
        cv::Mat img = cv::imread(argv[1]);
        cv::Mat mask = cv::imread(argv[2]);

        try {
                sensor_msgs::CvBridge cvBridge;
                IplImage ipl = img;
                srv.request.images.push_back(*(cvBridge.cvToImgMsg(&ipl)));
        } catch (sensor_msgs::CvBridgeException& error) {
                ROS_ERROR(
                                "could not convert the image with the cvBridge to a sensor_msgs/Image");
                return 1;
        }

        try {
                sensor_msgs::CvBridge cvBridge;
                IplImage ipl = mask;
                srv.request.images.push_back(*(cvBridge.cvToImgMsg(&ipl)));
        } catch (sensor_msgs::CvBridgeException& error) {
                ROS_ERROR(
                                "could not convert the mask with the cvBridge to a sensor_msgs/Image");
                return 1;
        }

        ros::ServiceClient client = n.serviceClient<
                        portrait_robot_msgs::alubsc_node_instr>(topic);
        ROS_INFO("Service request send");
        if (client.call(srv)) {
                ROS_INFO("got response!");
        } else {
                ROS_ERROR("Failed to call service %s", topic.c_str());
                return 1;
        }

        ROS_INFO("Waiting for the answer service request...");
        //setting up the service
        ros::Rate loopRate(10);
        while (ros::ok() && !gotAnwser) {
                ros::spinOnce();
                loopRate.sleep();
        }
        if (gotAnwser) {
                ROS_INFO("Got response");
        }

        return 0;

}