getLinesMain.cpp

Go to the documentation of this file.

#include <iostream>
#include <sstream>
#include <assert.h>
#include <algorithm>

#include <ros/ros.h>
#include <ros/package.h>
#include <tf/tf.h>
#include <image_transport/image_transport.h>
#include <opencv/cv.h>
#include <cv_bridge/CvBridge.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <sensor_msgs/Image.h>

#include <ee_cart_imped_action/ee_cart_imped_arm.hh>

#include <portrait_painter/getLines.h>
#include <portrait_painter/JobSubmission.h>
#include <portrait_painter/Point2D.h>
#include <portrait_robot_msgs/alubsc_node_instr.h>
#include <portrait_robot_msgs/alubsc_status_msg.h>
#include <portrait_painter/PainterUtils.h>

using namespace cv;
using namespace std;
using namespace portrait_painter;

int processImage(string filename);
void portrait_painter::sendStatus(string message, ros::ServiceClient client);
bool handleInstruction(portrait_robot_msgs::alubsc_node_instr::Request &req,
                portrait_robot_msgs::alubsc_node_instr::Response &res);

// If true a job has been scheduled for execution
bool jobScheduled;
string jobFilename;

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

        ros::init(argc, argv, "painter_lineExtractor");

        jobScheduled = false;

        ros::NodeHandle n;
        ros::ServiceServer InstructionServer = n.advertiseService(
                        "/alubsc/node_instr/painter", handleInstruction);
        while (ros::ok()) {
                if (jobScheduled) {
                        processImage(jobFilename);
                        jobScheduled = false;
                }
                ros::spinOnce();
        }

        // Test images.
        //processImage("bin/debugoutput.bmp");
        //processImage("bin/julian_handy_cannydebug_result.bmp");
}

/* @brief Callback for the Instruction Server, handles submission of new Images and Abort signals
 * @return true if everything went ok, false otherwise
 * */
bool handleInstruction(portrait_robot_msgs::alubsc_node_instr::Request &req,
                portrait_robot_msgs::alubsc_node_instr::Response &res) {
        string filename = ros::package::getPath("portrait_painter")
                        + "/bin/contours.bmp";
        // extract image
        try {
                sensor_msgs::CvBridge cvBridge;
                if (!cvBridge.fromImage(req.images[0], "mono8"))
                        throw sensor_msgs::CvBridgeException(
                                        "Conversion to OpenCV image failed");
                cv::Mat image = cv::Mat(cvBridge.toIpl(), true);
                cv::imwrite(filename, image);
        } catch (sensor_msgs::CvBridgeException &error) {
                ROS_ERROR("could not convert the image with the cvBridge to a cv::Mat");
                res.success = false;
                return false;
        }
        jobScheduled = true;
        jobFilename = filename;
        return true;
}

/* @brief Extracts Lines from Image and sends Draw commands to PainterMain
 * @param filename the filename of the saved Image file
 * */
int processImage(string filename) {
        Mat image = imread(filename, CV_LOAD_IMAGE_GRAYSCALE);
        // Adding a black bar so PR2 has enough place to sign.
        // The signing has size "percentualSizeOfImage" referring to the height of the
        // image.
        // And the bar is that high, so it holds (including an offset).
        float percentualSizeOfImage = 0.06;
        assert(percentualSizeOfImage > 0 && percentualSizeOfImage < 0.5);
        int newHeight = image.rows / (float) (1.0 - 1.5 * percentualSizeOfImage);
        Mat outputImage(newHeight, image.cols, 0);
        for (int x = 0; x < image.rows; x++)
                for (int y = 0; y < image.cols; y++)
                        outputImage.at<unsigned int>(x, y) = image.at<unsigned int>(x, y);
        // Change filename for test images, so it's not going to be overwritten.
        //filename = filename.substr(0, filename.length() - 4) + "_bar" + ".bmp";
        cv::imwrite(filename, outputImage);

        // Read the lines from the input, ass signing and draw the lines, to see everything is okay.
        GetLines gl;
        gl.getLines(filename);
        gl.drawPR2(percentualSizeOfImage);
        gl.drawLines();
        // Scale to size (1,1).
        vector<pair<CvPoint2D32f, vector<CvPoint2D32f> > > lines = gl.scaleTo(1,
                        false);

        ros::NodeHandle n;
        ros::ServiceClient client =
                        n.serviceClient<portrait_painter::JobSubmission>("job_submission");
        ros::ServiceClient msg_client = getMessageClient(n);

        // Create Job
        portrait_painter::JobSubmission job;

        job.request.height = outputImage.rows;
        job.request.width = outputImage.cols;

        // Iterate over all lines.
        for (size_t i = 0; i < lines.size(); i++) {
                job.request.points.clear();
                job.request.start.x = lines[i].first.x;
                job.request.start.y = lines[i].first.y;

                std::ostringstream oss;
                oss << "Drawing line no. " << i + 1 << ", " << lines.size() - i
                                << " to go";
                sendStatus(oss.str(), msg_client);
                job.request.last = (i == lines.size() - 1) ? true : false;

                // Iterate over all points in the line.
                for (size_t j = 0; j < lines[i].second.size(); j++) {
                        portrait_painter::Point2D p;
                        p.x = lines[i].second[j].x;
                        p.y = lines[i].second[j].y;
                        job.request.points.push_back(p);
                }
                if (client.call(job)) {
                        if (!job.response.success) {
                                // If the job submission was not successful, abort whole image.
                                sendStatus(
                                                "PainterMain is not ready! Canvas not set?\nAborting processing of image",
                                                msg_client);
                                break;
                        }
                } else {
                        ROS_ERROR("Failed to call service");
                        return 1;
                }
        }
        sendStatus("I am done drawing the picture! How do you like it?",
                        msg_client);
        // Move the Arm back to starting position.
        EECartImpedArm arm("r_arm_cart_imped_controller");

        // Trajectory
        ee_cart_imped_msgs::EECartImpedGoal traj;
        tf::Quaternion o = tf::createQuaternionFromRPY(0, 0, 0);
        EECartImpedArm::addTrajectoryPoint(traj, 0.5, -0.5, 0.5, o.x(), o.y(),
                        o.z(), o.w(), 20, 20, 20, 30, 30, 30, false, false, false, false,
                        false, false, 2, "/torso_lift_link");
        arm.startTrajectory(traj);
        return 0;
}