SetCanvasManually.cpp

Go to the documentation of this file.

#include <ros/ros.h>
#include <iostream>
#include <sstream>
#include <image_transport/image_transport.h>
#include <ee_cart_imped_action/ee_cart_imped_arm.hh>
#include <portrait_painter/SetCanvas.h>
#include <portrait_robot_msgs/alubsc_node_instr.h>
#include <portrait_robot_msgs/alubsc_status_msg.h>
#include <portrait_painter/PainterUtils.h>
#include <tf/tf.h>

#define CANVAS_OFFSET 0.2

using namespace portrait_painter;

int callCounter;

tf::Point leftUpper;
tf::Point rightUpper;
tf::Point leftLower;

bool handleInstruction(portrait_robot_msgs::alubsc_node_instr::Request &req,
                portrait_robot_msgs::alubsc_node_instr::Response &res);

int main(int argc, char **argv) {
        ros::init(argc, argv, "PainterCanvasSetter");
        ros::NodeHandle n;

        callCounter = 0;
        // Initialize Canvas setting service

        ros::ServiceServer instructionServer = n.advertiseService(
                        "alubsc/node_instr/manual_canvas_setting", handleInstruction);
        ros::spin();

        return 0;
}

bool handleInstruction(portrait_robot_msgs::alubsc_node_instr::Request &req,
                portrait_robot_msgs::alubsc_node_instr::Response &res) {
        ros::NodeHandle n;
        ros::ServiceClient msg_client = getMessageClient(n);
        ostringstream oss;

        if (req.abort) {
                callCounter = 0;
                return true;
        }

        switch (callCounter) {
        case 0: {
                sendStatus(
                                "Please move right Gripper to left upper corner of the canvas and click button!",
                                msg_client);

                moveGripperToInitialPos();
                callCounter++;
                break;
        }
        case 1:

                leftUpper = getGripperPosition();

                oss << setprecision(2) << "Left upper " << leftUpper.x() << ", "
                                << leftUpper.y() << ", " << leftUpper.z();
                sendStatus(oss.str(), msg_client);
                sendStatus(
                                "Please move right Gripper to right upper corner of the canvas and click button!",
                                msg_client);
                callCounter++;
                break;

        case 2:
                rightUpper = getGripperPosition();

                oss << setprecision(2) << "Right upper " << rightUpper.x() << ", "
                                << rightUpper.y() << ", " << rightUpper.z();
                sendStatus(oss.str(), msg_client);
                sendStatus(
                                "Please move right Gripper to left lower corner of the canvas and click button!",
                                msg_client);
                callCounter++;
                break;
        case 3: {
                leftLower = getGripperPosition();

                oss << setprecision(2) << "Left Lower " << leftLower.x() << ", "
                                << leftLower.y() << ", " << leftLower.z();
                sendStatus(oss.str(), msg_client);
                callCounter++;
                ros::ServiceClient client =
                                n.serviceClient<portrait_painter::SetCanvas>("canvas_setting");
                portrait_painter::SetCanvas req;

                //Assure aspect ratio of 342x442
                float width = (leftUpper - rightUpper).length();
                float height = (leftUpper - leftLower).length();
                //shorten the vector that is too long
                float desired_ratio = 342.0 / 442.0;
                if (width / height > desired_ratio) { //shorten width
                        tf::Point unit_right = (rightUpper - leftUpper);
                        unit_right.normalize();
                        rightUpper = leftUpper + unit_right * (height * desired_ratio);
                } else if (width / height < desired_ratio) { //shorten height
                        tf::Point unit_down = (leftLower - leftUpper);
                        unit_down.normalize();
                        leftLower = leftUpper + unit_down * (width / desired_ratio);
                }

                tf::pointTFToMsg(leftUpper, req.request.leftUpper);
                tf::pointTFToMsg(rightUpper, req.request.rightUpper);
                tf::pointTFToMsg(leftLower, req.request.leftLower);

                client.call(req);

                callCounter = 0;
                break;
        }
        default:
                break;
        }
        return true;
}