GraspSpongeServer.cpp
Go to the documentation of this file.
00001 
00002 /*
00003  * GraspSponge.cpp
00004  *
00005  *  Created on: Sep 27, 2012
00006  *      Author: tidyup
00007  */
00008 
00009 #include <ros/ros.h>
00010 #include <actionlib/client/simple_action_client.h>
00011 #include <pr2_controllers_msgs/JointTrajectoryAction.h>
00012 #include <trajectory_msgs/JointTrajectoryPoint.h>
00013 #include <pr2_controllers_msgs/Pr2GripperCommandAction.h>
00014 #include <arm_navigation_msgs/FilterJointTrajectory.h>
00015 #include <pr2_controllers_msgs/SingleJointPositionAction.h>
00016 #include <std_srvs/Empty.h>
00017 #include <algorithm>
00018 #include <iterator>
00019 #include <vector>
00020 #include <angles/angles.h>
00021 class GraspSpongeServer {
00022 
00023 public:
00024         GraspSpongeServer(ros::NodeHandle& n);
00025         ~GraspSpongeServer();
00026 private:
00027         pr2_controllers_msgs::JointTrajectoryGoal filterTrajectory(
00028                         const pr2_controllers_msgs::JointTrajectoryGoal & traj);
00029 
00030         ros::NodeHandle nh_;
00031         ros::ServiceServer srv_grasp_;
00032         ros::ServiceServer srv_putaway;
00033         ros::Publisher pub_;
00034 
00035         trajectory_msgs::JointTrajectoryPoint start_;
00036         trajectory_msgs::JointTrajectoryPoint above_;
00037         trajectory_msgs::JointTrajectoryPoint end_;
00038         pr2_controllers_msgs::JointTrajectoryGoal goal1_;
00039         pr2_controllers_msgs::JointTrajectoryGoal goal2_;
00040         pr2_controllers_msgs::JointTrajectoryGoal goal3_;
00041         pr2_controllers_msgs::JointTrajectoryGoal goal4_;
00042 
00043         ros::ServiceClient filter_trajectory_client_;
00044 
00045         actionlib::SimpleActionClient<pr2_controllers_msgs::JointTrajectoryAction>* traj_client_;
00046         actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction>* gripper_client_;
00047         actionlib::SimpleActionClient<
00048                         pr2_controllers_msgs::SingleJointPositionAction>* torso_client_;
00049         //assume table is convex polygon
00050         bool srvPutawayCB(std_srvs::Empty::Request& req,
00051                         std_srvs::Empty::Response& res);
00052         //assume table is convex polygon
00053         bool srvGraspCB(std_srvs::Empty::Request& req,
00054                         std_srvs::Empty::Response& res);
00055         void torsoUp();
00056         void torsoDown();
00057 
00058 };
00059 GraspSpongeServer::GraspSpongeServer(ros::NodeHandle& n) :
00060                 nh_(n), traj_client_(0), gripper_client_(0), torso_client_(0) {
00061         traj_client_ = new actionlib::SimpleActionClient<
00062                         pr2_controllers_msgs::JointTrajectoryAction>(
00063                         "/r_arm_controller/joint_trajectory_action", true);
00064         while (!traj_client_->waitForServer(ros::Duration(5.0))) {
00065                 ROS_INFO("Waiting for the joint_trajectory_action server");
00066         }
00067         gripper_client_ = new actionlib::SimpleActionClient<
00068                         pr2_controllers_msgs::Pr2GripperCommandAction>(
00069                         "/r_gripper_controller/gripper_action", true);
00070         while (!gripper_client_->waitForServer(ros::Duration(5.0))) {
00071                 ROS_INFO("Waiting for the gripper_action server");
00072         }
00073         torso_client_ = new actionlib::SimpleActionClient<
00074                         pr2_controllers_msgs::SingleJointPositionAction>(
00075                         "/torso_controller/position_joint_action", true);
00076         while (!torso_client_->waitForServer(ros::Duration(5.0))) {
00077                 ROS_INFO("Waiting for the torso_action server");
00078         }
00079         srv_grasp_ = nh_.advertiseService("pickup_sponge",
00080                         &GraspSpongeServer::srvGraspCB, this);
00081         srv_putaway = nh_.advertiseService("putdown_sponge",
00082                         &GraspSpongeServer::srvPutawayCB, this);
00083         ROS_INFO(
00084                         "Waiting for trajectory_filter_unnormalizer/filter_trajectory service");
00085         ros::service::waitForService(
00086                         "/trajectory_filter_unnormalizer/filter_trajectory");
00087         ROS_INFO("Found.");
00088         filter_trajectory_client_ = nh_.serviceClient<
00089                         arm_navigation_msgs::FilterJointTrajectory>(
00090                         "/trajectory_filter_unnormalizer/filter_trajectory");
00091 
00092         std::vector<std::string> joint_names;
00093         // First, the joint names, which apply to all waypoints
00094         joint_names.push_back("r_shoulder_pan_joint");
00095         joint_names.push_back("r_shoulder_lift_joint");
00096         joint_names.push_back("r_upper_arm_roll_joint");
00097         joint_names.push_back("r_elbow_flex_joint");
00098         joint_names.push_back("r_forearm_roll_joint");
00099         joint_names.push_back("r_wrist_flex_joint");
00100         joint_names.push_back("r_wrist_roll_joint");
00101 
00102         double pose1[] = { -1.455, 1.262, -1.540, -1.768, -15.694, -1.756, -7.990 };
00103         double pose2[] = { -0.468, 1.185, -1.076, -1.594, -15.656, -1.756, -7.990 };
00104         double pose3[] = { -0.379, 1.042, -1.751, -2.053, -16.242, -1.884, -7.956 };
00105         double pose4[] = { -0.402, 1.031, -1.752, -2.039, -15.197, -1.869, -7.844 };
00106         double pose5[] = { -0.522, 1.008, -1.645, -2.267, -15.067, -1.865, -7.852 };
00107         double pose6[] = { -1.269, 1.286, -1.371, -1.457, -12.616, -1.865, -7.852 };
00108 
00109         double final_pose[] = { -2.110, 1.230, -2.06, -1.69, 0.3, -1.32, 1.5 };
00110 
00111         std::vector<std::vector<double> > traj_points;
00112         traj_points.push_back(std::vector<double>());
00113         std::copy(pose1, pose1 + 7, std::back_inserter(traj_points.back()));
00114         traj_points.push_back(std::vector<double>());
00115         std::copy(pose2, pose2 + 7, std::back_inserter(traj_points.back()));
00116         traj_points.push_back(std::vector<double>());
00117         std::copy(pose3, pose3 + 7, std::back_inserter(traj_points.back()));
00118         traj_points.push_back(std::vector<double>());
00119         std::copy(pose4, pose4 + 7, std::back_inserter(traj_points.back()));
00120         traj_points.push_back(std::vector<double>());
00121         std::copy(pose5, pose5 + 7, std::back_inserter(traj_points.back()));
00122         traj_points.push_back(std::vector<double>());
00123         std::copy(pose6, pose6 + 7, std::back_inserter(traj_points.back()));
00124         traj_points.push_back(std::vector<double>());
00125         std::copy(final_pose, final_pose + 7,
00126                         std::back_inserter(traj_points.back()));
00127         //traj_points.push_back(std::vector<double>());
00128         //std::copy (pose8, pose8 + 7, std::back_inserter ( traj_points.back() ) );
00129 
00130 //  for(int i=0;i<traj_points.size();i++){
00131 //    traj_points[i][6]=angles::normalize_angle(traj_points[i][6]);
00132 //    traj_points[i][4]=angles::normalize_angle(traj_points[i][4]);
00133 //  }
00134 
00135         for (int i = 1; i < traj_points.size(); i++) {
00136                 double p6 = traj_points[i - 1][6];
00137                 double c6 = traj_points[i][6];
00138                 double d6 = c6 - p6;
00139                 while (d6 > M_PI) {
00140                         c6 -= 2 * M_PI;
00141                         d6 = c6 - p6;
00142                 }
00143                 while (d6 < -M_PI) {
00144                         c6 += 2 * M_PI;
00145                         d6 = c6 - p6;
00146                 }
00147                 traj_points[i][6] = c6;
00148 
00149                 double p4 = traj_points[i - 1][4];
00150                 double c4 = traj_points[i][4];
00151                 double d4 = c4 - p4;
00152                 while (d4 > M_PI) {
00153                         c4 -= 2 * M_PI;
00154                         d4 = c4 - p4;
00155                 }
00156                 while (d4 < -M_PI) {
00157                         c4 += 2 * M_PI;
00158                         d4 = c4 - p4;
00159                 }
00160                 traj_points[i][4] = c4;
00161         }
00162 
00163         goal1_.trajectory.joint_names = joint_names;
00164         goal2_.trajectory.joint_names = joint_names;
00165         goal3_.trajectory.joint_names = joint_names;
00166         goal4_.trajectory.joint_names = joint_names;
00167 
00168         double time = 3.0;
00169         double inc = 1.0;
00170         for (int i = 0; i <= 3; i++) {
00171 
00172                 trajectory_msgs::JointTrajectoryPoint tmp;
00173                 tmp.positions = traj_points[i];
00174                 tmp.velocities = std::vector<double>(7, 0);
00175                 tmp.time_from_start = ros::Duration(time);
00176                 goal1_.trajectory.points.push_back(tmp);
00177                 time = time + inc;
00178         }
00179 
00180         time = 3.0;
00181         for (int i = 4; i <= 6; i++) {
00182                 trajectory_msgs::JointTrajectoryPoint tmp;
00183 
00184                 tmp.positions = traj_points[i];
00185                 tmp.velocities = std::vector<double>(7, 0);
00186                 tmp.time_from_start = ros::Duration(time);
00187                 goal2_.trajectory.points.push_back(tmp);
00188                 time = time + inc;
00189         }
00190 
00191         ROS_INFO_STREAM(goal1_);
00192         ROS_INFO_STREAM(goal2_);
00193         //exit(1);
00194 
00195         //place
00196         time = 3.0;
00197         for (int i = 2; i >= 0; i--) {
00198 
00199                 trajectory_msgs::JointTrajectoryPoint tmp;
00200                 tmp.positions = traj_points[i];
00201                 tmp.velocities = std::vector<double>(7, 0);
00202                 tmp.time_from_start = ros::Duration(time);
00203                 goal4_.trajectory.points.push_back(tmp);
00204                 time = time + inc;
00205         }
00206 
00207         time = 3.0;
00208         for (int i = 5; i >= 3; i--) {
00209                 trajectory_msgs::JointTrajectoryPoint tmp;
00210 
00211                 tmp.positions = traj_points[i];
00212                 tmp.velocities = std::vector<double>(7, 0);
00213                 tmp.time_from_start = ros::Duration(time);
00214                 goal3_.trajectory.points.push_back(tmp);
00215                 time = time + inc;
00216         }
00217         trajectory_msgs::JointTrajectoryPoint tmp;
00218         tmp.positions = traj_points[6];
00219         tmp.velocities = std::vector<double>(7, 0);
00220         tmp.time_from_start = ros::Duration(time);
00221         goal4_.trajectory.points.push_back(tmp);
00222         time = time + inc;
00223 
00224 }
00225 GraspSpongeServer::~GraspSpongeServer() {
00226         delete traj_client_;
00227         delete gripper_client_;
00228         delete torso_client_;
00229 }
00230 
00231 pr2_controllers_msgs::JointTrajectoryGoal GraspSpongeServer::filterTrajectory(
00232                 const pr2_controllers_msgs::JointTrajectoryGoal & traj) {
00233         arm_navigation_msgs::FilterJointTrajectory::Request req;
00234         arm_navigation_msgs::FilterJointTrajectory::Response res;
00235 
00236         req.trajectory = traj.trajectory;
00237         if (!filter_trajectory_client_.call(req, res)) {
00238                 res.trajectory = req.trajectory;
00239                 ROS_WARN("filter_trajectory_client_ failed.");
00240         } else if (res.error_code.val
00241                         != arm_navigation_msgs::ArmNavigationErrorCodes::SUCCESS) {
00242                 res.trajectory = req.trajectory;
00243                 ROS_WARN(
00244                                 "filter_trajectory_client_ reported error: %d", res.error_code.val);
00245         }
00246 
00247         pr2_controllers_msgs::JointTrajectoryGoal filtered = traj;
00248         filtered.trajectory = res.trajectory;
00249         return filtered;
00250 }
00251 //tell the torso to go up
00252 void GraspSpongeServer::torsoUp() {
00253 
00254         pr2_controllers_msgs::SingleJointPositionGoal up;
00255         up.position = 0.25; //all the way up is 0.2
00256         up.min_duration = ros::Duration(2.0);
00257         up.max_velocity = 1.0;
00258 
00259         ROS_INFO("Sending up goal");
00260         torso_client_->sendGoal(up);
00261         torso_client_->waitForResult(ros::Duration(30));
00262 }
00263 
00264 //tell the torso to go down
00265 void GraspSpongeServer::torsoDown() {
00266 
00267         pr2_controllers_msgs::SingleJointPositionGoal down;
00268         down.position = 0.013;
00269         down.min_duration = ros::Duration(2.0);
00270         down.max_velocity = 1.0;
00271 
00272         ROS_INFO("Sending down goal");
00273         torso_client_->sendGoal(down);
00274         torso_client_->waitForResult(ros::Duration(30));
00275 }
00276 
00277 bool GraspSpongeServer::srvGraspCB(std_srvs::Empty::Request& req,
00278                 std_srvs::Empty::Response& res) {
00279 
00280         // move torso down
00281         torsoDown();
00282 
00283         //open gripper
00284         pr2_controllers_msgs::Pr2GripperCommandGoal gripper_goal;
00285         gripper_goal.command.position = 0.98;
00286         gripper_goal.command.max_effort = -1;
00287         if (!gripper_client_->sendGoalAndWait(gripper_goal).SUCCEEDED) {
00288                 ROS_ERROR("Closing gripper did not succeed.");
00289                 return false;
00290         }
00291 
00292         //grasp
00293         if (!traj_client_->sendGoalAndWait(filterTrajectory(goal1_)).SUCCEEDED) {
00294                 ROS_ERROR("Sponge grasping trajectory did not succeed.");
00295                 return false;
00296         }
00297 
00298         //close gripper
00299         gripper_goal.command.position = 0.02;
00300         gripper_goal.command.max_effort = 50;
00301         if (!gripper_client_->sendGoalAndWait(gripper_goal).SUCCEEDED) {
00302                 ROS_ERROR("Closing gripper did not succeed.");
00303                 return false;
00304         }
00305 
00306         ROS_INFO_STREAM("goal before filtering"<<goal2_);
00307         //grasp
00308         if (!traj_client_->sendGoalAndWait(filterTrajectory(goal2_)).SUCCEEDED) {
00309                 ROS_ERROR("Could not go back to start position.");
00310                 return false;
00311         }
00312 
00313         ROS_INFO_STREAM("goal after filtering"<<filterTrajectory(goal2_));
00314 
00315         //move torso up
00316         torsoUp();
00317 
00318         return true;
00319 
00320 }
00321 
00322 bool GraspSpongeServer::srvPutawayCB(std_srvs::Empty::Request& req,
00323                 std_srvs::Empty::Response& res) {
00324 
00325         //move torso down
00326         torsoDown();
00327 
00328         //grasp
00329         if (!traj_client_->sendGoalAndWait(filterTrajectory(goal3_)).SUCCEEDED) {
00330                 ROS_ERROR("Could not go back to start position.");
00331                 return false;
00332         }
00333 
00334         pr2_controllers_msgs::Pr2GripperCommandGoal gripper_goal;
00335         gripper_goal.command.position = 0.98;
00336         gripper_goal.command.max_effort = -1;
00337         if (!gripper_client_->sendGoalAndWait(gripper_goal).SUCCEEDED) {
00338                 ROS_ERROR("Closing gripper did not succeed.");
00339                 return false;
00340         }
00341 
00342         //grasp
00343         if (!traj_client_->sendGoalAndWait(filterTrajectory(goal4_)).SUCCEEDED) {
00344                 ROS_ERROR("Could not go back to start position.");
00345                 return false;
00346         }
00347 
00348         return true;
00349 }
00350 
00351 int main(int argc, char** argv) {
00352         // Init the ROS node
00353         ros::init(argc, argv, "sponge_server");
00354         ros::NodeHandle n;
00355         GraspSpongeServer server(n);
00356         // Wait for trajectory completion
00357         ros::spin();
00358 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


coverage_3d_tools
Author(s): Juergen Hess
autogenerated on Wed Dec 26 2012 15:25:37