move_pr2.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <pr2_controllers_msgs/JointTrajectoryAction.h>
00003 #include <actionlib/client/simple_action_client.h>
00004 #include <pr2_controllers_msgs/Pr2GripperCommandAction.h>
00005 #include <gazebo_msgs/BodyRequest.h>
00006 #include <gazebo_msgs/ApplyBodyWrench.h>
00007 #include <gazebo_msgs/SetModelConfiguration.h>
00008 #include <std_srvs/Empty.h>
00009 #include <pr2_mechanism_msgs/SwitchController.h>
00010 #include <pr2_mechanism_msgs/ListControllers.h>
00011 
00012 typedef actionlib::SimpleActionClient< pr2_controllers_msgs::JointTrajectoryAction > TrajClient;
00013 
00014 class RobotArm
00015 {
00016 private:
00017   // Action client for the joint trajectory action 
00018   // used to trigger the arm movement action
00019   TrajClient* traj_client_;
00020 
00021 public:
00023   RobotArm() 
00024   {
00025     // tell the action client that we want to spin a thread by default
00026     traj_client_ = new TrajClient("r_arm_controller/joint_trajectory_action", true);
00027 
00028     // wait for action server to come up
00029     while(!traj_client_->waitForServer(ros::Duration(5.0))){
00030       ROS_INFO("Waiting for the joint_trajectory_action server");
00031     }
00032   }
00033 
00035   ~RobotArm()
00036   {
00037     delete traj_client_;
00038   }
00039 
00041   void startTrajectory(pr2_controllers_msgs::JointTrajectoryGoal goal)
00042   {
00043     // When to start the trajectory: 1s from now
00044     //goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(3.0);
00045     ros::Duration wait_time(20.0);
00046     actionlib::SimpleClientGoalState state = traj_client_->sendGoalAndWait(goal,wait_time,wait_time);
00047     if (state == actionlib::SimpleClientGoalState::SUCCEEDED)
00048       ROS_INFO("The arm traj finished with state [%s]",state.toString().c_str());
00049     else
00050       ROS_INFO("The arm traj failed with state [%s]",state.toString().c_str());
00051   }
00052 
00054 
00059   pr2_controllers_msgs::JointTrajectoryGoal armExtensionTrajectory()
00060   {
00061     //our goal variable
00062     pr2_controllers_msgs::JointTrajectoryGoal goal;
00063 
00064     // First, the joint names, which apply to all waypoints
00065     goal.trajectory.joint_names.push_back("r_shoulder_pan_joint");
00066     goal.trajectory.joint_names.push_back("r_shoulder_lift_joint");
00067     goal.trajectory.joint_names.push_back("r_upper_arm_roll_joint");
00068     goal.trajectory.joint_names.push_back("r_elbow_flex_joint");
00069     goal.trajectory.joint_names.push_back("r_forearm_roll_joint");
00070     goal.trajectory.joint_names.push_back("r_wrist_flex_joint");
00071     goal.trajectory.joint_names.push_back("r_wrist_roll_joint");
00072 
00073     // We will have two waypoints in this goal trajectory
00074     goal.trajectory.points.resize(2);
00075 
00076     // First trajectory point
00077     // Positions
00078     int ind = 0;
00079     goal.trajectory.points[ind].positions.resize(7);
00080     goal.trajectory.points[ind].positions[0] = 0.0;
00081     goal.trajectory.points[ind].positions[1] = 0.0;
00082     goal.trajectory.points[ind].positions[2] = 0.0;
00083     goal.trajectory.points[ind].positions[3] = 0.0;
00084     goal.trajectory.points[ind].positions[4] = 0.0;
00085     goal.trajectory.points[ind].positions[5] = 0.0;
00086     goal.trajectory.points[ind].positions[6] = 0.0;
00087     // To be reached 1 second after starting along the trajectory
00088     goal.trajectory.points[ind].time_from_start = ros::Duration(3.0);
00089 
00090     // Second trajectory point
00091     // Positions
00092     ind += 1;
00093     goal.trajectory.points[ind].positions.resize(7);
00094     goal.trajectory.points[ind].positions[0] = 0.0;
00095     goal.trajectory.points[ind].positions[1] = 0.25;
00096     goal.trajectory.points[ind].positions[2] = 0.0;
00097     goal.trajectory.points[ind].positions[3] = 0.0;
00098     goal.trajectory.points[ind].positions[4] = 0.0;
00099     goal.trajectory.points[ind].positions[5] = 0.0;
00100     goal.trajectory.points[ind].positions[6] = 0.0;
00101     // To be reached 2 seconds after starting along the trajectory
00102     goal.trajectory.points[ind].time_from_start = ros::Duration(6.0);
00103 
00104     //we are done; return the goal
00105     return goal;
00106   }
00107 
00109   actionlib::SimpleClientGoalState getState()
00110   {
00111     return traj_client_->getState();
00112   }
00113  
00114 };
00115 
00116 class PR2Controllers
00117 {
00118 
00119    PR2Controllers()
00120    {
00121    }
00122 
00123    ~PR2Controllers()
00124    {
00125    }
00126 
00127    void waitForControllers()
00128    {
00129    }
00130 
00131    void stopControllers()
00132    {
00133    }
00134 
00135    void startControllers()
00136    {
00137    }
00138 
00139 
00140 };
00141 
00142 
00143 
00144 // Our Action interface type, provided as a typedef for convenience
00145 typedef actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction> GripperClient;
00146 
00147 class Gripper{
00148 private:
00149   GripperClient* gripper_client_;  
00150 
00151 public:
00152   //Action client initialization
00153   Gripper(std::string action_topic){
00154 
00155     //Initialize the client for the Action interface to the gripper controller
00156     //and tell the action client that we want to spin a thread by default
00157     gripper_client_ = new GripperClient(action_topic, true);
00158     
00159     //wait for the gripper action server to come up 
00160     while(!gripper_client_->waitForServer(ros::Duration(60.0))){
00161       ROS_INFO("Waiting for the r_gripper_controller/gripper_action action server to come up");
00162     }
00163   }
00164 
00165   ~Gripper(){
00166     delete gripper_client_;
00167   }
00168 
00169   //Open the gripper
00170   void open(){
00171     pr2_controllers_msgs::Pr2GripperCommandGoal open;
00172     open.command.position = 0.08;
00173     open.command.max_effort = 150.0;  // use -1 to not limit effort (negative)
00174     
00175     ROS_INFO("Sending open goal");
00176 
00177     ROS_WARN("cancelling all gripper actionclient goals");
00178     gripper_client_->cancelAllGoals();
00179 
00180     bool success = false;
00181     while (!success)
00182     {
00183       actionlib::SimpleClientGoalState state =
00184         gripper_client_->sendGoalAndWait(open,ros::Duration(50.0),ros::Duration(30.0));
00185       bool finished_before_timeout = gripper_client_->waitForResult(ros::Duration(50.0));
00186 
00187       if (finished_before_timeout)
00188       {
00189         ROS_INFO("The gripper opened.");
00190         success = true;
00191       }
00192       else
00193       {
00194         ROS_WARN("The gripper failed to open [%s].",gripper_client_->getState().toString().c_str());
00195       }
00196 
00197       // final check if goal reached (doesn't seem to work)
00198       // success = (gripper_client_->getState() != actionlib::SimpleClientGoalState::SUCCEEDED);
00199       // ROS_ERROR("state [%d] success [%d]", gripper_client_->getState().state_, success);
00200     }
00201   }
00202 
00203   //Close the gripper
00204   void close(){
00205     pr2_controllers_msgs::Pr2GripperCommandGoal squeeze;
00206     squeeze.command.position = 0.0;
00207     squeeze.command.max_effort = 50.0;  // Close gently
00208     
00209     ROS_INFO("Sending squeeze goal");
00210     if (gripper_client_->sendGoalAndWait(squeeze,ros::Duration(5.0),ros::Duration(3.0))
00211         == actionlib::SimpleClientGoalState::SUCCEEDED)
00212       ROS_INFO("The gripper closed!");
00213     else
00214       ROS_INFO("The gripper failed to close.");
00215     gripper_client_->sendGoal(squeeze);
00216   }
00217 };
00218 
00219 int main(int argc, char** argv)
00220 {
00221   // Init the ROS node
00222   ros::init(argc, argv, "robot_driver");
00223   ros::NodeHandle rh("");
00224   ros::Duration(1e-9).sleep();
00225 
00226   // wait for services
00227   std::string pause_srv_name = "/gazebo/pause_physics";
00228   ros::service::waitForService(pause_srv_name);
00229 
00230   std::string unpause_srv_name = "/gazebo/unpause_physics";
00231   ros::service::waitForService(unpause_srv_name);
00232 
00233   std::string clear_body_wrenches_srv_name = "/gazebo/clear_body_wrenches";
00234   ros::service::waitForService(clear_body_wrenches_srv_name);
00235 
00236   Gripper r_gripper("r_gripper_controller/gripper_action");
00237   Gripper l_gripper("l_gripper_controller/gripper_action");
00238 
00239   std::string switch_controller_srv_name = "/pr2_controller_manager/switch_controller";
00240   ros::service::waitForService(switch_controller_srv_name);
00241 
00242   std::string list_controllers_srv_name = "/pr2_controller_manager/list_controllers";
00243   ros::service::waitForService(list_controllers_srv_name);
00244 
00245   // wait for pr2 controllers
00246   ros::ServiceClient list_controller_client = rh.serviceClient<pr2_mechanism_msgs::ListControllers>(list_controllers_srv_name);
00247   pr2_mechanism_msgs::ListControllers list_controller;
00248   int controllers_count = 0;
00249   while (controllers_count != 8)
00250   {
00251     controllers_count = 0;
00252     list_controller_client.call(list_controller);
00253     for (unsigned int i = 0; i < list_controller.response.controllers.size() ; ++i)
00254     {
00255       std::string name = list_controller.response.controllers[i];
00256       std::string state = list_controller.response.state[i];
00257       if ((name == "r_gripper_controller" || name == "l_gripper_controller" ||
00258           name == "laser_tilt_controller" || name == "head_traj_controller" ||
00259           name == "r_arm_controller" || name == "l_arm_controller" ||
00260           name == "base_controller" || name == "torso_controller") && state == "running")
00261         controllers_count++;
00262       ROS_INFO("controller [%s] state [%s] count[%d]",name.c_str(), state.c_str(),controllers_count);
00263     }
00264   }
00265 
00266   // clear object wrench
00267   /*
00268   ros::ServiceClient clear_client = rh.serviceClient<gazebo_msgs::BodyRequest>(clear_body_wrenches_srv_name);
00269   gazebo_msgs::BodyRequest body_request;
00270   body_request.request.body_name = "object_1::coke_can";
00271   clear_client.call(body_request);
00272   */
00273 
00274   // open gripper
00275   r_gripper.open();
00276   r_gripper.open(); // only works after two calls
00277   //l_gripper.open();
00278 
00279   ROS_INFO("stopping controllers");
00280   // stop arm controller
00281   ros::ServiceClient switch_controller_client = rh.serviceClient<pr2_mechanism_msgs::SwitchController>(switch_controller_srv_name);
00282   pr2_mechanism_msgs::SwitchController switch_controller;
00283   switch_controller.request.start_controllers.clear();
00284   switch_controller.request.stop_controllers.clear();
00285   switch_controller.request.stop_controllers.push_back("r_gripper_controller");
00286   switch_controller.request.stop_controllers.push_back("l_gripper_controller");
00287   switch_controller.request.stop_controllers.push_back("laser_tilt_controller");
00288   switch_controller.request.stop_controllers.push_back("head_traj_controller");
00289   switch_controller.request.stop_controllers.push_back("r_arm_controller");
00290   switch_controller.request.stop_controllers.push_back("l_arm_controller");
00291   switch_controller.request.stop_controllers.push_back("base_controller");
00292   switch_controller.request.stop_controllers.push_back("torso_controller");
00293   switch_controller.request.strictness = pr2_mechanism_msgs::SwitchControllerRequest::STRICT;
00294   switch_controller_client.call(switch_controller);
00295   
00296   ROS_INFO("pausing physics");
00297   // pause simulation
00298   ros::ServiceClient ps_client = rh.serviceClient<std_srvs::Empty>(pause_srv_name);
00299   std_srvs::Empty ps;
00300   ps_client.call(ps);
00301 
00302   ROS_INFO("setting arm pose");
00303   // set arm model configuration
00304   std::string smcn = "/gazebo/set_model_configuration";
00305   ros::service::waitForService(smcn);
00306   ros::ServiceClient smc_client = rh.serviceClient<gazebo_msgs::SetModelConfiguration>(smcn);
00307   gazebo_msgs::SetModelConfiguration smc;
00308   smc.request.model_name = "pr2";
00309   smc.request.urdf_param_name = "robot_description";
00310 
00311   smc.request.joint_names.push_back("head_pan_joint"); 
00312   smc.request.joint_names.push_back("head_tilt_joint"); 
00313   smc.request.joint_names.push_back("laser_tilt_mount_joint");
00314 
00315   smc.request.joint_positions.push_back(-0.04695624787873154);
00316   smc.request.joint_positions.push_back(0.9571995901909744);
00317   smc.request.joint_positions.push_back(1.0137128597170113);
00318 
00319 
00320 
00321   smc.request.joint_names.push_back("r_upper_arm_roll_joint"); 
00322   smc.request.joint_names.push_back("r_shoulder_pan_joint"); 
00323   smc.request.joint_names.push_back("r_shoulder_lift_joint"); 
00324   smc.request.joint_names.push_back("r_forearm_roll_joint"); 
00325   smc.request.joint_names.push_back("r_elbow_flex_joint"); 
00326   smc.request.joint_names.push_back("r_wrist_flex_joint"); 
00327   smc.request.joint_names.push_back("r_wrist_roll_joint"); 
00328  
00329   smc.request.joint_positions.push_back(-1.6399960594910876);
00330   smc.request.joint_positions.push_back(-0.3689082990354322);
00331   smc.request.joint_positions.push_back(0.3212309310513026);
00332   smc.request.joint_positions.push_back(-3.5471901137256694);
00333   smc.request.joint_positions.push_back(-1.459007345767927);
00334   smc.request.joint_positions.push_back(-0.6873569547257024);
00335   smc.request.joint_positions.push_back(-1.4355428273114548);
00336 
00337 
00338   smc.request.joint_names.push_back("l_upper_arm_roll_joint"); 
00339   smc.request.joint_names.push_back("l_shoulder_pan_joint"); 
00340   smc.request.joint_names.push_back("l_shoulder_lift_joint"); 
00341   smc.request.joint_names.push_back("l_forearm_roll_joint"); 
00342   smc.request.joint_names.push_back("l_elbow_flex_joint"); 
00343   smc.request.joint_names.push_back("l_wrist_flex_joint"); 
00344   smc.request.joint_names.push_back("l_wrist_roll_joint"); 
00345 
00346   smc.request.joint_positions.push_back(1.6399982206352233);
00347   smc.request.joint_positions.push_back(2.1350018853307198);
00348   smc.request.joint_positions.push_back(-0.02000609892474703);
00349   smc.request.joint_positions.push_back(1.6399955487793614);
00350   smc.request.joint_positions.push_back(-2.070018727445361);
00351   smc.request.joint_positions.push_back(-1.6800004066137673);
00352   smc.request.joint_positions.push_back(1.3980012258720143);
00353 
00354 
00355   // 'l_gripper_joint', 'l_gripper_l_finger_joint', 'l_gripper_r_finger_joint', 'l_gripper_r_finger_tip_joint', 'l_gripper_l_finger_tip_joint', 'l_gripper_motor_screw_joint', 'l_gripper_motor_slider_joint', 
00356   // 3.895490011392481e-05, 0.0023094056377544747, 0.0023094056377544747, 0.0023094056377544747, 0.0023094056377544747, 0.0, 0.0, 
00357 
00358 /* need to set parallel links on the gripper too, but not exposed through urdf
00359   smc.request.joint_names.push_back("r_gripper_l_finger_joint"); 
00360   smc.request.joint_names.push_back("r_gripper_r_finger_joint"); 
00361   smc.request.joint_names.push_back("r_gripper_r_finger_tip_joint"); 
00362   smc.request.joint_names.push_back("r_gripper_l_finger_tip_joint"); 
00363   smc.request.joint_names.push_back("r_gripper_joint"); 
00364   smc.request.joint_names.push_back("r_gripper_motor_screw_joint"); 
00365   smc.request.joint_names.push_back("r_gripper_motor_slider_joint"); 
00366 
00367   smc.request.joint_positions.push_back(0.5014809427821072);
00368   smc.request.joint_positions.push_back(0.5014809427821072);
00369   smc.request.joint_positions.push_back(0.5014809427821072);
00370   smc.request.joint_positions.push_back(0.5014809427821072);
00371   smc.request.joint_positions.push_back(0.0859999741332338);
00372   smc.request.joint_positions.push_back(0.0);
00373   smc.request.joint_positions.push_back(0.0);
00374 */
00375 
00376 
00377 
00378   smc_client.call(smc);
00379   
00380   // unpause simulation
00381   ros::ServiceClient ups_client = rh.serviceClient<std_srvs::Empty>(unpause_srv_name);
00382   std_srvs::Empty ups;
00383   ups_client.call(ups);
00384 
00385   /* check another pose
00386   ROS_INFO("setting arm pose 2");
00387   gazebo_msgs::SetModelConfiguration smc1;
00388   smc1.request.model_name = "pr2";
00389   smc1.request.urdf_param_name = "robot_description";
00390   smc1.request.joint_names.push_back("r_upper_arm_roll_joint");
00391   smc1.request.joint_names.push_back("r_shoulder_pan_joint");
00392   smc1.request.joint_names.push_back("r_shoulder_lift_joint");
00393   smc1.request.joint_names.push_back("r_forearm_roll_joint");
00394   smc1.request.joint_names.push_back("r_elbow_flex_joint");
00395   smc1.request.joint_names.push_back("r_wrist_flex_joint");
00396   smc1.request.joint_names.push_back("r_wrist_roll_joint");
00397   smc1.request.joint_positions.push_back(0.0);
00398   smc1.request.joint_positions.push_back(-1.0);
00399   smc1.request.joint_positions.push_back(-0.2);
00400   smc1.request.joint_positions.push_back(0.0);
00401   smc1.request.joint_positions.push_back(-0.2);
00402   smc1.request.joint_positions.push_back(-0.2);
00403   smc1.request.joint_positions.push_back(0.0);
00404   smc_client.call(smc1);
00405  */
00406 
00407   ROS_INFO("restarting controllers");
00408   // start arm controller
00409   switch_controller.request.start_controllers.clear();
00410   switch_controller.request.start_controllers.push_back("r_gripper_controller");
00411   switch_controller.request.start_controllers.push_back("l_gripper_controller");
00412   switch_controller.request.start_controllers.push_back("laser_tilt_controller");
00413   switch_controller.request.start_controllers.push_back("head_traj_controller");
00414   switch_controller.request.start_controllers.push_back("r_arm_controller");
00415   switch_controller.request.start_controllers.push_back("l_arm_controller");
00416   switch_controller.request.start_controllers.push_back("base_controller");
00417   switch_controller.request.start_controllers.push_back("torso_controller");
00418   switch_controller.request.stop_controllers.clear();
00419   switch_controller_client.call(switch_controller);
00420 
00421   /* 
00422   ROS_INFO("closing grippers");
00423   r_gripper.close();
00424   //l_gripper.close();
00425 
00426   ROS_INFO("stopping controllers");
00427   // stop arm controller
00428   switch_controller.request.start_controllers.clear();
00429   switch_controller.request.stop_controllers.clear();
00430   switch_controller.request.stop_controllers.push_back("r_gripper_controller");
00431   switch_controller.request.stop_controllers.push_back("l_gripper_controller");
00432   switch_controller.request.stop_controllers.push_back("laser_tilt_controller");
00433   switch_controller.request.stop_controllers.push_back("head_traj_controller");
00434   switch_controller.request.stop_controllers.push_back("r_arm_controller");
00435   switch_controller.request.stop_controllers.push_back("l_arm_controller");
00436   switch_controller.request.stop_controllers.push_back("base_controller");
00437   switch_controller.request.stop_controllers.push_back("torso_controller");
00438   switch_controller.request.strictness = pr2_mechanism_msgs::SwitchControllerRequest::STRICT;
00439   switch_controller_client.call(switch_controller);
00440 
00441   ROS_INFO("resetting simulation");
00442   // reset simulation
00443   std::string rsn = "/gazebo/reset_simulation";
00444   ros::service::waitForService(rsn);
00445   ros::ServiceClient rs_client = rh.serviceClient<std_srvs::Empty>(rsn);
00446   std_srvs::Empty rs;
00447   rs_client.call(rs);
00448 
00449   ROS_INFO("stop all controllers");
00450   // start arm controller
00451   switch_controller.request.start_controllers.clear();
00452   switch_controller.request.start_controllers.push_back("r_gripper_controller");
00453   switch_controller.request.start_controllers.push_back("l_gripper_controller");
00454   switch_controller.request.start_controllers.push_back("laser_tilt_controller");
00455   switch_controller.request.start_controllers.push_back("head_traj_controller");
00456   switch_controller.request.start_controllers.push_back("r_arm_controller");
00457   switch_controller.request.start_controllers.push_back("l_arm_controller");
00458   switch_controller.request.start_controllers.push_back("base_controller");
00459   switch_controller.request.start_controllers.push_back("torso_controller");
00460   switch_controller.request.stop_controllers.clear();
00461   switch_controller_client.call(switch_controller);
00462   */
00463 
00464 /*
00465   RobotArm arm;
00466 
00467   // Start the trajectory
00468   arm.startTrajectory(arm.armExtensionTrajectory());
00469 
00470   //sleep(5); // FIXME: seems startTrajectory finishes without waiting?
00471 
00472   // close gripper
00473   gripper.close();
00474 */
00475 
00476 /*
00477   // pull object down
00478   ros::ServiceClient wrench_client = rh.serviceClient<gazebo_msgs::ApplyBodyWrench>("/gazebo/apply_body_wrench");
00479   gazebo_msgs::ApplyBodyWrench wrench;
00480   //wrench.request.body_name       = "object_1::object_1_base_link";
00481   //wrench.request.reference_frame = "object_1::object_1_base_link";
00482   wrench.request.body_name       = "object_1::coke_can";
00483   wrench.request.reference_frame = "object_1::coke_can";
00484   wrench.request.wrench.force.z = -25.0;
00485   wrench.request.start_time = ros::Time(0.0);
00486   wrench.request.duration = ros::Duration(-1.0);
00487   wrench_client.call(wrench);
00488 */
00489 
00490 
00491 
00492 }
00493 


pr2_gazebo
Author(s): John Hsu
autogenerated on Thu Apr 24 2014 15:48:38