pen_gripper.cpp

Go to the documentation of this file.

/*
 * pen_gripper.cpp
 *
 *  Created on: 15.12.2010
 *      Author: Richard Schneider
 */
#include <pen_gripper/pen_gripper.h>
#include <tf/transform_datatypes.h>
#define PI 3.1415926536
#include <string>
using namespace std;

bool point_sort_x(pcl::PointXYZ a, pcl::PointXYZ b) {
        return a.x > b.x;
}
bool point_sort_y(pcl::PointXYZ a, pcl::PointXYZ b) {
        return a.y > b.y;
}
bool point_sort_z(pcl::PointXYZ a, pcl::PointXYZ b) {
        return a.z > b.z;
}

PenGripper::~PenGripper() {
        delete head;

        delete arm_l;
        delete arm_r;

        delete GripperRight;
        delete GripperLeft;
}

PenGripper::PenGripper(ros::NodeHandle& nh) {

        n = &nh;

        // head
        head = new actionlib::SimpleActionClient<
                        pr2_controllers_msgs::PointHeadAction>(
                        "/head_traj_controller/point_head_action", true);

        // arms
        arm_r = new EECartImpedArm("r_arm_cart_imped_controller");
        arm_l = new EECartImpedArm("l_arm_cart_imped_controller");

        // right gripper start position
        r_start_pos.x = 0.0;
        r_start_pos.y = -0.5;
        r_start_pos.z = 0.3;

        // left gripper start position
        l_start_pos.x = 0.0;
        l_start_pos.y = 0.5;
        l_start_pos.z = 0.3;

        // gripper action client
        GripperRight = new actionlib::SimpleActionClient<
                        pr2_controllers_msgs::Pr2GripperCommandAction>(
                        "r_gripper_controller/gripper_action", true);
        GripperLeft = new actionlib::SimpleActionClient<
                        pr2_controllers_msgs::Pr2GripperCommandAction>(
                        "l_gripper_controller/gripper_action", true);

        //wait for the gripper action server to come up
        while (!GripperRight->waitForServer(ros::Duration(5.0))) {
                ROS_INFO("Waiting for the right gripper action server to come up");
        }

        //wait for the gripper action server to come up
        while (!GripperRight->waitForServer(ros::Duration(5.0))) {
                ROS_INFO("Waiting for the left gripper action server to come up");
        }

        // status client publishing at /alubsc/status_msg
        status_clt = n->serviceClient < portrait_robot_msgs::alubsc_status_msg
                        > ("/alubsc/status_msg");
        // narrow stereo textured point cloud
        pcl_sub = n->subscribe("/narrow_stereo_textured/points2", 1,
                        &PenGripper::narrow_stereo_cb, this);
        // filtered cloud
        pcl_pub = n->advertise < sensor_msgs::PointCloud2
                        > ("/filtered_pointcloud", 1);

        // pcl marker publisher
        marker_middle_pub = n->advertise < visualization_msgs::Marker
                        > ("visualization_middle", 1);

        // 1 for pen gripper
        status.request.ID = 1;

        // pen status
        red_pen.isReady = false;

        // success of execution
        success = false;
        // client request
        abort = true;
}

void PenGripper::open_gripper(const string gripper) {

        if (gripper == "right") {
                ROS_INFO("open the right gripper");
                status.request.message = "open the right gripper";
                status_clt.call(status);

                pr2_controllers_msgs::Pr2GripperCommandGoal open;
                open.command.position = 0.08;
                open.command.max_effort = -1.0; // Do not limit effort (negative)

                // open the gripper and wait for the result
                GripperRight->sendGoal(open);
                GripperRight->waitForResult();

                if (GripperRight->getState()
                                == actionlib::SimpleClientGoalState::SUCCEEDED) {
                        ROS_INFO("The right gripper is open");
                        status.request.message = "The right gripper is open.";
                        status_clt.call(status);
                } else {
                        ROS_INFO("The right gripper failed to open.");
                        status.request.message = "The right gripper failed to open.";
                        status_clt.call(status);
                }
        }

        if (gripper == "left") {
                ROS_INFO("open the left gripper");
                status.request.message = "open the left gripper";
                status_clt.call(status);

                pr2_controllers_msgs::Pr2GripperCommandGoal open;
                open.command.position = 0.08;
                open.command.max_effort = -1.0; // Do not limit effort (negative)

                // open the gripper and wait for the result
                GripperLeft->sendGoal(open);
                GripperLeft->waitForResult();

                if (GripperLeft->getState()
                                == actionlib::SimpleClientGoalState::SUCCEEDED) {
                        ROS_INFO("The left gripper open");
                        status.request.message = "The left gripper is open.";
                        status_clt.call(status);
                } else {
                        ROS_INFO("The left gripper failed to open.");
                        status.request.message = "The left gripper failed to open.";
                        status_clt.call(status);
                }
        }

}

