PainterMain.cpp

Go to the documentation of this file.

#include <vector>

#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>

#include <ee_cart_imped_action/ee_cart_imped_arm.hh>

#include <portrait_painter/SetCanvas.h>
#include <portrait_painter/PainterUtils.h>
#include <portrait_painter/JobSubmission.h>
#include <portrait_painter/Point2D.h>

#define MAXSPEED 0.2

// distance to the canvas the robot has to have while not drawing
#define CANVAS_OFFSET 0.1

using namespace portrait_painter;

double SPEED;
// The listener to read the gripper position

// Method declaration
bool validateJob();
void runJob();

// The Canvas position
tf::Point leftUpper;
tf::Point rightUpper;
tf::Point leftLower;

// True if the canvas was set
bool canvasSet = false;

// Job Data:
// True if there is a job currently in process
bool jobActive = false;

// List of Points to draw
std::vector<portrait_painter::Point2D> jobPoints;
portrait_painter::Point2D jobStart;

// The unit vector that points from leftUpper to rightUpper
tf::Point cornerRight;
// The unit vector that points from leftUpper to leftLower
tf::Point cornerDown;

// The arm Controller publisher
ros::Publisher armController;

// Sets the Canvas to a certain set of coordinates
bool handleCanvasSetting(portrait_painter::SetCanvas::Request &req,
                portrait_painter::SetCanvas::Response &res) {

        tf::pointMsgToTF(req.leftUpper, leftUpper);
        tf::pointMsgToTF(req.rightUpper, rightUpper);
        tf::pointMsgToTF(req.leftLower, leftLower);

        cornerRight = (rightUpper - leftUpper);
        //cornerRight.normalize();

        cornerDown = leftLower - leftUpper;
        //cornerDown.normalize();

        canvasSet = true;

        std::ostringstream stream;
        ros::NodeHandle n;
        ros::ServiceClient msg_client = getMessageClient(n);

        stream << setprecision(2) << "New Canvas Position: \n(" << leftUpper.x()
                        << ", " << leftUpper.y() << ", " << leftUpper.z() << ") \n("
                        << rightUpper.x() << ", " << rightUpper.y() << ", "
                        << rightUpper.z() << ") \n(" << leftLower.x() << ", "
                        << leftLower.y() << ", " << leftLower.z() << ") \n";
        sendStatus(stream.str(), msg_client);

        ROS_INFO(
                        "New Corner Right: (%f, %f, %f)", cornerRight.x(), cornerRight.y(), cornerRight.z());
        ROS_INFO(
                        "New Corner Down: (%f, %f, %f)", cornerDown.x(), cornerDown.y(), cornerDown.z());
        return true;
}

// Recieves and Handles a new Job submission
bool handleJobSumission(portrait_painter::JobSubmission::Request &req,
                portrait_painter::JobSubmission::Response &res) {
        // If the canvas was not set properly, the Painter will refuse to accept Jobs
        if (!canvasSet || jobActive) {
                res.success = false;
                return true;
        }

        int pointNumber = req.points.size();
        portrait_painter::Point2D start = req.start;

        ROS_INFO(
                        "Recieved Job! %d Points\n Startpoint: (%f,%f)", pointNumber, start.x, start.y);

        // Save the submitted list to the global list
        jobPoints = req.points;
        jobStart = req.start;

        if (!validateJob()) {
                res.success = false;
                return true;
        }
        res.success = true;
        jobActive = true;

        // Start Job
        runJob();

        // Job done
        printf("Job Done!\n");
        jobActive = false;

        return true;
}

