GraspSpongeServer.cpp

Go to the documentation of this file.

/*
 * GraspSponge.cpp
 *
 *  Created on: Sep 27, 2012
 *      Author: tidyup
 */

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <pr2_controllers_msgs/JointTrajectoryAction.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
#include <pr2_controllers_msgs/Pr2GripperCommandAction.h>
#include <arm_navigation_msgs/FilterJointTrajectory.h>
#include <pr2_controllers_msgs/SingleJointPositionAction.h>
#include <std_srvs/Empty.h>
#include <algorithm>
#include <iterator>
#include <vector>
#include <angles/angles.h>
class GraspSpongeServer {

public:
        GraspSpongeServer(ros::NodeHandle& n);
        ~GraspSpongeServer();
private:
        pr2_controllers_msgs::JointTrajectoryGoal filterTrajectory(
                        const pr2_controllers_msgs::JointTrajectoryGoal & traj);

        ros::NodeHandle nh_;
        ros::ServiceServer srv_grasp_;
        ros::ServiceServer srv_putaway;
        ros::Publisher pub_;

        trajectory_msgs::JointTrajectoryPoint start_;
        trajectory_msgs::JointTrajectoryPoint above_;
        trajectory_msgs::JointTrajectoryPoint end_;
        pr2_controllers_msgs::JointTrajectoryGoal goal1_;
        pr2_controllers_msgs::JointTrajectoryGoal goal2_;
        pr2_controllers_msgs::JointTrajectoryGoal goal3_;
        pr2_controllers_msgs::JointTrajectoryGoal goal4_;

        ros::ServiceClient filter_trajectory_client_;