void PenGripper::close_gripper(const string gripper) {

        if (gripper == "right") {

                ROS_INFO("close the right gripper");

                status.request.message = "close the right gripper";
                status_clt.call(status);

                pr2_controllers_msgs::Pr2GripperCommandGoal squeeze;
                squeeze.command.position = 0.0;
                squeeze.command.max_effort = 50.0; // Close gently

                // close the gripper and wait for the result
                GripperRight->sendGoal(squeeze);
                GripperRight->waitForResult();
                if (GripperRight->getState()
                                == actionlib::SimpleClientGoalState::SUCCEEDED) {
                        ROS_INFO("The right gripper is closed");
                        status.request.message = "The right gripper is closed";
                        status_clt.call(status);
                } else {
                        ROS_INFO("The right gripper failed to close.");
                        status.request.message = "The right gripper failed to close";
                        status_clt.call(status);
                }
        }

        if (gripper == "left") {

                ROS_INFO("close the left gripper");

                status.request.message = "close the left gripper";
                status_clt.call(status);

                pr2_controllers_msgs::Pr2GripperCommandGoal squeeze;
                squeeze.command.position = 0.0;
                squeeze.command.max_effort = 50.0; // Close gently

                // close the gripper and wait for the result
                GripperLeft->sendGoal(squeeze);
                GripperLeft->waitForResult();
                if (GripperLeft->getState()
                                == actionlib::SimpleClientGoalState::SUCCEEDED) {
                        ROS_INFO("The left gripper is closed");
                        status.request.message = "The left gripper is closed";
                        status_clt.call(status);
                } else {
                        ROS_INFO("The left gripper failed to close.");
                        status.request.message = "The left gripper failed to close";
                        status_clt.call(status);
                }
        }

}

void PenGripper::take_start_position() {

        ROS_INFO("take start position");
        status.request.message = "take start position";
        status_clt.call(status);

        // move the head
        ROS_INFO("set the head posture");
        status.request.message = "set the head posture";
        status_clt.call(status);

        //wait for head controller action server to come up
        while (!head->waitForServer(ros::Duration(5.0))) {
                ROS_INFO("Waiting for the point_head_action server to come up");
        }

        //the goal message with a "looking at" goal
        pr2_controllers_msgs::PointHeadGoal goal;

        //the target point, expressed in the requested frame
        geometry_msgs::PointStamped point;
        // point is in /torso_lift_link frame
        point.header.frame_id = "/torso_lift_link";
        // actual point values
        point.point.x = 0.6;
        point.point.y = 0.0;
        point.point.z = -0.4;
        goal.target = point;

        //we are pointing the high-def camera frame
        //(pointing_axis defaults to X-axis)
        goal.pointing_frame = "high_def_frame";

        //take at least 0.5 seconds to get there
        goal.min_duration = ros::Duration(5.0);

        //and go no faster than 1 rad/s
        goal.max_velocity = 1.0;

        //send the goal
        head->sendGoal(goal);

        //wait for it to get there (abort after 2 secs to prevent getting stuck)
        head->waitForResult(ros::Duration(2));

//      ROS_INFO("start the projector");
//      status.request.message = "start the projector";
//      status_clt.call(status);
//
//      // start the projector
//      system(
//                      "rosrun dynamic_reconfigure dynparam set /camera_synchronizer_node narrow_stereo_trig_mode 3");

        // horizontal orientation for the gripper
        tf::Quaternion up = tf::createQuaternionFromRPY(0.0, -0.5 * PI, PI);

        // hanging orientation for the gripper
        btQuaternion down = tf::createQuaternionFromRPY(0.0, 0.5 * PI, PI);

        // trajectory goals for the gripper movement
        ee_cart_imped_msgs::EECartImpedGoal traj_r, traj_l;

        // add start position point of the left gripper
        EECartImpedArm::addTrajectoryPoint(traj_l, l_start_pos.x, l_start_pos.y,
                        l_start_pos.z, up.x(), up.y(), up.z(), up.w(), 1000, 1000, 1000, 30,
                        30, 30, false, false, false, false, false, false, 6,
                        "/torso_lift_link");

        // add start position point of the right gripper
        EECartImpedArm::addTrajectoryPoint(traj_r, r_start_pos.x, r_start_pos.y,
                        r_start_pos.z, up.x(), up.y(), up.z(), up.w(), 1000, 1000, 1000, 30,
                        30, 30, false, false, false, false, false, false, 6,
                        "/torso_lift_link");

        ROS_INFO("set the arm position");
        status.request.message = "set the arm position";
        status_clt.call(status);

        // starts the arm movement
        arm_l->startTrajectory(traj_l);
        arm_r->startTrajectory(traj_r);
        // remove all points from the goal
        arm_l->stopTrajectory();
        arm_r->stopTrajectory();

}