// This method handles the whole painting process of a job
void runJob() {

        ROS_INFO("Job Started");

        // Fit Image to canvas
        // Action Client
        EECartImpedArm arm("r_arm_cart_imped_controller");

        // Trajectory
        ee_cart_imped_msgs::EECartImpedGoal traj;

        portrait_painter::Point2D canvasPosition;
        canvasPosition.x = jobStart.x;
        canvasPosition.y = jobStart.y;

        // Move Gripper to start position
        double timestamp = 0.0;
        double speed = SPEED;

        tf::Point nextGripperPosition = leftUpper
                        + (cornerRight * canvasPosition.x);
        nextGripperPosition = nextGripperPosition + (cornerDown * canvasPosition.y);
        nextGripperPosition[2] += CANVAS_OFFSET;
        ROS_INFO(
                        "Job start: (%f, %f, %f)", nextGripperPosition.x(), nextGripperPosition.y(), nextGripperPosition.z());

        tf::Quaternion o = tf::createQuaternionFromRPY(0, 0, 0);

        int stiffness = 50;

        tf::Point previousPosition = getGripperPosition();
        double length = (nextGripperPosition - previousPosition).length();
        timestamp += length / speed;

        EECartImpedArm::addTrajectoryPoint(traj, nextGripperPosition.x(),
                        nextGripperPosition.y(), nextGripperPosition.z(), o.x(), o.y(),
                        o.z(), o.w(), 1000, 1000, 1000, 30, 30, 30, false, false, false,
                        false, false, false, timestamp, "/torso_lift_link");
        timestamp += CANVAS_OFFSET * 3 / speed;

        nextGripperPosition[2] -= 3 * CANVAS_OFFSET;

        ROS_INFO(
                        "2nd Point: (%f, %f, %f)", nextGripperPosition.x(), nextGripperPosition.y(), nextGripperPosition.z());

        EECartImpedArm::addTrajectoryPoint(traj, nextGripperPosition.x(),
                        nextGripperPosition.y(), nextGripperPosition.z(), o.x(), o.y(),
                        o.z(), o.w(), 1000, 1000, stiffness, 30, 30, 30, false, false,
                        false, false, false, false, timestamp, "/torso_lift_link");

        for (unsigned int i = 0; i < jobPoints.size(); i++) {
                // Remember last Point for distance calculation
                tf::Point lastGripperPosition = nextGripperPosition;

                // Calculate the next point on the canvas (relative to absolute)
                canvasPosition.x += jobPoints[i].x;
                canvasPosition.y += jobPoints[i].y;

                nextGripperPosition = leftUpper + (cornerRight * canvasPosition.x);
                nextGripperPosition = nextGripperPosition
                                + (cornerDown * canvasPosition.y);
                nextGripperPosition[2] -= 2 * CANVAS_OFFSET;

                ROS_INFO(
                                "Next point: (%f, %f, %f)", nextGripperPosition.x(), nextGripperPosition.y(), nextGripperPosition.z());

                double length = (nextGripperPosition - lastGripperPosition).length();
                timestamp += length / speed;

                // Add next point to the trajectory
                EECartImpedArm::addTrajectoryPoint(traj, nextGripperPosition.x(),
                                nextGripperPosition.y(), nextGripperPosition.z(), o.x(), o.y(),
                                o.z(), o.w(), 1000, 1000, stiffness, 30, 30, 30, false, false,
                                false, false, false, false, timestamp, "/torso_lift_link");

                printf("timestamp: %f", timestamp);

        }

        arm.startTrajectory(traj);
}

// This method checks if a submitted job would cause the pen to leave the canvas
// return: true if job is valid; false otherwise
bool validateJob() {
        portrait_painter::Point2D currentPosition = jobStart;

        int pointNum = jobPoints.size();

        for (int i = 0; i < pointNum; i++) {
                currentPosition.x += jobPoints[i].x;
                currentPosition.y += jobPoints[i].y;

                if (currentPosition.x > 1.01 || currentPosition.x < -0.01
                                || currentPosition.y > 1.01 || currentPosition.y < -0.01) {
                        ROS_INFO(
                                        "Invalid Job! Leaves canvas in step %d (%f, %f)", i, currentPosition.x, currentPosition.y);
                        return false;
                }
        }
        return true;
}

int main(int argc, char **argv) {
        // Initialize Canvas

        leftUpper.setZero();
        rightUpper.setZero();
        leftLower.setZero();
        cornerRight.setZero();
        cornerDown.setZero();

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

        if (argc != 2) {
                printf("Please specify desired speed in meters/second\n");
                return (1);
        }
        SPEED = atof(argv[1]);
        if (SPEED > MAXSPEED) {
                printf("Warning: Speed to high! Was set to %1.1f for safety reasons!\n",
                                MAXSPEED);
                SPEED = MAXSPEED;
        }

        // Initialize Canvas setting service
        ros::ServiceServer canvasSetting = n.advertiseService("canvas_setting",
                        handleCanvasSetting);
        // Initialize Job Submission service
        ros::ServiceServer jobSubmission = n.advertiseService("job_submission",
                        handleJobSumission);

        ROS_INFO("Painter Services up and running");

        ros::spin();

        return 0;
}