00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #include <mtconnect_cnc_robot_example/move_arm_action_clients/MovePickPlaceServer.h>
00019 #include <boost/bind.hpp>
00020
00021
00022 typedef actionlib::SimpleClientGoalState GoalState;
00023
00024 using namespace mtconnect_cnc_robot_example;
00025
00026 MovePickPlaceServer::MovePickPlaceServer() :
00027 MoveArmActionClient(),
00028 pickup_gh_(),
00029 place_gh_()
00030 {
00031
00032
00033 }
00034
00035 MovePickPlaceServer::~MovePickPlaceServer()
00036 {
00037
00038 }
00039
00040 void MovePickPlaceServer::run()
00041 {
00042 if(!setup())
00043 {
00044 return;
00045 }
00046
00047 ros::AsyncSpinner spinner(2);
00048 spinner.start();
00049
00050
00051 arm_pickup_server_ptr_->start();
00052 arm_place_server_ptr_->start();
00053
00054 while(ros::ok())
00055 {
00056 ros::Duration(DURATION_LOOP_PAUSE).sleep();
00057 }
00058 }
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080 bool MovePickPlaceServer::fetchParameters(std::string name_space)
00081 {
00082 ros::NodeHandle nh("~");
00083 bool success = nh.getParam(PARAM_ARM_GROUP,arm_group_);
00084 if(success)
00085 {
00086 ROS_INFO_STREAM("Successfully read setup parameters");
00087 }
00088 else
00089 {
00090 ROS_ERROR_STREAM("Failed to read setup parameters");
00091 }
00092
00093 return success;
00094 }
00095
00096 bool MovePickPlaceServer::setup()
00097 {
00098 int wait_attempts = 0;
00099 ros::NodeHandle nh;
00100
00101 if(!MoveArmActionClient::setup())
00102 {
00103 return false;
00104 }
00105
00106
00107 ROS_INFO_STREAM("Setting up pickup server");
00108 arm_pickup_server_ptr_ = MoveArmPickupServerPtr(new MoveArmPickupServer(nh,DEFAULT_PICKUP_ACTION,
00109 boost::bind(&MovePickPlaceServer::pickupGoalCallback,this, _1),
00110 boost::bind(&MovePickPlaceServer::pickupCancelCallback,this,_1),
00111 false));
00112
00113
00114 ROS_INFO_STREAM("Setting up place server");
00115 arm_place_server_ptr_ = MoveArmPlaceServerPtr(new MoveArmPlaceServer(nh,DEFAULT_PLACE_ACTION,
00116 boost::bind(&MovePickPlaceServer::placeGoalCallback,this, _1),
00117 boost::bind(&MovePickPlaceServer::placeCancelCallback,this,_1),
00118 false));
00119
00120
00121 ROS_INFO_STREAM("Setting up grasp client");
00122 grasp_action_client_ptr_ = GraspActionClientPtr(new GraspActionClient(DEFAULT_GRASP_ACTION,true));
00123 while(!grasp_action_client_ptr_->waitForServer(ros::Duration(DURATION_WAIT_SERVER)))
00124 {
00125 ROS_WARN_STREAM("Waiting for grasp action server "<<DEFAULT_GRASP_ACTION);
00126 if(wait_attempts++ > MAX_WAIT_ATTEMPTS)
00127 {
00128 ROS_ERROR_STREAM("Grasp action service was not found");
00129 return false;
00130 }
00131 }
00132
00133 return true;
00134 }
00135
00136 bool MovePickPlaceServer::moveArmThroughPickSequence(const object_manipulation_msgs::PickupGoal &pickup_goal)
00137 {
00138
00139 geometry_msgs::PoseArray pick_pose_sequence;
00140 geometry_msgs::PoseArray temp_pick_sequence;
00141 GraspGoal grasp_goal;
00142
00143
00144 createPickupMoveSequence(pickup_goal,pick_pose_sequence);
00145 grasp_goal.grasp = pickup_goal.desired_grasps[0];
00146
00147
00148 temp_pick_sequence.poses.assign(1,pick_pose_sequence.poses.front());
00149 if(moveArm(temp_pick_sequence))
00150 {
00151 ROS_INFO_STREAM(ros::this_node::getName()<<": Pre-grasp Arm Move Achieved");
00152 }
00153 else
00154 {
00155 ROS_ERROR_STREAM(ros::this_node::getName()<<": Pre-grasp Arm Move Failed");
00156 return false;
00157 }
00158
00159
00160 grasp_goal.goal = GraspGoal::PRE_GRASP;
00161 grasp_action_client_ptr_->sendGoal(grasp_goal);
00162 if(grasp_action_client_ptr_->waitForResult(ros::Duration(DURATION_WAIT_GRASP_RESULT)))
00163 {
00164 ROS_INFO_STREAM(ros::this_node::getName()<<": Pre-graps Gripper Move Achieved");
00165 }
00166 else
00167 {
00168 ROS_ERROR_STREAM(ros::this_node::getName()<<": Pre-grasp Gripper Move Rejected with error flag: "
00169 <<(unsigned int)grasp_action_client_ptr_->getState().state_);
00170 return false;
00171 }
00172
00173
00174 temp_pick_sequence.poses.assign(1,*(pick_pose_sequence.poses.begin() + 1));
00175 if(moveArm(temp_pick_sequence))
00176 {
00177 ROS_INFO_STREAM(ros::this_node::getName()<<": Pick Arm Move Achieved");
00178 }
00179 else
00180 {
00181 ROS_ERROR_STREAM(ros::this_node::getName()<<": Pick Arm Move Failed");
00182 return false;
00183 }
00184
00185
00186 grasp_goal.goal = GraspGoal::GRASP;
00187 grasp_action_client_ptr_->sendGoal(grasp_goal);
00188 if(grasp_action_client_ptr_->waitForResult(ros::Duration(DURATION_WAIT_GRASP_RESULT)))
00189 {
00190 ROS_INFO_STREAM(ros::this_node::getName()<<": Grasp Gripper Move Achieved");
00191 }
00192 else
00193 {
00194 ROS_ERROR_STREAM(ros::this_node::getName()<<": Grasp Gripper Move Rejected with error flag: "
00195 <<(unsigned int)grasp_action_client_ptr_->getState().state_);
00196 return false;
00197 }
00198
00199
00200 temp_pick_sequence.poses.assign(1,pick_pose_sequence.poses.back());
00201 if(moveArm(temp_pick_sequence))
00202 {
00203 ROS_INFO_STREAM(ros::this_node::getName()<<": Lift Arm Moves Achieved");
00204 }
00205 else
00206 {
00207 ROS_ERROR_STREAM(ros::this_node::getName()<<": Lift Arm Move Failed");
00208 return false;
00209 }
00210
00211 return true;
00212 }
00213
00214 bool MovePickPlaceServer::moveArmThroughPlaceSequence(const object_manipulation_msgs::PlaceGoal &place_goal)
00215 {
00216
00217 geometry_msgs::PoseArray place_pose_sequence;
00218 geometry_msgs::PoseArray temp_place_sequence;
00219 GraspGoal grasp_goal;
00220
00221
00222 createPlaceMoveSequence(place_goal,place_pose_sequence);
00223 grasp_goal.grasp = place_goal.grasp;
00224
00225
00226 temp_place_sequence.poses.assign(place_pose_sequence.poses.begin(), place_pose_sequence.poses.end()-1);
00227 if(moveArm(temp_place_sequence))
00228 {
00229 ROS_INFO_STREAM(ros::this_node::getName()<<": Approach and Place Arm Moves Achieved");
00230 }
00231 else
00232 {
00233 ROS_ERROR_STREAM(ros::this_node::getName()<<": Approach and Place Arm Moves Failed");
00234 return false;
00235 }
00236
00237
00238 grasp_goal.goal = GraspGoal::RELEASE;
00239 grasp_action_client_ptr_->sendGoal(grasp_goal);
00240 if(grasp_action_client_ptr_->waitForResult(ros::Duration(DURATION_WAIT_GRASP_RESULT)))
00241 {
00242 ROS_INFO_STREAM(ros::this_node::getName()<<": Release Gripper Move Achieved");
00243 }
00244 else
00245 {
00246 ROS_ERROR_STREAM(ros::this_node::getName()<<": Release Gripper Move Rejected with error flag: "
00247 <<(unsigned int)grasp_action_client_ptr_->getState().state_);
00248 return false;
00249 }
00250
00251
00252 temp_place_sequence.poses.assign(1,place_pose_sequence.poses.back());
00253 if(moveArm(temp_place_sequence))
00254 {
00255 ROS_INFO_STREAM(ros::this_node::getName()<<": Retreat Arm Move Achieved");
00256 }
00257 else
00258 {
00259 ROS_ERROR_STREAM(ros::this_node::getName()<<": Retreat Arm Move Failed");
00260 return false;
00261 }
00262
00263 return true;
00264 }
00265
00266 void MovePickPlaceServer::pickupGoalCallback(PickupGoalHandle gh)
00267 {
00268 const object_manipulation_msgs::PickupGoal &goal = *(gh.getGoal());
00269 object_manipulation_msgs::PickupResult res;
00270
00271
00272 if(gh.getGoalStatus().status == GoalState::ACTIVE && pickup_gh_ == gh)
00273 {
00274
00275 ROS_WARN_STREAM("Pickup goal is already being processed, ignoring request");
00276 return;
00277 }
00278
00279
00280 pickup_gh_.getGoalStatus().status == GoalState::ACTIVE ? pickupCancelCallback(pickup_gh_): (void)NULL ;
00281 place_gh_.getGoalStatus().status == GoalState::ACTIVE ? placeCancelCallback(place_gh_) :(void) NULL ;
00282
00283
00284 pickup_gh_ = gh;
00285 gh.setAccepted("accepted");
00286
00287
00288 if(moveArmThroughPickSequence(goal))
00289 {
00290 res.manipulation_result.value = res.manipulation_result.SUCCESS;
00291 gh.setSucceeded(res,"Succeeded");
00292 }
00293 else
00294 {
00295 res.manipulation_result.value = res.manipulation_result.FAILED;
00296 gh.setAborted(res,"Failed");
00297 }
00298 }
00299
00300 void MovePickPlaceServer::pickupCancelCallback(PickupGoalHandle gh)
00301 {
00302 object_manipulation_msgs::PickupResult res;
00303
00304
00305 if(pickup_gh_ == gh && gh.getGoalStatus().status == GoalState::ACTIVE)
00306 {
00307
00308
00309
00310 if(move_arm_client_ptr_->getState().state_ == GoalState::ACTIVE)
00311 {
00312
00313 move_arm_client_ptr_->cancelGoal();
00314 }
00315
00316
00317 if(grasp_action_client_ptr_->getState().state_ == GoalState::ACTIVE)
00318 {
00319 grasp_action_client_ptr_->cancelGoal();
00320 }
00321
00322 res.manipulation_result.value = res.manipulation_result.CANCELLED;
00323 gh.setCanceled(res,"Canceled");
00324
00325 }
00326 }
00327
00328 void MovePickPlaceServer::placeGoalCallback(PlaceGoalHandle gh)
00329 {
00330 const object_manipulation_msgs::PlaceGoal &goal = *(gh.getGoal());
00331 object_manipulation_msgs::PlaceResult res;
00332
00333
00334
00335 if(gh.getGoalStatus().status == GoalState::ACTIVE && place_gh_ == gh)
00336 {
00337
00338 ROS_WARN_STREAM("Place goal is already being processed, ignoring request");
00339 return;
00340 }
00341
00342
00343 pickup_gh_.getGoalStatus().status == GoalState::ACTIVE ? pickupCancelCallback(pickup_gh_): (void)NULL ;
00344 place_gh_.getGoalStatus().status == GoalState::ACTIVE ? placeCancelCallback(place_gh_) : (void)NULL ;
00345
00346
00347 place_gh_ = gh;
00348 gh.setAccepted("accepted");
00349
00350
00351 if(moveArmThroughPlaceSequence(goal))
00352 {
00353 res.manipulation_result.value = res.manipulation_result.SUCCESS;
00354 gh.setSucceeded(res,"Succeeded");
00355 }
00356 else
00357 {
00358 res.manipulation_result.value = res.manipulation_result.FAILED;
00359 gh.setAborted(res,"Failed");
00360 }
00361 }
00362
00363 void MovePickPlaceServer::placeCancelCallback(PlaceGoalHandle gh)
00364 {
00365 object_manipulation_msgs::PlaceResult res;
00366
00367
00368 if(place_gh_ == gh && gh.getGoalStatus().status == GoalState::ACTIVE)
00369 {
00370
00371
00372
00373 if(move_arm_client_ptr_->getState().state_ == GoalState::ACTIVE)
00374 {
00375
00376 move_arm_client_ptr_->cancelGoal();
00377 }
00378
00379
00380 if(grasp_action_client_ptr_->getState().state_ == GoalState::ACTIVE)
00381 {
00382 grasp_action_client_ptr_->cancelGoal();
00383 }
00384
00385 res.manipulation_result.value = res.manipulation_result.CANCELLED;
00386 gh.setCanceled(res,"Canceled");
00387 }
00388 }
00389
00390 bool MovePickPlaceServer::createPickupMoveSequence(const object_manipulation_msgs::PickupGoal &goal,
00391 geometry_msgs::PoseArray &pickup_pose_sequence)
00392 {
00393 tf::Transform obj_to_tcp_tf, world_to_obj_tf, world_to_tcp_tf, tcp_to_pregrasp_tf,tcp_to_lift_tf;
00394 tf::Vector3 approach_dir;
00395 double approach_dist, lift_dist;
00396 const object_manipulation_msgs::Grasp &grasp = goal.desired_grasps[0];
00397 CartesianTrajectory traj;
00398
00399
00400 traj.arm_group_ = goal.arm_name;
00401 traj.frame_id_ = goal.target.reference_frame_id;
00402 traj.link_name_ = goal.lift.direction.header.frame_id;
00403
00404
00405 tf::poseMsgToTF(goal.target.potential_models[0].pose.pose,world_to_obj_tf);
00406 tf::poseMsgToTF(grasp.grasp_pose,obj_to_tcp_tf);
00407 world_to_tcp_tf = world_to_obj_tf * obj_to_tcp_tf;
00408
00409
00410 approach_dist =grasp.desired_approach_distance;
00411 lift_dist = goal.lift.desired_distance;
00412 tf::vector3MsgToTF(goal.lift.direction.vector,approach_dir);
00413 approach_dir.normalize();
00414
00415
00416 tcp_to_pregrasp_tf = tf::Transform(tf::Quaternion::getIdentity(),(-approach_dist) * approach_dir);
00417 tcp_to_lift_tf = tf::Transform(tf::Quaternion::getIdentity(),(-lift_dist) * approach_dir);
00418 traj.cartesian_points_.push_back(world_to_tcp_tf * tcp_to_pregrasp_tf );
00419 traj.cartesian_points_.push_back(world_to_tcp_tf);
00420 traj.cartesian_points_.push_back(world_to_tcp_tf * tcp_to_lift_tf);
00421
00422 return getTrajectoryInArmSpace(traj,pickup_pose_sequence);
00423 }
00424
00425 bool MovePickPlaceServer::createPlaceMoveSequence(const object_manipulation_msgs::PlaceGoal &goal,
00426 geometry_msgs::PoseArray &place_pose_sequence)
00427 {
00428 tf::Transform obj_to_tcp_tf, world_to_place_tf, world_to_tcp_tf, tcp_to_approach_tf,tcp_to_retreat_tf;
00429 tf::Vector3 approach_dir;
00430 double approach_dist, retreat_dist;
00431 const object_manipulation_msgs::Grasp &grasp = goal.grasp;
00432 CartesianTrajectory traj;
00433
00434
00435 traj.arm_group_ = goal.arm_name;
00436 traj.frame_id_ = goal.place_locations[0].header.frame_id;
00437 traj.link_name_ = goal.approach.direction.header.frame_id;
00438
00439
00440 tf::poseMsgToTF(goal.place_locations[0].pose,world_to_place_tf);
00441 tf::poseMsgToTF(grasp.grasp_pose,obj_to_tcp_tf);
00442 world_to_tcp_tf = world_to_place_tf * obj_to_tcp_tf;
00443
00444
00445 approach_dist =goal.approach.desired_distance;
00446 retreat_dist = goal.desired_retreat_distance;
00447 tf::vector3MsgToTF(goal.approach.direction.vector,approach_dir);
00448 approach_dir.normalize();
00449
00450
00451 tcp_to_approach_tf = tf::Transform(tf::Quaternion::getIdentity(),(-approach_dist) * approach_dir);
00452 tcp_to_retreat_tf = tf::Transform(tf::Quaternion::getIdentity(),(-retreat_dist) * approach_dir);
00453 traj.cartesian_points_.push_back(world_to_tcp_tf * tcp_to_approach_tf );
00454 traj.cartesian_points_.push_back(world_to_tcp_tf);
00455 traj.cartesian_points_.push_back(world_to_tcp_tf * tcp_to_retreat_tf);
00456
00457 return getTrajectoryInArmSpace(traj,place_pose_sequence);
00458 }