PainterUtils.cpp

Go to the documentation of this file.

#include <string>
#include <ros/ros.h>
#include <portrait_painter/PainterUtils.h>
#include <portrait_robot_msgs/alubsc_status_msg.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <ee_cart_imped_action/ee_cart_imped_arm.hh>
#include <ee_cart_imped_msgs/EECartImpedGoal.h>

using namespace std;
using namespace portrait_painter;

void portrait_painter::sendStatus(string message, ros::ServiceClient client) {
        if (client.exists()) {
                portrait_robot_msgs::alubsc_status_msg status;
                status.request.ID = NODE_ID;
                status.request.message = message;
                client.call(status);
        }
}

ros::ServiceClient portrait_painter::getMessageClient(ros::NodeHandle n) {
        return n.serviceClient<portrait_robot_msgs::alubsc_status_msg>(
                        "/alubsc/status_msg");
}

// returns the right Gripper position in /torso_lift_link
tf::Vector3 portrait_painter::getGripperPosition() {
        bool success = false;

        tf::Vector3 res(0, 0, 0);
        tf::TransformListener listener;

        tf::StampedTransform transform;
        while (!success) {
                try {
                        listener.lookupTransform("/torso_lift_link",
                                        "/r_gripper_tool_frame", ros::Time(0), transform);
                        res[0] = transform.getOrigin().x();
                        res[1] = transform.getOrigin().y();
                        res[2] = transform.getOrigin().z();
                        success = true;
                } catch (tf::TransformException& ex) {

                }
        }
        return res;
}

void portrait_painter::moveGripperToInitialPos() {
        // Action Client
        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(), 0, 0, 0, 30, 30, 30, false, false, false, false,
                        false, false, 2, "/torso_lift_link");
        arm.startTrajectory(traj);
}