00001 00002 /* 00003 * Copyright 2013 Southwest Research Institute 00004 00005 Licensed under the Apache License, Version 2.0 (the "License"); 00006 you may not use this file except in compliance with the License. 00007 You may obtain a copy of the License at 00008 00009 http://www.apache.org/licenses/LICENSE-2.0 00010 00011 Unless required by applicable law or agreed to in writing, software 00012 distributed under the License is distributed on an "AS IS" BASIS, 00013 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 00014 See the License for the specific language governing permissions and 00015 limitations under the License. 00016 */ 00017 00018 #include <mtconnect_cnc_robot_example/move_arm_action_clients/MovePickPlaceServer.h> 00019 00020 using namespace mtconnect_cnc_robot_example; 00021 00022 class MovePickPlaceTest: public MovePickPlaceServer 00023 { 00024 public: 00025 MovePickPlaceTest(): 00026 MovePickPlaceServer() 00027 { 00028 00029 } 00030 00031 ~MovePickPlaceTest() 00032 { 00033 00034 } 00035 00036 00037 virtual void run() 00038 { 00039 if(!setup()) 00040 { 00041 return; 00042 } 00043 00044 ros::AsyncSpinner spinner(2); 00045 spinner.start(); 00046 00047 ROS_INFO_STREAM("Cycling through pick and place moves"); 00048 while(ros::ok()) 00049 { 00050 if(moveArmThroughPickSequence(pickup_goal_)) 00051 { 00052 ROS_INFO_STREAM("Pickup move completed"); 00053 } 00054 else 00055 { 00056 ROS_ERROR_STREAM("Pickup move failed, exiting"); 00057 break; 00058 } 00059 00060 if(moveArmThroughPlaceSequence(place_goal_)) 00061 { 00062 ROS_INFO_STREAM("Place move completed"); 00063 } 00064 else 00065 { 00066 ROS_ERROR_STREAM("Place move failed, exiting"); 00067 break; 00068 } 00069 } 00070 } 00071 00072 virtual bool fetchParameters(std::string ns = "") 00073 { 00074 // fetching pick and place parameters for protected pick place members (only needed if not servicing client requests) 00075 if(MovePickPlaceServer::fetchParameters() && pickup_goal_.fetchParameters() && place_goal_.fetchParameters()) 00076 { 00077 ROS_INFO_STREAM("Successfully read arm and pick place info parameters"); 00078 return true; 00079 } 00080 else 00081 { 00082 ROS_ERROR_STREAM("Failed to read arm and pick place info parameters"); 00083 return false; 00084 } 00085 } 00086 00087 public: 00088 00089 PickupGoalInfo pickup_goal_; 00090 PlaceGoalInfo place_goal_; 00091 }; 00092 00093 int main(int argc,char** argv) 00094 { 00095 00096 ros::init(argc,argv,"move_pick_place_test"); 00097 MoveArmActionClientPtr arm_client_ptr = MoveArmActionClientPtr(new MovePickPlaceTest()); 00098 00099 // run through pick place motions 00100 arm_client_ptr->run(); 00101 00102 return 0; 00103 }