00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include <ias_drawer_executive/OperateHandleController.h>
00031 #include <ias_drawer_executive/Torso.h>
00032 #include <ias_drawer_executive/RobotDriver.h>
00033 #include <ias_drawer_executive/Geometry.h>
00034 #include <ias_drawer_executive/Poses.h>
00035 #include <ias_drawer_executive/Perception3d.h>
00036 #include <ias_drawer_executive/RobotArm.h>
00037 #include <ias_drawer_executive/Pressure.h>
00038 #include <ias_drawer_executive/Gripper.h>
00039 #include <ias_drawer_executive/Head.h>
00040 #include <ias_drawer_executive/DemoScripts.h>
00041 #include <ias_drawer_executive/Current.h>
00042 #include <actionlib/server/simple_action_server.h>
00043
00044 #include <ias_drawer_executive/OperateHandleAction.h>
00045 #include <ias_drawer_executive/CloseHandleAction.h>
00046 #include <ias_drawer_executive/PickBottleAction.h>
00047 #include <ias_drawer_executive/PickPlateAction.h>
00048 #include <ias_drawer_executive/GenericAction.h>
00049
00050 #include <ias_drawer_executive/OpenContainerAction.h>
00051 #include <ias_drawer_executive/CloseContainerAction.h>
00052
00053 #include "boost/date_time/posix_time/posix_time.hpp"
00054
00055
00056 class OpenContainerAction
00057 {
00058 protected:
00059
00060 ros::NodeHandle nh_;
00061 actionlib::SimpleActionServer<ias_drawer_executive::OpenContainerAction> as_;
00062 std::string action_name_;
00063
00064 ias_drawer_executive::OpenContainerFeedback feedback_;
00065 ias_drawer_executive::OpenContainerResult result_;
00066
00067 public:
00068
00069 OpenContainerAction(std::string name) :
00070 as_(nh_, name, boost::bind(&OpenContainerAction::executeCB, this, _1)),
00071 action_name_(name)
00072 {
00073 ROS_INFO("%s up and running..", action_name_.c_str());
00074 }
00075
00076 ~OpenContainerAction(void)
00077 {
00078 }
00079
00080 void executeCB(const ias_drawer_executive::OpenContainerGoalConstPtr &goal)
00081 {
00082
00083 ros::Rate r(20);
00084 bool success = true;
00085
00086 int arm = goal->arm;
00087 geometry_msgs::PoseStamped position = goal->position;
00088
00089 tf::Stamped<tf::Pose> aM;
00090 aM.frame_id_ = position.header.frame_id;
00091 aM.setOrigin(btVector3(position.pose.position.x, position.pose.position.y, position.pose.position.z));
00092 aM.setRotation(btQuaternion(position.pose.orientation.x,position.pose.orientation.y,position.pose.orientation.z,position.pose.orientation.w));
00093 tf::Stamped<tf::Pose> aMb;
00094 aMb = Geometry::getPoseIn("base_link",aM);
00095
00096 int handle = OperateHandleController::maxHandle + 1;
00097 boost::thread t = boost::thread(&OperateHandleController::operateHandle,arm,aMb,0);
00098
00099
00100
00101 using namespace boost::posix_time;
00102
00103
00104 time_duration td = seconds(1);
00105
00106
00107 while (!t.timed_join(td) && ros::ok()) {
00108
00109 if (as_.isPreemptRequested() || !ros::ok())
00110 {
00111 ROS_INFO("%s: Preempted", action_name_.c_str());
00112
00113 as_.setPreempted();
00114 success = false;
00115 break;
00116 }
00117 }
00118
00119
00120 result_.trajectory.poses.resize(OperateHandleController::openingTraj[handle].size());
00121 result_.trajectory.header.frame_id = "map";
00122
00123 std::vector<tf::Stamped<tf::Pose> *>::iterator it;
00124 int i = 0;
00125 for (it = OperateHandleController::openingTraj[handle].begin(); it != OperateHandleController::openingTraj[handle].end(); ++it, ++i)
00126 {
00127 geometry_msgs::Pose curr;
00128 curr.position.x=(*it)->getOrigin().x();
00129 curr.position.y=(*it)->getOrigin().y();
00130 curr.position.z=(*it)->getOrigin().z();
00131 curr.orientation.x=(*it)->getRotation().x();
00132 curr.orientation.y=(*it)->getRotation().y();
00133 curr.orientation.z=(*it)->getRotation().z();
00134 curr.orientation.w=(*it)->getRotation().w();
00135 result_.trajectory.poses[i] = curr;
00136 }
00137
00138
00139
00140 if (success)
00141 {
00142
00143 ROS_INFO("%s: Succeeded", action_name_.c_str());
00144
00145 as_.setSucceeded(result_);
00146 }
00147 }
00148 };
00149
00150
00151 class CloseContainerAction
00152 {
00153 protected:
00154
00155 ros::NodeHandle nh_;
00156 actionlib::SimpleActionServer<ias_drawer_executive::CloseContainerAction> as_;
00157 std::string action_name_;
00158
00159 ias_drawer_executive::CloseContainerFeedback feedback_;
00160 ias_drawer_executive::CloseContainerResult result_;
00161
00162 public:
00163
00164 CloseContainerAction(std::string name) :
00165 as_(nh_, name, boost::bind(&CloseContainerAction::executeCB, this, _1)),
00166 action_name_(name)
00167 {
00168 ROS_INFO("%s up and running..", action_name_.c_str());
00169 }
00170
00171 ~CloseContainerAction(void)
00172 {
00173 }
00174
00175 void executeCB(const ias_drawer_executive::CloseContainerGoalConstPtr &goal)
00176 {
00177
00178 ros::Rate r(20);
00179 bool success = true;
00180
00181 int arm = goal->arm;
00182
00183 geometry_msgs::PoseArray traj = goal->opening_trajectory;
00184
00185 int handle = ++OperateHandleController::maxHandle;
00186 ROS_INFO("handle : %i", handle);
00187 std::vector<tf::Stamped<tf::Pose> * > openingTrajAct;
00188 OperateHandleController::openingTraj.push_back(openingTrajAct);
00189
00190 OperateHandleController::openingTraj[handle].resize(traj.poses.size());
00191 ROS_INFO("OHC trajec size %zu" , OperateHandleController::openingTraj.size());
00192 ROS_INFO("OHC trajec[handle] size %zu" , OperateHandleController::openingTraj[handle].size());
00193 ROS_INFO("traj size %zu" , traj.poses.size());
00194 for (size_t i=0; i < traj.poses.size(); ++i) {
00195 tf::Stamped<tf::Pose> *pose = new tf::Stamped<tf::Pose>();
00196 pose->frame_id_ = "map";
00197 pose->setOrigin(btVector3(traj.poses[i].position.x, traj.poses[i].position.y, traj.poses[i].position.z));
00198 pose->setRotation(btQuaternion(traj.poses[i].orientation.x,traj.poses[i].orientation.y,traj.poses[i].orientation.z,traj.poses[i].orientation.w));
00199
00200 ROS_INFO("TRAJ PT %zu: %f %f %f, %f %f %f %f",i ,traj.poses[i].position.x, traj.poses[i].position.y, traj.poses[i].position.z,
00201 traj.poses[i].orientation.x,traj.poses[i].orientation.y,traj.poses[i].orientation.z,traj.poses[i].orientation.w);
00202 OperateHandleController::openingTraj[handle][i] = pose;
00203 }
00204
00205 OperateHandleController::close(arm,handle);
00206
00207 if (success)
00208 {
00209
00210 ROS_INFO("%s: Succeeded", action_name_.c_str());
00211
00212 as_.setSucceeded(result_);
00213 }
00214 }
00215 };
00216
00217
00218
00219
00220 class OperateHandleAction
00221 {
00222 protected:
00223
00224 ros::NodeHandle nh_;
00225 actionlib::SimpleActionServer<ias_drawer_executive::OperateHandleAction> as_;
00226 std::string action_name_;
00227
00228 ias_drawer_executive::OperateHandleFeedback feedback_;
00229 ias_drawer_executive::OperateHandleResult result_;
00230
00231 public:
00232
00233 OperateHandleAction(std::string name) :
00234 as_(nh_, name, boost::bind(&OperateHandleAction::executeCB, this, _1)),
00235 action_name_(name)
00236 {
00237 ROS_INFO("%s up and running..", action_name_.c_str());
00238 }
00239
00240 ~OperateHandleAction(void)
00241 {
00242 }
00243
00244 void executeCB(const ias_drawer_executive::OperateHandleGoalConstPtr &goal)
00245 {
00246
00247 ros::Rate r(20);
00248 bool success = true;
00249
00250
00251
00252 ROS_INFO("%s: Executing, creating OperateHandleAction arm %i posIdx %i heightIdx %i", action_name_.c_str(), goal->arm, goal->positionIdx, goal->heightIdx);
00253
00254
00255
00256
00257
00258
00259
00260
00261
00262
00263
00264
00265
00266
00267
00268
00269
00270
00271
00272
00273 int arm = goal->arm;
00274 int posIdx = goal->positionIdx;
00275 int hgtIdx = goal->heightIdx;
00276
00277 if (posIdx >= 0)
00278 {
00279 Torso *torso = Torso::getInstance();
00280 boost::thread t = boost::thread((posIdx == 8) ? &Torso::up : &Torso::down, torso);
00281 ROS_INFO("TARGET POSE IN MAP %f %f %f %f",Poses::poses[posIdx][0],Poses::poses[posIdx][1],Poses::poses[posIdx][2],Poses::poses[posIdx][3]);
00282 RobotDriver::getInstance()->moveBase(Poses::poses[posIdx]);
00283 t.join();
00284 }
00285 if (hgtIdx >= 0)
00286 {
00287 tf::Stamped<tf::Pose> aM = OperateHandleController::getHandlePoseFromMarker(arm,hgtIdx);
00288 int handle = OperateHandleController::operateHandle(arm,aM);
00289 result_.trajectoryHandle = handle;
00290 }
00291 else
00292 {
00293 OperateHandleController::close(0,hgtIdx);
00294 result_.trajectoryHandle = hgtIdx;
00295 }
00296
00297 if (success)
00298 {
00299
00300 ROS_INFO("%s: Succeeded", action_name_.c_str());
00301
00302 as_.setSucceeded(result_);
00303 }
00304 }
00305 };
00306
00307
00308 class CloseHandleAction
00309 {
00310 protected:
00311
00312 ros::NodeHandle nh_;
00313 actionlib::SimpleActionServer<ias_drawer_executive::CloseHandleAction> as_;
00314 std::string action_name_;
00315
00316 ias_drawer_executive::CloseHandleFeedback feedback_;
00317 ias_drawer_executive::CloseHandleResult result_;
00318
00319 public:
00320
00321 CloseHandleAction(std::string name) :
00322 as_(nh_, name, boost::bind(&CloseHandleAction::executeCB, this, _1)),
00323 action_name_(name)
00324 {
00325 ROS_INFO("%s up and running..", action_name_.c_str());
00326 }
00327
00328 ~CloseHandleAction(void)
00329 {
00330 }
00331
00332 void executeCB(const ias_drawer_executive::CloseHandleGoalConstPtr &goal)
00333 {
00334
00335 ros::Rate r(20);
00336 bool success = true;
00337
00338
00339
00340 ROS_INFO("%s: Executing, creating CloseHandleAction arm %i handle %i ", action_name_.c_str(), goal->arm, goal->handle);
00341
00342
00343 int handle = goal->handle;
00344 OperateHandleController::close(0,handle);
00345
00346 result_.trajectoryHandle = handle;
00347
00348 if (success)
00349 {
00350
00351 ROS_INFO("%s: Succeeded", action_name_.c_str());
00352
00353 as_.setSucceeded(result_);
00354 }
00355 }
00356 };
00357
00358
00359
00360 class PickBottleAction
00361 {
00362 protected:
00363
00364 ros::NodeHandle nh_;
00365 actionlib::SimpleActionServer<ias_drawer_executive::PickBottleAction> as_;
00366 std::string action_name_;
00367
00368 ias_drawer_executive::PickBottleFeedback feedback_;
00369 ias_drawer_executive::PickBottleResult result_;
00370
00371 public:
00372
00373 PickBottleAction(std::string name) :
00374 as_(nh_, name, boost::bind(&PickBottleAction::executeCB, this, _1)),
00375 action_name_(name)
00376 {
00377 ROS_INFO("%s up and running..", action_name_.c_str());
00378 }
00379
00380 ~PickBottleAction(void)
00381 {
00382 }
00383
00384 void executeCB(const ias_drawer_executive::PickBottleGoalConstPtr &goal)
00385 {
00386
00387 ros::Rate r(20);
00388 bool success = true;
00389
00390
00391
00392 ROS_INFO("%s: Executing, creating CloseHandleAction arm %i ", action_name_.c_str(), goal->arm);
00393
00394 int arm_ = goal->arm;
00395
00396 {
00397
00398 pr2_controllers_msgs::JointTrajectoryGoal goalB = RobotArm::getInstance(1)->twoPointTrajectory(Poses::prepDishL1,Poses::prepDishL1);
00399 boost::thread t3(&RobotArm::startTrajectory, RobotArm::getInstance(1), goalB,true);
00400
00401 Torso *torso = Torso::getInstance();
00402 boost::thread t2(&Torso::up, torso);
00403
00404
00405
00406 RobotArm::getInstance(0)->tucked = true;
00407 double p[] = { 0.255, -0.571, -0.025, 1.000};
00408
00409 RobotDriver::getInstance()->moveBase(p);
00410
00411
00412
00413 RobotHead::getInstance()->lookAt("/map", 1.243111, -0.728864, 0.9);
00414 tf::Stamped<tf::Pose> bottle = Perception3d::getBottlePose();
00415
00416
00417 {
00418
00419
00420
00421
00422 std::vector<int> arm;
00423 std::vector<tf::Stamped<tf::Pose> > goal;
00424 tf::Stamped<tf::Pose> result;
00425
00426 tf::Stamped<tf::Pose> p0;
00427 p0.frame_id_="map";
00428 p0.stamp_=ros::Time();
00429 p0.setOrigin(btVector3(0.8, -0.455, 1.251));
00430 p0.setRotation(btQuaternion(0.005, -0.053, -0.029, 0.998));
00431 goal.push_back(p0);
00432 arm.push_back(arm_);
00433
00434 tf::Stamped<tf::Pose> p1;
00435 p1.frame_id_="map";
00436 p1.stamp_=ros::Time();
00437 p1.setOrigin(btVector3(0.8, -0.655, 1.251));
00438 p1.setRotation(btQuaternion(0.005, -0.053, -0.029, 0.998));
00439 goal.push_back(p1);
00440 arm.push_back(arm_);
00441
00442 tf::Stamped<tf::Pose> p2;
00443 p2.frame_id_="map";
00444 p2.stamp_=ros::Time();
00445
00446 p2.setOrigin(bottle.getOrigin() + btVector3(-.1,0,.03));
00447 p2.setRotation(btQuaternion(0.005, -0.053, -0.029, 0.998));
00448 goal.push_back(p2);
00449 arm.push_back(arm_);
00450
00451 tf::Stamped<tf::Pose> p3;
00452 p3.frame_id_="map";
00453 p3.stamp_=ros::Time();
00454
00455 p3.setOrigin(bottle.getOrigin() + btVector3(.02,0,.03));
00456 p3.setRotation(btQuaternion(0.005001, -0.053009, -0.029005, 0.998160));
00457 goal.push_back(p3);
00458 arm.push_back(arm_);
00459
00460 RobotArm::findBaseMovement(result, arm, goal,true, true);
00461
00462 }
00463
00464 Gripper::getInstance(arm_)->open();
00465 RobotArm::getInstance(arm_)->universal_move_toolframe_ik(0.8, -0.455, 1.251, 0.005, -0.053, -0.029, 0.998, "map");
00466 Gripper::getInstance(arm_)->updatePressureZero();
00467 RobotArm::getInstance(arm_)->universal_move_toolframe_ik(0.8, -0.655, 1.251, 0.005, -0.053, -0.029, 0.998, "map");
00468
00469 RobotArm::getInstance(arm_)->universal_move_toolframe_ik(bottle.getOrigin().x() - .1, bottle.getOrigin().y(), bottle.getOrigin().z() + .03, 0.005, -0.053, -0.029005, 0.998160, "map");
00470
00471
00472 RobotArm::getInstance(arm_)->universal_move_toolframe_ik(bottle.getOrigin().x(), bottle.getOrigin().y(), bottle.getOrigin().z() + .03, 0.005, -0.053, -0.029005, 0.998160, "map");
00473
00474 Gripper::getInstance(arm_)->closeCompliant();
00475 Gripper::getInstance(arm_)->close(0.04);
00476
00477 RobotArm::getInstance(arm_)->universal_move_toolframe_ik(bottle.getOrigin().x() - .1, bottle.getOrigin().y(), bottle.getOrigin().z() + .03, 0.005, -0.053, -0.029005, 0.998160, "map");
00478 RobotArm::getInstance(arm_)->universal_move_toolframe_ik(0.8, -0.655, 1.251, 0.005, -0.053, -0.029, 0.998, "map");
00479 RobotArm::getInstance(arm_)->universal_move_toolframe_ik(0.8, -0.455, 1.251, 0.005, -0.053, -0.029, 0.998, "map");
00480 }
00481
00482
00483 result_.ok = 1;
00484
00485 if (success)
00486 {
00487
00488 ROS_INFO("%s: Succeeded", action_name_.c_str());
00489
00490 as_.setSucceeded(result_);
00491 }
00492 }
00493 };
00494
00495
00496
00497 class PickPlateAction
00498 {
00499 protected:
00500
00501 ros::NodeHandle nh_;
00502 actionlib::SimpleActionServer<ias_drawer_executive::PickPlateAction> as_;
00503 std::string action_name_;
00504
00505 ias_drawer_executive::PickPlateFeedback feedback_;
00506 ias_drawer_executive::PickPlateResult result_;
00507
00508 public:
00509
00510 PickPlateAction(std::string name) :
00511 as_(nh_, name, boost::bind(&PickPlateAction::executeCB, this, _1)),
00512 action_name_(name)
00513 {
00514 ROS_INFO("%s up and running..", action_name_.c_str());
00515 }
00516
00517 ~PickPlateAction(void)
00518 {
00519 }
00520
00521 void executeCB(const ias_drawer_executive::PickPlateGoalConstPtr &goal)
00522 {
00523
00524 ros::Rate r(20);
00525 bool success = true;
00526
00527
00528
00529 ROS_INFO("%s: Executing, creating PickPlateAction ", action_name_.c_str());
00530
00531
00532
00533
00534 RobotHead::getInstance()->lookAt("/map", .4 ,1.13, .7);
00535
00536
00537
00538
00539
00540 double target[] = {-0.129, 1.017, 0.018, 1.000};
00541 ROS_INFO("POSE IN BASE %f %f %f", target[0],target[1], target[2]);
00542
00543
00544 Torso *torso = Torso::getInstance();
00545 boost::thread t = boost::thread(&Torso::up, torso);
00546
00547
00548
00549 pr2_controllers_msgs::JointTrajectoryGoal goalA = RobotArm::getInstance(0)->twoPointTrajectory(Poses::prepDishR1,Poses::prepDishR1);
00550 pr2_controllers_msgs::JointTrajectoryGoal goalB = RobotArm::getInstance(1)->twoPointTrajectory(Poses::prepDishL1,Poses::prepDishL1);
00551 boost::thread t2(&RobotArm::startTrajectory, RobotArm::getInstance(0), goalA,true);
00552 boost::thread t3(&RobotArm::startTrajectory, RobotArm::getInstance(1), goalB,true);
00553
00554 t2.join();
00555 t3.join();
00556
00557 RobotArm::getInstance(0)->tucked = true;
00558 RobotDriver::getInstance()->moveBase(target,false);
00559
00560 int sysret = system("rosrun dynamic_reconfigure dynparam set /camera_synchronizer_node projector_mode 1");
00561 ROS_INFO("sytem call: switch projector on: %i", sysret);
00562
00563 t.join();
00564
00565
00566 RobotHead::getInstance()->lookAt("/map", .4 ,1.13, .7);
00567
00568
00569
00570
00571
00572
00573
00574
00575
00576
00577
00578
00579
00580
00581 OperateHandleController::getPlate(0);
00582
00583 if (success)
00584 {
00585
00586 ROS_INFO("%s: Succeeded", action_name_.c_str());
00587
00588 as_.setSucceeded(result_);
00589 }
00590 }
00591 };
00592
00593 class GenericAction
00594 {
00595 protected:
00596
00597 ros::NodeHandle nh_;
00598 actionlib::SimpleActionServer<ias_drawer_executive::GenericAction> as_;
00599 std::string action_name_;
00600 ias_drawer_executive::GenericFeedback feedback_;
00601 ias_drawer_executive::GenericResult result_;
00602
00603 int (*fn)(int);
00604
00605 public:
00606
00607 GenericAction(std::string name, int (*pt2Func)(int)) :
00608 as_(nh_, name, boost::bind(&GenericAction::executeCB, this, _1)),
00609 action_name_(name)
00610 {
00611 ROS_INFO("%s up and running..", action_name_.c_str());
00612 fn = pt2Func;
00613 }
00614
00615 ~GenericAction(void)
00616 {
00617 }
00618
00619 void executeCB(const ias_drawer_executive::GenericGoalConstPtr &goal)
00620 {
00621
00622 bool success=true;
00623 result_.handle = fn(goal->arm);
00624 if (success)
00625 {
00626
00627 ROS_INFO("%s: Succeeded", action_name_.c_str());
00628
00629 as_.setSucceeded(result_);
00630 }
00631 }
00632 };
00633
00634
00635 int main(int argc, char** argv)
00636 {
00637 ros::init(argc, argv, "operate_handle_action");
00638
00639 OperateHandleAction opHandle("operate_handle_action");
00640 CloseHandleAction clHandle("close_handle_action");
00641 PickBottleAction puBottle("pick_bottle_action");
00642 PickPlateAction pPlate("pick_plate_action");
00643
00644
00645 GenericAction openFridge("open_fridge", &DemoScripts::openFridge);
00646
00647 GenericAction takeBottle("take_bottle", &DemoScripts::takeBottle);
00648
00649 GenericAction closeFridge("close_fridge", &DemoScripts::closeFridge);
00650
00651 GenericAction serveBottle("serve_bottle", &DemoScripts::serveBottle);
00652
00653 GenericAction openDrawer("open_drawer", &DemoScripts::openDrawer);
00654
00655 GenericAction takePlate("take_drawer", &DemoScripts::takePlate);
00656
00657 GenericAction servePlateToIsland("serve_plate_to_island", &DemoScripts::servePlateToIsland);
00658
00659 GenericAction takeSilverware("take_silverware", &DemoScripts::takeSilverware);
00660
00661 GenericAction serveToTable("serve_to_table", &DemoScripts::serveToTable);
00662
00663 GenericAction takePlateFromIsland("take_plate_from_island", &DemoScripts::takePlateFromIsland);
00664
00665
00666
00668 GenericAction getPotOut_openDrawer("get_pot_out_open_drawer",&Current::getPotOut_openDrawer);
00669
00670 GenericAction getPotOut_pickPlacePot("get_pot_out_pick_place_pot",&Current::getPotOut_pickPlacePot);
00671
00672 GenericAction manipulateKnob("manipulate_knob",&Current::manipulateKnob);
00673
00674 GenericAction openDrawerUnderOven("open_drawer_under_oven",&Current::openDrawerUnderOven);
00675
00676 GenericAction getLidOut("get_lid_out",&Current::getLidOut);
00677
00678 GenericAction getBowlOut("get_bowl_out",&Current::getBowlOut);
00679
00680 GenericAction closeDrawerUnderOven("close_drawer_under_oven",&Current::closeDrawerUnderOven);
00681
00682 GenericAction pourPopCorn("pour_popcorn",&Current::pourPopCorn);
00683
00684 GenericAction putLidOn("put_lid_on",&Current::putLidOn);
00685
00686 GenericAction removeLid("remove_lid",&Current::removeLid);
00687
00688 GenericAction takePotFromIsland("take_pot_from_island",&Current::takePotFromIsland);
00689
00690 GenericAction pourReadyPopcorn("pour_ready_popcorn",&Current::pourReadyPopcorn);
00691
00692
00693 OpenContainerAction openContainer("open_container_action");
00694 CloseContainerAction closeContainer("close_container_action");
00695
00696 ros::spin();
00697
00698 return 0;
00699 }