void PenGripper::narrow_stereo_cb(
                const sensor_msgs::PointCloud2ConstPtr& cloud2_msg_ptr) {


        ROS_INFO("the pen is ready now");
        status.request.message = "the pen is ready now";
        status_clt.call(status);

        red_pen.isReady = true;
//      if (!abort) {
//              ROS_INFO("pen detection");
//
//              // input msg in poincloud1 format
//              sensor_msgs::PointCloud cloud1_msg;
//
//              // transformed cloud msg
//              sensor_msgs::PointCloud transformed_cloud1_msg;
//              // transformed cloud2 msg
//              sensor_msgs::PointCloud2 transformed_cloud2_msg;
//
//              // incoming pointcloud in /torso_lift_link frame
//              pcl::PointCloud < pcl::PointXYZ > cloud;
//
//              //Passthrough Filter
//              pcl::PassThrough < pcl::PointXYZ > pass;
//
//              // filtered pointcloud
//              pcl::PointCloud < pcl::PointXYZ > cloud_filtered;
//
//              // tf transformation
//              tf::StampedTransform transform;
//              sensor_msgs::convertPointCloud2ToPointCloud(*cloud2_msg_ptr,
//                              cloud1_msg);
//
//              ros::Time now = ros::Time::now();
//              tflistener.waitForTransform("/torso_lift_link",
//                              "/narrow_stereo_optical_frame", now, ros::Duration(5.0 / 30.0));
//
//              tflistener.lookupTransform("/torso_lift_link",
//                              "/narrow_stereo_optical_frame", now, transform);
//
//              tflistener.transformPointCloud("/torso_lift_link", cloud1_msg,
//                              transformed_cloud1_msg);
//              sensor_msgs::convertPointCloudToPointCloud2(transformed_cloud1_msg,
//                              transformed_cloud2_msg);
//
//              // pointcloud 2 message to Pointcloud
//              pcl::fromROSMsg(transformed_cloud2_msg, cloud);
//
//              ROS_INFO("PointCloud before filtering has: %zu data points.",
//                              cloud.points.size());
//
//              // build the passthrough filter, substract all points with x < 0.3 or x > 0.8
//              pass.setInputCloud(cloud.makeShared());
//              pass.setFilterFieldName("x");
//              pass.setFilterLimits(0.3, 0.8);
//              pass.filter(cloud_filtered);
//
//              ROS_INFO("PointCloud after filtering has: %zu data points.",
//                              cloud_filtered.points.size());
//
//              // all objects needed
//              pcl::NormalEstimation < pcl::PointXYZ, pcl::Normal > ne;
//
//              pcl::PointCloud < pcl::Normal > cloud_normals;
//              pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr tree = boost::make_shared<
//                              pcl::KdTreeFLANN<pcl::PointXYZ> >();
//
//              // Estimate point normals
//              ne.setSearchMethod(tree);
//              ne.setInputCloud(cloud_filtered.makeShared());
//              ne.setKSearch(50);
//              ne.compute(cloud_normals);
//
//              // Create the segmentation object for the planar model and set all the parameters
//              pcl::SACSegmentationFromNormals < pcl::PointXYZ, pcl::Normal > seg;
//              pcl::PointIndices::Ptr inliers_plane(new pcl::PointIndices()),
//                              inliers_cylinder(new pcl::PointIndices());
//              pcl::ModelCoefficients::Ptr coefficients_plane(
//                              new pcl::ModelCoefficients()), coefficients_cylinder(
//                              new pcl::ModelCoefficients());
//
//              // plane segmentation
//              seg.setOptimizeCoefficients(true);
//              seg.setModelType(pcl::SACMODEL_NORMAL_PLANE);
//              seg.setNormalDistanceWeight(0.1);
//              seg.setMethodType(pcl::SAC_RANSAC);
//              seg.setMaxIterations(100);
//              seg.setDistanceThreshold(0.03);
//              seg.setInputCloud(cloud_filtered.makeShared());
//              seg.setInputNormals(cloud_normals.makeShared());
//
//              // Obtain the plane inliers and coefficients
//              seg.segment(*inliers_plane, *coefficients_plane);
//              //std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl;
//
//              if (inliers_plane->indices.size() == 0) {
//
//                      ROS_INFO("could not estimate the table surface");
//                      status.request.message = "could not estimate the table surface";
//                      status_clt.call(status);
//                      red_pen.isReady = false;
//                      success = false;
//                      return;
//
//              } else {
//
//                      // mean height
//                      float z_mean;
//                      z_mean = 0.0;
//
//                      // compute a mean height of all estimated table surface inliers
//                      for (size_t i = 0; i < inliers_plane->indices.size(); i++) {
//                              z_mean = z_mean
//                                              + cloud_filtered.points[inliers_plane->indices[i]].z;
//                      }
//
//                      z_mean = z_mean / inliers_plane->indices.size();
//
//                      if (z_mean < -0.40) {
//                              ROS_INFO("the table surface is to low");
//                              status.request.message = "the table surface is to low";
//                              status_clt.call(status);
//                              red_pen.isReady = false;
//                              success = false;
//                              return;
//                      } else {
//
//                              ROS_INFO("found the table surface");
//                              status.request.message = "found the table surface";
//                              status_clt.call(status);
//
//                              ROS_INFO("table height %f", z_mean);
//
//                              // find the highest point in the cloud
//                              std::sort(cloud_filtered.begin(), cloud_filtered.end(),
//                                              point_sort_z);
//
//                              ROS_INFO("pen height: %f", cloud_filtered.points[0].z - z_mean);
//
//                              //check if the pen is at least 0.10 cm high
//                              if (cloud_filtered.points[0].z - z_mean >= 0.10) {
//
//                                      red_pen.middle.x = cloud_filtered.points[0].x;
//                                      red_pen.middle.y = cloud_filtered.points[0].y;
//
//                                      // pen middle is between highest point and table surface
//                                      red_pen.middle.z = z_mean
//                                                      + (cloud_filtered.points[0].z - z_mean) / 2;
//
//                                      // set the pen middle point
//                                      red_pen.head.x = cloud_filtered.points[0].x;
//                                      red_pen.head.y = cloud_filtered.points[0].y;
//                                      red_pen.head.z = cloud_filtered.points[0].z;
//
//                                      // output
//                                      sensor_msgs::PointCloud2 filtered_pointcloud_msg;
//                                      pcl::toROSMsg(cloud_filtered, filtered_pointcloud_msg);
//                                      pcl_pub.publish(filtered_pointcloud_msg);
//
//                                      ROS_INFO("the pen is ready now");
//                                      status.request.message = "the pen is ready now";
//                                      status_clt.call(status);
//
//                                      red_pen.isReady = true;
//
//                              } else {
//                                      ROS_INFO("the pen is not ready");
//                                      status.request.message = "the pen is not ready";
//                                      status_clt.call(status);
//                                      red_pen.isReady = false;
//                                      success = false;
//                              }
//
//                              // ros marker
//                              visualization_msgs::Marker marker_middle;
//
//                              // Set the frame ID and timestamp.  See the TF tutorials for information on these.
//                              marker_middle.header.frame_id = "/torso_lift_link";
//
//                              marker_middle.header.stamp = ros::Time::now();
//
//                              // Set the namespace and id for this marker_head.  This serves to create a unique ID
//                              // Any marker_head sent with the same namespace and id will overwrite the old one
//                              marker_middle.ns = "pen_gripper";
//                              marker_middle.id = 1;
//                              marker_middle.type = visualization_msgs::Marker::SPHERE;
//
//                              // Set the marker_head action.  Options are ADD and DELETE
//
//                              marker_middle.action = visualization_msgs::Marker::ADD;
//
//                              // Set the pose of the marker_head.  This is a full 6DOF pose relative to the frame/time specified in the header
//
//                              marker_middle.pose.position.x = red_pen.head.x;
//                              marker_middle.pose.position.y = red_pen.head.y;
//                              marker_middle.pose.position.z = red_pen.head.z;
//
//                              // Set the scale of the marker_head -- 1x1x1 here means 1m on a side
//
//                              marker_middle.scale.x = 0.02;
//                              marker_middle.scale.y = 0.02;
//                              marker_middle.scale.z = 0.02;
//
//                              // Set the color -- be sure to set alpha to something non-zero!
//
//                              marker_middle.color.r = 1.0f;
//                              marker_middle.color.g = 0.0f;
//                              marker_middle.color.b = 0.0f;
//                              marker_middle.color.a = 1.0;
//
//                              marker_middle.lifetime = ros::Duration();
//
//                              // Publish the marker_head
//                              ROS_INFO("markers published");
//                              marker_middle_pub.publish(marker_middle);
//                      }
//
//              }
//
//              status.request.message = "stop the projector";
//              status_clt.call(status);
//              // stop the projector
//              system(
//                              "rosrun dynamic_reconfigure dynparam set /camera_synchronizer_node narrow_stereo_trig_mode 4");
//
//      }

}