        actionlib::SimpleActionClient<pr2_controllers_msgs::JointTrajectoryAction>* traj_client_;
        actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction>* gripper_client_;
        actionlib::SimpleActionClient<
                        pr2_controllers_msgs::SingleJointPositionAction>* torso_client_;
        //assume table is convex polygon
        bool srvPutawayCB(std_srvs::Empty::Request& req,
                        std_srvs::Empty::Response& res);
        //assume table is convex polygon
        bool srvGraspCB(std_srvs::Empty::Request& req,
                        std_srvs::Empty::Response& res);
        void torsoUp();
        void torsoDown();

};
GraspSpongeServer::GraspSpongeServer(ros::NodeHandle& n) :
                nh_(n), traj_client_(0), gripper_client_(0), torso_client_(0) {
        traj_client_ = new actionlib::SimpleActionClient<
                        pr2_controllers_msgs::JointTrajectoryAction>(
                        "/r_arm_controller/joint_trajectory_action", true);
        while (!traj_client_->waitForServer(ros::Duration(5.0))) {
                ROS_INFO("Waiting for the joint_trajectory_action server");
        }
        gripper_client_ = new actionlib::SimpleActionClient<
                        pr2_controllers_msgs::Pr2GripperCommandAction>(
                        "/r_gripper_controller/gripper_action", true);
        while (!gripper_client_->waitForServer(ros::Duration(5.0))) {
                ROS_INFO("Waiting for the gripper_action server");
        }
        torso_client_ = new actionlib::SimpleActionClient<
                        pr2_controllers_msgs::SingleJointPositionAction>(
                        "/torso_controller/position_joint_action", true);
        while (!torso_client_->waitForServer(ros::Duration(5.0))) {
                ROS_INFO("Waiting for the torso_action server");
        }
        srv_grasp_ = nh_.advertiseService("pickup_sponge",
                        &GraspSpongeServer::srvGraspCB, this);
        srv_putaway = nh_.advertiseService("putdown_sponge",
                        &GraspSpongeServer::srvPutawayCB, this);
        ROS_INFO(
                        "Waiting for trajectory_filter_unnormalizer/filter_trajectory service");
        ros::service::waitForService(
                        "/trajectory_filter_unnormalizer/filter_trajectory");
        ROS_INFO("Found.");
        filter_trajectory_client_ = nh_.serviceClient<
                        arm_navigation_msgs::FilterJointTrajectory>(
                        "/trajectory_filter_unnormalizer/filter_trajectory");

        std::vector<std::string> joint_names;
        // First, the joint names, which apply to all waypoints
        joint_names.push_back("r_shoulder_pan_joint");
        joint_names.push_back("r_shoulder_lift_joint");
        joint_names.push_back("r_upper_arm_roll_joint");
        joint_names.push_back("r_elbow_flex_joint");
        joint_names.push_back("r_forearm_roll_joint");
        joint_names.push_back("r_wrist_flex_joint");
        joint_names.push_back("r_wrist_roll_joint");

        double pose1[] = { -1.455, 1.262, -1.540, -1.768, -15.694, -1.756, -7.990 };
        double pose2[] = { -0.468, 1.185, -1.076, -1.594, -15.656, -1.756, -7.990 };
        double pose3[] = { -0.379, 1.042, -1.751, -2.053, -16.242, -1.884, -7.956 };
        double pose4[] = { -0.402, 1.031, -1.752, -2.039, -15.197, -1.869, -7.844 };
        double pose5[] = { -0.522, 1.008, -1.645, -2.267, -15.067, -1.865, -7.852 };
        double pose6[] = { -1.269, 1.286, -1.371, -1.457, -12.616, -1.865, -7.852 };

        double final_pose[] = { -2.110, 1.230, -2.06, -1.69, 0.3, -1.32, 1.5 };

        std::vector<std::vector<double> > traj_points;
        traj_points.push_back(std::vector<double>());
        std::copy(pose1, pose1 + 7, std::back_inserter(traj_points.back()));
        traj_points.push_back(std::vector<double>());
        std::copy(pose2, pose2 + 7, std::back_inserter(traj_points.back()));
        traj_points.push_back(std::vector<double>());
        std::copy(pose3, pose3 + 7, std::back_inserter(traj_points.back()));
        traj_points.push_back(std::vector<double>());
        std::copy(pose4, pose4 + 7, std::back_inserter(traj_points.back()));
        traj_points.push_back(std::vector<double>());
        std::copy(pose5, pose5 + 7, std::back_inserter(traj_points.back()));
        traj_points.push_back(std::vector<double>());
        std::copy(pose6, pose6 + 7, std::back_inserter(traj_points.back()));
        traj_points.push_back(std::vector<double>());
        std::copy(final_pose, final_pose + 7,
                        std::back_inserter(traj_points.back()));
        //traj_points.push_back(std::vector<double>());
        //std::copy (pose8, pose8 + 7, std::back_inserter ( traj_points.back() ) );

//  for(int i=0;i<traj_points.size();i++){
//    traj_points[i][6]=angles::normalize_angle(traj_points[i][6]);
//    traj_points[i][4]=angles::normalize_angle(traj_points[i][4]);
//  }

        for (int i = 1; i < traj_points.size(); i++) {
                double p6 = traj_points[i - 1][6];
                double c6 = traj_points[i][6];
                double d6 = c6 - p6;
                while (d6 > M_PI) {
                        c6 -= 2 * M_PI;
                        d6 = c6 - p6;
                }
                while (d6 < -M_PI) {
                        c6 += 2 * M_PI;
                        d6 = c6 - p6;
                }
                traj_points[i][6] = c6;

                double p4 = traj_points[i - 1][4];
                double c4 = traj_points[i][4];
                double d4 = c4 - p4;
                while (d4 > M_PI) {
                        c4 -= 2 * M_PI;
                        d4 = c4 - p4;
                }
                while (d4 < -M_PI) {
                        c4 += 2 * M_PI;
                        d4 = c4 - p4;
                }
                traj_points[i][4] = c4;
        }

        goal1_.trajectory.joint_names = joint_names;
        goal2_.trajectory.joint_names = joint_names;
        goal3_.trajectory.joint_names = joint_names;
        goal4_.trajectory.joint_names = joint_names;

        double time = 3.0;
        double inc = 1.0;
        for (int i = 0; i <= 3; i++) {

                trajectory_msgs::JointTrajectoryPoint tmp;
                tmp.positions = traj_points[i];
                tmp.velocities = std::vector<double>(7, 0);
                tmp.time_from_start = ros::Duration(time);
                goal1_.trajectory.points.push_back(tmp);
                time = time + inc;
        }

        time = 3.0;
        for (int i = 4; i <= 6; i++) {
                trajectory_msgs::JointTrajectoryPoint tmp;

                tmp.positions = traj_points[i];
                tmp.velocities = std::vector<double>(7, 0);
                tmp.time_from_start = ros::Duration(time);
                goal2_.trajectory.points.push_back(tmp);
                time = time + inc;
        }

        ROS_INFO_STREAM(goal1_);
        ROS_INFO_STREAM(goal2_);
        //exit(1);

        //place
        time = 3.0;
        for (int i = 2; i >= 0; i--) {

                trajectory_msgs::JointTrajectoryPoint tmp;
                tmp.positions = traj_points[i];
                tmp.velocities = std::vector<double>(7, 0);
                tmp.time_from_start = ros::Duration(time);
                goal4_.trajectory.points.push_back(tmp);
                time = time + inc;
        }

        time = 3.0;
        for (int i = 5; i >= 3; i--) {
                trajectory_msgs::JointTrajectoryPoint tmp;

                tmp.positions = traj_points[i];
                tmp.velocities = std::vector<double>(7, 0);
                tmp.time_from_start = ros::Duration(time);
                goal3_.trajectory.points.push_back(tmp);
                time = time + inc;
        }
        trajectory_msgs::JointTrajectoryPoint tmp;
        tmp.positions = traj_points[6];
        tmp.velocities = std::vector<double>(7, 0);
        tmp.time_from_start = ros::Duration(time);
        goal4_.trajectory.points.push_back(tmp);
        time = time + inc;

}
GraspSpongeServer::~GraspSpongeServer() {
        delete traj_client_;
        delete gripper_client_;
        delete torso_client_;
}

