00001
00002
00003
00004
00005
00006
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
00050 bool srvPutawayCB(std_srvs::Empty::Request& req,
00051 std_srvs::Empty::Response& res);
00052
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
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
00128
00129
00130
00131
00132
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
00194
00195
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
00252 void GraspSpongeServer::torsoUp() {
00253
00254 pr2_controllers_msgs::SingleJointPositionGoal up;
00255 up.position = 0.25;
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
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
00281 torsoDown();
00282
00283
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
00293 if (!traj_client_->sendGoalAndWait(filterTrajectory(goal1_)).SUCCEEDED) {
00294 ROS_ERROR("Sponge grasping trajectory did not succeed.");
00295 return false;
00296 }
00297
00298
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
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
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
00326 torsoDown();
00327
00328
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
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
00353 ros::init(argc, argv, "sponge_server");
00354 ros::NodeHandle n;
00355 GraspSpongeServer server(n);
00356
00357 ros::spin();
00358 }