void PenGripper::grip_pen()

{

        success = true;
        //check if the pen is ready
        if (!red_pen.isReady) {
                ROS_INFO("pen is not ready");
                status.request.message = "pen is not ready";
                status_clt.call(status);

        } else {
                // open the right gripper
                this->open_gripper("right");

                ROS_INFO("grip the pen");
                status.request.message = "grip the pen";
                status_clt.call(status);

                ee_cart_imped_msgs::EECartImpedGoal traj_r, end;

                // gripper orientation in euler angles
                tf::Quaternion grip_forward = tf::createQuaternionFromRPY(0.0, 0.0, PI);
                tf::Quaternion grip_left = tf::createQuaternionFromRPY(0.5 * PI, 0.0, PI);

                // point at 0.1 m of the right side of the pen
                EECartImpedArm::addTrajectoryPoint(traj_r, double(red_pen.middle.x),
                                double(red_pen.middle.y - 0.1), double(red_pen.middle.z),
                                grip_left.x(), grip_left.y(), grip_left.z(), grip_left.w(),
                                1000, 1000, 1000, 30, 30, 30, false, false, false, false, false,
                                false, 8, "/torso_lift_link");
                // pen position point
                EECartImpedArm::addTrajectoryPoint(traj_r, double(red_pen.middle.x),
                                double(red_pen.middle.y + 0.04),
                                double(red_pen.middle.z - 0.01), grip_left.x(), grip_left.y(),
                                grip_left.z(), grip_left.w(), 1000, 1000, 1000, 30, 30, 30,
                                false, false, false, false, false, false, 12,
                                "/torso_lift_link");

                // start the gripper movement
                arm_r->startTrajectory(traj_r);

                // grip the pen
                this->close_gripper("right");

                // end position point
                EECartImpedArm::addTrajectoryPoint(end, double(r_start_pos.x),
                                double(r_start_pos.y), double(r_start_pos.z), grip_left.x(),
                                grip_left.y(), grip_left.z(), grip_left.w(), 1000, 1000, 1000,
                                30, 30, 30, false, false, false, false, false, false, 8,
                                "/torso_lift_link");
                // start to go to the end position
                arm_r->startTrajectory(end);

                // pen gripping was successfull
                success = true;

        }

}

void PenGripper::reset() {
        // reset the pen_gripper object members
        red_pen.isReady = true;
        success = true;
        abort = true;

//      red_pen.isReady = false;
//      success = false;
//      abort = true;
}

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

        // handle the requests
        if (!req.abort) {
                ROS_INFO("Incomming Request...");
                status.request.message = "Incomming Request...";
                status_clt.call(status);

                // activate the pen_gripper
                abort = false;
                res.success = true;
                return true;
        } else {
                ROS_INFO("reset...");
                status.request.message = "reset...";
                status_clt.call(status);

                // reset the object
                this->reset();
                res.success = true;
                return true;

        }
}