OperateHandleServer.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2010, Thomas Ruehr <ruehr@cs.tum.edu>
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
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     // create messages that are used to published feedback/result
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         // helper variables
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; //OperateHandleController::getHandlePoseFromMarker(arm,hgtIdx);
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         //int handle = OperateHandleController::operateHandle(arm,aMb);
00100 
00101         using namespace boost::posix_time;
00102 
00103         //time_duration td = hours(1) + seconds(10); //01:00:01
00104         time_duration td = seconds(1); //01:00:01
00105         //td = hours(1) + nanoseconds(5); //01:00:00.000000005
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                // set the action state to preempted
00113                as_.setPreempted();
00114                success = false;
00115                break;
00116             }
00117         }
00118 
00119         //static std::vector<tf::Stamped<tf::Pose> *> handlePoses;
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         //result_.
00138         // result_.trajectoryHandle = handle;
00139 
00140         if (success)
00141         {
00142             //result_.sequence = feedback_.sequence;
00143             ROS_INFO("%s: Succeeded", action_name_.c_str());
00144             // set the action state to succeeded
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     // create messages that are used to published feedback/result
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         // helper variables
00178         ros::Rate r(20);
00179         bool success = true;
00180 
00181         int arm  = goal->arm;
00182         //geometry_msgs::PoseStamped position = goal->position;
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         //openingTrajAct.resize(traj.poses.size());
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             //result_.sequence = feedback_.sequence;
00210             ROS_INFO("%s: Succeeded", action_name_.c_str());
00211             // set the action state to succeeded
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     // create messages that are used to published feedback/result
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         // helper variables
00247         ros::Rate r(20);
00248         bool success = true;
00249 
00250         // push_back the seeds for the fibonacci sequence
00251         // publish info to the console for the user
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         // start executing the action
00255         /*for(int i=1; i<=goal->order; i++)
00256         {
00257           // check that preempt has not been requested by the client
00258           if (as_.isPreemptRequested() || !ros::ok())
00259           {
00260             ROS_INFO("%s: Preempted", action_name_.c_str());
00261             // set the action state to preempted
00262             as_.setPreempted();
00263             success = false;
00264             break;
00265           }
00266           //feedback_.sequence.push_back(feedback_.sequence[i] + feedback_.sequence[i-1]);
00267           // publish the feedback
00268           //as_.publishFeedback(feedback_);
00269           // this sleep is not necessary, the sequence is computed at 1 Hz for demonstration purposes
00270           r.sleep();
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   // close the thing again, expects base to be in a good spot where good means it wont crush the door when driving from current to next closing position
00292         {
00293             OperateHandleController::close(0,hgtIdx);
00294             result_.trajectoryHandle = hgtIdx;
00295         }
00296 
00297         if (success)
00298         {
00299             //result_.sequence = feedback_.sequence;
00300             ROS_INFO("%s: Succeeded", action_name_.c_str());
00301             // set the action state to succeeded
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     // create messages that are used to published feedback/result
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         // helper variables
00335         ros::Rate r(20);
00336         bool success = true;
00337 
00338         // push_back the seeds for the fibonacci sequence
00339         // publish info to the console for the user
00340         ROS_INFO("%s: Executing, creating CloseHandleAction  arm %i handle %i ", action_name_.c_str(), goal->arm, goal->handle);
00341 
00342         //int arm  = goal->arm;
00343         int handle  = goal->handle;
00344         OperateHandleController::close(0,handle);
00345 
00346         result_.trajectoryHandle = handle;
00347 
00348         if (success)
00349         {
00350             //result_.sequence = feedback_.sequence;
00351             ROS_INFO("%s: Succeeded", action_name_.c_str());
00352             // set the action state to succeeded
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     // create messages that are used to published feedback/result
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         // helper variables
00387         ros::Rate r(20);
00388         bool success = true;
00389 
00390         // push_back the seeds for the fibonacci sequence
00391         // publish info to the console for the user
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             //RobotDriver::getInstance()->moveBase();
00405 
00406             RobotArm::getInstance(0)->tucked = true;
00407             double p[] = { 0.255, -0.571, -0.025, 1.000};
00408             //double p[] = { 0.255, -0.571, -0.108, 0.994 };
00409             RobotDriver::getInstance()->moveBase(p);
00410 
00411             //t2.join();t3.join();
00412 
00413             RobotHead::getInstance()->lookAt("/map", 1.243111, -0.728864, 0.9);
00414             tf::Stamped<tf::Pose> bottle = Perception3d::getBottlePose();
00415 
00416 
00417             {
00418                 //double fridgeLink = atof(argv[2]);
00419                 //double fridgeLink = .1;
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                 //p2.setOrigin(btVector3(1.165 - fridgeLink, -0.655, 1.151));
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                 //p3.setOrigin(btVector3(1.265000 - fridgeLink, -0.655000, 0.951000));
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                 //RobotArm::findBaseMovement(result, arm, goal,false);
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             //RobotArm::getInstance(1)->universal_move_toolframe_ik(1.065, -0.655, 1.151, 0.005, -0.053, -0.029, 0.998, "map");
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             //RobotArm::getInstance(1)->universal_move_toolframe_ik(1.265000, -0.655000, 0.951000, 0.005001, -0.053009, -0.029005, 0.998160, "map");
00471             //RobotArm::getInstance(1)->universal_move_toolframe_ik(1.2, -0.655000, 0.951000, 0.005001, -0.053009, -0.029005, 0.998160, "map");
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             //RobotArm::getInstance(1)->universal_move_toolframe_ik(1.065, -0.655, 1.151, 0.005, -0.053, -0.029, 0.998, "map");
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             //result_.sequence = feedback_.sequence;
00488             ROS_INFO("%s: Succeeded", action_name_.c_str());
00489             // set the action state to succeeded
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     // create messages that are used to published feedback/result
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         // helper variables
00524         ros::Rate r(20);
00525         bool success = true;
00526 
00527         // push_back the seeds for the fibonacci sequence
00528         // publish info to the console for the user
00529         ROS_INFO("%s: Executing, creating PickPlateAction ", action_name_.c_str());
00530 
00531         //int arm  = goal->arm;
00532         //int handle  = goal->handle;
00533         // in drawer
00534         RobotHead::getInstance()->lookAt("/map", .4 ,1.13, .7);
00535         // at sink
00536         //RobotHead::getInstance()->lookAt("/map",0.913379, 0.824588, 0.7);
00537 
00538         // at sink
00539         //double target[4];       target[0] =-0.048;       target[1] = 1.029;       target[2] = 0;       target[3] = 1;
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         //RobotArm::getInstance(0)->tucked = true;
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         //RobotArm::getInstance(1)->startTrajectory(RobotArm::getInstance(1)->twoPointTrajectory(Poses::prepDishL0,Poses::prepDishL1));
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         //in drawer
00566         RobotHead::getInstance()->lookAt("/map", .4 ,1.13, .7);
00567         // at sink
00568         //RobotHead::getInstance()->lookAt("/map",0.913379, 0.824588, 0.7);
00569 
00570         // 20 up 7 down from drawer to table
00571 
00572 
00573         //bin/ias_drawer_executi2 0 0.65, -0.4, 1.15 0.651, 0.295, -0.621, -0.322 ; bin/ias_drawer_executive -2 1 0.65, 0.4, 1.15 -0.295, 0.621, -0.322, 0.651
00574         //RobotArm::getInstance(0)->startTrajectory(RobotArm::getInstance(0)->twoPointTrajectory(Poses::prepDishR0,Poses::prepDishR1));
00575         //boost::thread t2(&RobotArm::startTrajectory, RobotArm::getInstance(0), RobotArm::getInstance(0)->twoPointTrajectory(Poses::prepDishR0,Poses::prepDishR1));
00576         //boost::thread t3(&RobotArm::startTrajectory, RobotArm::getInstance(1), RobotArm::getInstance(1)->twoPointTrajectory(Poses::prepDishL0,Poses::prepDishL1));
00577 
00578         //RobotArm::getInstance(0)->universal_move_toolframe_ik(0.65, -0.4, 1.15, 0.651, 0.295, -0.621, -0.322);
00579         //RobotArm::getInstance(1)->universal_move_toolframe_ik(0.65, 0.4, 1.15, -0.295, 0.621, -0.322, 0.651);
00580         //OperateHandleController::getPlate(atoi(argv[2]));
00581         OperateHandleController::getPlate(0);
00582 
00583         if (success)
00584         {
00585             //result_.sequence = feedback_.sequence;
00586             ROS_INFO("%s: Succeeded", action_name_.c_str());
00587             // set the action state to succeeded
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             //result_.sequence = feedback_.sequence;
00627             ROS_INFO("%s: Succeeded", action_name_.c_str());
00628             // set the action state to succeeded
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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


ias_drawer_executive
Author(s): Thomas Ruehr
autogenerated on Thu May 23 2013 09:44:57