pr2_controllers_msgs::JointTrajectoryGoal GraspSpongeServer::filterTrajectory(
                const pr2_controllers_msgs::JointTrajectoryGoal & traj) {
        arm_navigation_msgs::FilterJointTrajectory::Request req;
        arm_navigation_msgs::FilterJointTrajectory::Response res;

        req.trajectory = traj.trajectory;
        if (!filter_trajectory_client_.call(req, res)) {
                res.trajectory = req.trajectory;
                ROS_WARN("filter_trajectory_client_ failed.");
        } else if (res.error_code.val
                        != arm_navigation_msgs::ArmNavigationErrorCodes::SUCCESS) {
                res.trajectory = req.trajectory;
                ROS_WARN(
                                "filter_trajectory_client_ reported error: %d", res.error_code.val);
        }

        pr2_controllers_msgs::JointTrajectoryGoal filtered = traj;
        filtered.trajectory = res.trajectory;
        return filtered;
}
//tell the torso to go up
void GraspSpongeServer::torsoUp() {

        pr2_controllers_msgs::SingleJointPositionGoal up;
        up.position = 0.25; //all the way up is 0.2
        up.min_duration = ros::Duration(2.0);
        up.max_velocity = 1.0;

        ROS_INFO("Sending up goal");
        torso_client_->sendGoal(up);
        torso_client_->waitForResult(ros::Duration(30));
}

//tell the torso to go down
void GraspSpongeServer::torsoDown() {

        pr2_controllers_msgs::SingleJointPositionGoal down;
        down.position = 0.013;
        down.min_duration = ros::Duration(2.0);
        down.max_velocity = 1.0;

        ROS_INFO("Sending down goal");
        torso_client_->sendGoal(down);
        torso_client_->waitForResult(ros::Duration(30));
}

bool GraspSpongeServer::srvGraspCB(std_srvs::Empty::Request& req,
                std_srvs::Empty::Response& res) {

        // move torso down
        torsoDown();

        //open gripper
        pr2_controllers_msgs::Pr2GripperCommandGoal gripper_goal;
        gripper_goal.command.position = 0.98;
        gripper_goal.command.max_effort = -1;
        if (!gripper_client_->sendGoalAndWait(gripper_goal).SUCCEEDED) {
                ROS_ERROR("Closing gripper did not succeed.");
                return false;
        }

        //grasp
        if (!traj_client_->sendGoalAndWait(filterTrajectory(goal1_)).SUCCEEDED) {
                ROS_ERROR("Sponge grasping trajectory did not succeed.");
                return false;
        }

        //close gripper
        gripper_goal.command.position = 0.02;
        gripper_goal.command.max_effort = 50;
        if (!gripper_client_->sendGoalAndWait(gripper_goal).SUCCEEDED) {
                ROS_ERROR("Closing gripper did not succeed.");
                return false;
        }

        ROS_INFO_STREAM("goal before filtering"<<goal2_);
        //grasp
        if (!traj_client_->sendGoalAndWait(filterTrajectory(goal2_)).SUCCEEDED) {
                ROS_ERROR("Could not go back to start position.");
                return false;
        }

        ROS_INFO_STREAM("goal after filtering"<<filterTrajectory(goal2_));

        //move torso up
        torsoUp();

        return true;

}

bool GraspSpongeServer::srvPutawayCB(std_srvs::Empty::Request& req,
                std_srvs::Empty::Response& res) {

        //move torso down
        torsoDown();

        //grasp
        if (!traj_client_->sendGoalAndWait(filterTrajectory(goal3_)).SUCCEEDED) {
                ROS_ERROR("Could not go back to start position.");
                return false;
        }

        pr2_controllers_msgs::Pr2GripperCommandGoal gripper_goal;
        gripper_goal.command.position = 0.98;
        gripper_goal.command.max_effort = -1;
        if (!gripper_client_->sendGoalAndWait(gripper_goal).SUCCEEDED) {
                ROS_ERROR("Closing gripper did not succeed.");
                return false;
        }

        //grasp
        if (!traj_client_->sendGoalAndWait(filterTrajectory(goal4_)).SUCCEEDED) {
                ROS_ERROR("Could not go back to start position.");
                return false;
        }

        return true;
}

int main(int argc, char** argv) {
        // Init the ROS node
        ros::init(argc, argv, "sponge_server");
        ros::NodeHandle n;
        GraspSpongeServer server(n);
        // Wait for trajectory completion
        ros::spin();
}