00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #include <mtconnect_msgs/CloseChuckAction.h>
00020 #include <mtconnect_msgs/OpenChuckAction.h>
00021 #include <mtconnect_msgs/CloseDoorAction.h>
00022 #include <mtconnect_msgs/OpenDoorAction.h>
00023 #include <mtconnect_msgs/MaterialLoadAction.h>
00024 #include <mtconnect_msgs/MaterialUnloadAction.h>
00025 #include <object_manipulation_msgs/PickupAction.h>
00026 #include <object_manipulation_msgs/PlaceAction.h>
00027 #include <arm_navigation_msgs/MoveArmAction.h>
00028 #include <mtconnect_cnc_robot_example/utilities/utilities.h>
00029 #include <actionlib/client/simple_action_client.h>
00030 #include <actionlib/server/simple_action_server.h>
00031 #include <ros/ros.h>
00032 #include <boost/assign/list_inserter.hpp>
00033 #include <boost/assign/list_of.hpp>
00034 #include <mtconnect_msgs/RobotSpindle.h>
00035 #include <mtconnect_msgs/RobotStates.h>
00036 #include <boost/thread.hpp>
00037
00038
00039 typedef actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> MoveArmClient;
00040 typedef actionlib::SimpleActionClient<object_manipulation_msgs::PickupAction> MovePickupClient;
00041 typedef actionlib::SimpleActionClient<object_manipulation_msgs::PlaceAction> MovePlaceClient;
00042 typedef actionlib::SimpleActionClient<mtconnect_msgs::OpenDoorAction> CncOpenDoorClient;
00043 typedef actionlib::SimpleActionClient<mtconnect_msgs::CloseDoorAction> CncCloseDoorClient;
00044 typedef actionlib::SimpleActionClient<mtconnect_msgs::OpenChuckAction> CncOpenChuckClient;
00045 typedef actionlib::SimpleActionClient<mtconnect_msgs::CloseChuckAction> CncCloseChuckClient;
00046 typedef actionlib::SimpleActionServer<mtconnect_msgs::MaterialLoadAction> MaterialLoadServer;
00047 typedef actionlib::SimpleActionServer<mtconnect_msgs::MaterialUnloadAction> MaterialUnloadServer;
00048 typedef boost::shared_ptr<MoveArmClient> MoveArmClientPtr;
00049 typedef boost::shared_ptr<MovePickupClient> MovePickupClientPtr;
00050 typedef boost::shared_ptr<MovePlaceClient> MovePlaceClientPtr;
00051 typedef boost::shared_ptr<CncOpenDoorClient> CncOpenDoorClientPtr;
00052 typedef boost::shared_ptr<CncCloseDoorClient> CncCloseDoorClientPtr;
00053 typedef boost::shared_ptr<CncOpenChuckClient> CncOpenChuckClientPtr;
00054 typedef boost::shared_ptr<CncCloseChuckClient> CncCloseChuckClientPtr;
00055 typedef boost::shared_ptr<MaterialLoadServer> MaterialLoadServerPtr;
00056 typedef boost::shared_ptr<MaterialUnloadServer> MaterialUnloadServerPtr;
00057 typedef std::vector<int> MaterialHandlingSequence;
00058
00059
00060 static const std::string PARAM_ARM_GROUP = "arm_group";
00061 static const std::string PARAM_UNLOAD_PICKUP_GOAL = "unload_pickup_goal";
00062 static const std::string PARAM_UNLOAD_PLACE_GOAL = "unload_place_goal";
00063 static const std::string PARAM_LOAD_PICKUP_GOAL = "load_pickup_goal";
00064 static const std::string PARAM_LOAD_PLACE_GOAL = "load_place_goal";
00065 static const std::string PARAM_JOINT_HOME_POSITION = "/joint_home_position";
00066 static const std::string PARAM_JOINT_WAIT_POSITION = "/joint_wait_position";
00067 static const std::string DEFAULT_MOVE_ARM_ACTION = "move_arm_action";
00068 static const std::string DEFAULT_PICKUP_ACTION = "pickup_action_service";
00069 static const std::string DEFAULT_PLACE_ACTION = "place_action_service";
00070 static const std::string DEFAULT_MATERIAL_LOAD_ACTION= "material_load_action";
00071 static const std::string DEFAULT_MATERIAL_UNLOAD_ACTION= "material_unload_action";
00072 static const std::string DEFAULT_CNC_OPEN_DOOR_ACTION = "cnc_open_door_action";
00073 static const std::string DEFAULT_CNC_CLOSE_DOOR_ACTION = "cnc_close_door_action";
00074 static const std::string DEFAULT_CNC_OPEN_CHUCK_ACTION = "cnc_open_chuck_action";
00075 static const std::string DEFAULT_CNC_CLOSE_CHUCK_ACTION = "cnc_close_chuck_action";
00076 static const std::string DEFAULT_ROBOT_STATES_TOPIC = "robot_states";
00077 static const std::string DEFAULT_ROBOT_SPINDLE_TOPIC = "robot_spindle";
00078 static const std::string CNC_ACTION_ACTIVE_FLAG = "ACTIVE";
00079 static const double DEFAULT_JOINT_ERROR_TOLERANCE = 0.01f;
00080 static const int DEFAULT_PATH_PLANNING_ATTEMPTS = 2;
00081 static const std::string DEFAULT_PATH_PLANNER = "/ompl_planning/plan_kinematic_path";
00082 static const double DURATION_LOOP_PAUSE = 2.0f;
00083 static const double DURATION_TIMER_INTERVAL = 4.0f;
00084 static const double DURATION_WAIT_SERVER = 2.0f;
00085 static const double DURATION_PLANNING_TIME = 5.0f;
00086 static const double DURATION_WAIT_RESULT = 40.0f;
00087 static const double DURATION_PATH_COMPLETION = 2.0f;
00088 static const int MAX_WAIT_ATTEMPTS = 40;
00089
00090 class SimpleMaterialHandlingServer
00091 {
00092 public:
00093 SimpleMaterialHandlingServer():
00094 material_load_pickup_goal_(),
00095 material_unload_pickup_goal_(),
00096 material_load_place_goal_(),
00097 material_unload_place_goal_(),
00098 joint_home_pos_(),
00099 joint_wait_pos_()
00100
00101 {
00102
00103 }
00104
00105 ~SimpleMaterialHandlingServer()
00106 {
00107
00108 }
00109
00110 void run()
00111 {
00112 if(!setup())
00113 {
00114 return;
00115 }
00116
00117 ros::AsyncSpinner spinner(4);
00118 spinner.start();
00119
00120
00121 material_load_server_ptr_->start();
00122 material_unload_server_ptr_->start();
00123
00124
00125 move_arm_goal_.motion_plan_request.goal_constraints.joint_constraints.clear();
00126 joint_home_pos_.toJointConstraints(DEFAULT_JOINT_ERROR_TOLERANCE,DEFAULT_JOINT_ERROR_TOLERANCE,
00127 move_arm_goal_.motion_plan_request.goal_constraints.joint_constraints);
00128
00129 move_arm_client_ptr_->sendGoal(move_arm_goal_);
00130 if(move_arm_client_ptr_->waitForResult(ros::Duration(DURATION_WAIT_RESULT)) &&
00131 (move_arm_client_ptr_->getResult()->error_code.val == arm_navigation_msgs::ArmNavigationErrorCodes::SUCCESS))
00132 {
00133 ROS_INFO_STREAM("Move home succeeded, proceeding");
00134 }
00135 else
00136 {
00137 ROS_ERROR_STREAM("Move home failed, exiting");
00138 return;
00139 }
00140
00141
00142 while(ros::ok())
00143 {
00144 ros::Duration(DURATION_LOOP_PAUSE).sleep();
00145 }
00146 }
00147
00148 public:
00149
00150 MaterialHandlingSequence material_load_sequence_;
00151 MaterialHandlingSequence material_unload_sequence_;
00152
00153 protected:
00154
00155 bool fetchParameters(std::string ns = "")
00156 {
00157 ros::NodeHandle ph("~");
00158 return ph.getParam(PARAM_ARM_GROUP,arm_group_) &&
00159 material_load_pickup_goal_.fetchParameters(PARAM_LOAD_PICKUP_GOAL) && material_load_place_goal_.fetchParameters(PARAM_LOAD_PLACE_GOAL) &&
00160 material_unload_pickup_goal_.fetchParameters(PARAM_UNLOAD_PICKUP_GOAL) && material_unload_place_goal_.fetchParameters(PARAM_UNLOAD_PLACE_GOAL) &&
00161 joint_home_pos_.fetchParameters(PARAM_JOINT_HOME_POSITION) && joint_wait_pos_.fetchParameters(PARAM_JOINT_WAIT_POSITION);
00162 }
00163
00164 bool setup()
00165 {
00166 using namespace boost::assign;
00167 using namespace industrial_msgs;
00168 typedef mtconnect_msgs::MaterialHandlingFeedback Feedback;
00169
00170 ros::NodeHandle nh;
00171 int wait_attempts = 0;
00172
00173 if(fetchParameters())
00174 {
00175 ROS_INFO_STREAM("Read all parameters successfully");
00176 }
00177 else
00178 {
00179 ROS_ERROR_STREAM("Failed to read parameters, exiting");
00180 return false;
00181 }
00182
00183
00184 material_load_sequence_ =
00185 list_of((int)Feedback::TRANSPORTING_MATERIAL)
00186 ((int)Feedback::PLACING_MATERIAL)
00187 ((int)Feedback::CLOSING_CHUCK)
00188 ((int)Feedback::CLEARING_WORK_AREA)
00189 ((int)Feedback::CLOSING_DOOR);
00190
00191 material_unload_sequence_ =
00192 list_of((int)Feedback::OPENNING_DOOR)
00193 ((int)Feedback::OPENNING_CHUCK)
00194 ((int)Feedback::RETRIEVING_WORKPIECE)
00195 ((int)Feedback::TRANSPORTING_WORKPIECE)
00196 ((int)Feedback::CLOSING_CHUCK)((int)Feedback::CLOSING_DOOR);
00197
00198
00199 move_arm_goal_.motion_plan_request.group_name = arm_group_;
00200 move_arm_goal_.motion_plan_request.num_planning_attempts = DEFAULT_PATH_PLANNING_ATTEMPTS;
00201 move_arm_goal_.planner_service_name = DEFAULT_PATH_PLANNER;
00202 move_arm_goal_.motion_plan_request.allowed_planning_time = ros::Duration(DURATION_PLANNING_TIME);
00203 move_arm_goal_.motion_plan_request.planner_id = "";
00204 move_arm_goal_.motion_plan_request.expected_path_duration = ros::Duration(DURATION_PATH_COMPLETION);
00205
00206
00207 material_load_server_ptr_ = MaterialLoadServerPtr(new MaterialLoadServer(nh,DEFAULT_MATERIAL_LOAD_ACTION,
00208 boost::bind(&SimpleMaterialHandlingServer::materialLoadGoalCallback,this,_1),false));
00209
00210 material_unload_server_ptr_ = MaterialUnloadServerPtr(new MaterialUnloadServer(nh,DEFAULT_MATERIAL_UNLOAD_ACTION,
00211 boost::bind(&SimpleMaterialHandlingServer::materialUnloadGoalCallback,this,_1),false));
00212
00213
00214 move_arm_client_ptr_ = MoveArmClientPtr(new MoveArmClient(DEFAULT_MOVE_ARM_ACTION,true));
00215 arm_pickup_client_ptr_ = MovePickupClientPtr(new MovePickupClient(DEFAULT_PICKUP_ACTION,true));
00216 arm_place_client_ptr_ = MovePlaceClientPtr(new MovePlaceClient(DEFAULT_PLACE_ACTION,true));
00217 open_door_client_ptr_ =CncOpenDoorClientPtr(new CncOpenDoorClient(DEFAULT_CNC_OPEN_DOOR_ACTION,true));
00218 close_door_client_ptr_ =CncCloseDoorClientPtr(new CncCloseDoorClient(DEFAULT_CNC_CLOSE_DOOR_ACTION,true));
00219 open_chuck_client_ptr_ =CncOpenChuckClientPtr(new CncOpenChuckClient(DEFAULT_CNC_OPEN_CHUCK_ACTION,true));
00220 close_chuck_client_ptr_ =CncCloseChuckClientPtr(new CncCloseChuckClient(DEFAULT_CNC_CLOSE_CHUCK_ACTION,true));
00221
00222
00223 robot_states_pub_ = nh.advertise<mtconnect_msgs::RobotStates>(DEFAULT_ROBOT_STATES_TOPIC,1);
00224 robot_spindle_pub_ = nh.advertise<mtconnect_msgs::RobotSpindle>(DEFAULT_ROBOT_SPINDLE_TOPIC,1);
00225
00226
00227 robot_state_msg_.avail.val = TriState::ENABLED;
00228 robot_state_msg_.mode.val = RobotMode::AUTO;
00229 robot_state_msg_.rexec.val = TriState::HIGH;
00230 robot_spindle_msg_.c_unclamp.val = TriState::HIGH;
00231 robot_spindle_msg_.s_inter.val = TriState::HIGH;
00232
00233
00234 publish_timer_ = nh.createTimer(ros::Duration(DURATION_TIMER_INTERVAL),
00235 &SimpleMaterialHandlingServer::publishTimerCallback,this,false,false);
00236
00237
00238
00239 while( ros::ok() && (
00240 (!move_arm_client_ptr_->isServerConnected() && !move_arm_client_ptr_->waitForServer(ros::Duration(DURATION_WAIT_SERVER))) ||
00241 (!arm_pickup_client_ptr_->isServerConnected() && !arm_pickup_client_ptr_->waitForServer(ros::Duration(DURATION_WAIT_SERVER))) ||
00242 (!arm_place_client_ptr_->isServerConnected() && !arm_place_client_ptr_->waitForServer(ros::Duration(DURATION_WAIT_SERVER))) ||
00243 (!open_door_client_ptr_->isServerConnected() && !open_door_client_ptr_->waitForServer(ros::Duration(DURATION_WAIT_SERVER))) ||
00244 (!close_door_client_ptr_->isServerConnected() && !close_door_client_ptr_->waitForServer(ros::Duration(DURATION_WAIT_SERVER))) ||
00245 (!open_chuck_client_ptr_->isServerConnected() && !open_chuck_client_ptr_->waitForServer(ros::Duration(DURATION_WAIT_SERVER))) ||
00246 (!close_chuck_client_ptr_->isServerConnected() && !close_chuck_client_ptr_->waitForServer(ros::Duration(DURATION_WAIT_SERVER)))
00247 ))
00248 {
00249 if(wait_attempts++ > MAX_WAIT_ATTEMPTS)
00250 {
00251 ROS_ERROR_STREAM("One or more action cnc/robot servers were not found, exiting");
00252 return false;
00253 }
00254 ROS_WARN_STREAM("Waiting for cnc/robot action servers");
00255 ros::Duration(DURATION_WAIT_SERVER).sleep();
00256 }
00257
00258 publish_timer_.start();
00259
00260 ROS_INFO_STREAM("Found all action servers");
00261 return true;
00262 }
00263
00264 void materialLoadGoalCallback(const MaterialLoadServer::GoalConstPtr &gh)
00265 {
00266
00267 MaterialLoadServer::Result res;
00268
00269 ROS_INFO_STREAM("MATERIAL LOAD request in progress");
00270
00271 if(executeMaterialHandlingTasks(material_load_sequence_))
00272 {
00273 res.load_state = "Succeeded";
00274 material_load_server_ptr_->setSucceeded(res);
00275 ROS_INFO_STREAM("MATERIAL LOAD request succeeded");
00276 }
00277 else
00278 {
00279 res.load_state = "Failed";
00280 material_load_server_ptr_->setAborted(res);
00281 ROS_ERROR_STREAM("MATERIAL LOAD request failed");
00282 }
00283 }
00284
00285 void materialUnloadGoalCallback(const MaterialUnloadServer::GoalConstPtr &gh)
00286 {
00287
00288 MaterialUnloadServer::Result res;
00289
00290
00291 ROS_INFO_STREAM("MATERIAL UNLOAD request in progress");
00292 if(executeMaterialHandlingTasks(material_unload_sequence_))
00293 {
00294 res.unload_state= "Succeeded";
00295 material_unload_server_ptr_->setSucceeded(res);
00296 ROS_INFO_STREAM("MATERIAL UNLOAD request succeeded");
00297 }
00298 else
00299 {
00300 res.unload_state = "Failed";
00301 material_unload_server_ptr_->setAborted(res);
00302 ROS_ERROR_STREAM("MATERIAL UNLOAD request failed");
00303 }
00304 }
00305
00306 bool executeMaterialHandlingTasks(const MaterialHandlingSequence &seq)
00307 {
00308
00309 typedef mtconnect_msgs::MaterialHandlingFeedback Feedback;
00310 typedef MaterialHandlingSequence::const_iterator ConstSequenceIterator;
00311
00312
00313 std::string task_name;
00314
00315
00316 mtconnect_msgs::OpenDoorGoal open_door_goal;
00317 mtconnect_msgs::CloseDoorGoal close_door_goal;
00318 mtconnect_msgs::OpenChuckGoal open_chuck_goal;
00319 mtconnect_msgs::CloseChuckGoal close_chuck_goal;
00320 open_door_goal.open_door = CNC_ACTION_ACTIVE_FLAG;
00321 close_door_goal.close_door = CNC_ACTION_ACTIVE_FLAG;
00322 open_chuck_goal.open_chuck = CNC_ACTION_ACTIVE_FLAG;
00323 close_chuck_goal.close_chuck = CNC_ACTION_ACTIVE_FLAG;
00324
00325 for(ConstSequenceIterator i = seq.begin(); i != seq.end(); i++)
00326 {
00327 switch (*i)
00328 {
00329 case Feedback::TRANSPORTING_MATERIAL:
00330
00331 task_name = "TRANSPORTING_MATERIAL";
00332 ROS_INFO_STREAM(task_name <<" task in progress");
00333 arm_pickup_client_ptr_->sendGoal(material_load_pickup_goal_);
00334 if(arm_pickup_client_ptr_->waitForResult(ros::Duration(DURATION_WAIT_RESULT)))
00335 {
00336 if(arm_pickup_client_ptr_->getResult()->manipulation_result.value != object_manipulation_msgs::ManipulationResult::SUCCESS)
00337 {
00338 ROS_ERROR_STREAM(task_name <<" task failed with status '"<< arm_pickup_client_ptr_->getResult()->manipulation_result.value
00339 <<"', exiting");
00340 return false;
00341 }
00342
00343 ROS_INFO_STREAM(task_name <<" task completed");
00344 }
00345 else
00346 {
00347 ROS_ERROR_STREAM(task_name <<" task failed, exiting");
00348 return false;
00349 }
00350 break;
00351
00352 case Feedback::TRANSPORTING_WORKPIECE:
00353
00354 task_name = "TRANSPORTING_WORKPIECE";
00355 ROS_INFO_STREAM(task_name <<" task in progress");
00356 arm_place_client_ptr_->sendGoal(material_unload_place_goal_);
00357 if(arm_place_client_ptr_->waitForResult(ros::Duration(DURATION_WAIT_RESULT)))
00358 {
00359 if(arm_place_client_ptr_->getResult()->manipulation_result.value != object_manipulation_msgs::ManipulationResult::SUCCESS)
00360 {
00361 ROS_ERROR_STREAM(task_name <<" task failed with status '"<< arm_place_client_ptr_->getResult()->manipulation_result.value
00362 <<"', exiting");
00363 return false;
00364 }
00365
00366 ROS_INFO_STREAM(task_name <<" task completed");
00367 }
00368 else
00369 {
00370 ROS_ERROR_STREAM(task_name <<" task failed, exiting");
00371 return false;
00372 }
00373 break;
00374
00375 case Feedback::OPENNING_DOOR:
00376
00377 task_name = "OPENNING_DOOR";
00378 ROS_INFO_STREAM(task_name <<" task in progress");
00379 open_door_client_ptr_->sendGoal(open_door_goal);
00380 if(open_door_client_ptr_->waitForResult(ros::Duration(DURATION_WAIT_RESULT)))
00381 {
00382 ROS_INFO_STREAM(task_name <<" task completed");
00383 }
00384 else
00385 {
00386 ROS_ERROR_STREAM(task_name <<" task failed due to timeout, exiting");
00387 return false;
00388 }
00389 break;
00390
00391 case Feedback::OPENNING_CHUCK:
00392
00393 task_name = "OPENNING_CHUCK";
00394 ROS_INFO_STREAM(task_name <<" task in progress");
00395 open_chuck_client_ptr_->sendGoal(open_chuck_goal);
00396 if(open_chuck_client_ptr_->waitForResult(ros::Duration(DURATION_WAIT_RESULT)))
00397 {
00398 ROS_INFO_STREAM(task_name <<" task completed");
00399 }
00400 else
00401 {
00402 ROS_ERROR_STREAM(task_name <<" task failed due to timeout, exiting");
00403 return false;
00404 }
00405 break;
00406
00407 break;
00408 case Feedback::CLOSING_CHUCK:
00409
00410 task_name = "CLOSING_CHUCK";
00411 ROS_INFO_STREAM(task_name <<" task in progress");
00412 close_chuck_client_ptr_->sendGoal(close_chuck_goal);
00413 if(close_chuck_client_ptr_->waitForResult(ros::Duration(DURATION_WAIT_RESULT)))
00414 {
00415 ROS_INFO_STREAM(task_name <<" task completed");
00416 }
00417 else
00418 {
00419 ROS_ERROR_STREAM(task_name <<" task failed due to timeout, exiting");
00420 return false;
00421 }
00422 break;
00423
00424 case Feedback::CLOSING_DOOR:
00425
00426 task_name = "CLOSING_DOOR";
00427 ROS_INFO_STREAM(task_name <<" task in progress");
00428 close_door_client_ptr_->sendGoal(close_door_goal);
00429 if(close_door_client_ptr_->waitForResult(ros::Duration(DURATION_WAIT_RESULT)))
00430 {
00431 ROS_INFO_STREAM(task_name <<" task completed");
00432 }
00433 else
00434 {
00435 ROS_ERROR_STREAM(task_name <<" task failed due to timeout, exiting");
00436 return false;
00437 }
00438 break;
00439
00440 case Feedback::PLACING_MATERIAL:
00441
00442 task_name = "PLACING_MATERIAL";
00443 ROS_INFO_STREAM(task_name <<" task in progress");
00444 arm_place_client_ptr_->sendGoal(material_load_place_goal_);
00445 if(arm_place_client_ptr_->waitForResult(ros::Duration(DURATION_WAIT_RESULT)))
00446 {
00447 if(arm_place_client_ptr_->getResult()->manipulation_result.value != object_manipulation_msgs::ManipulationResult::SUCCESS)
00448 {
00449 ROS_ERROR_STREAM(task_name <<" task failed with status '"<< arm_place_client_ptr_->getResult()->manipulation_result.value
00450 <<"', exiting");
00451 return false;
00452 }
00453
00454 ROS_INFO_STREAM(task_name <<" task completed");
00455 }
00456 else
00457 {
00458 ROS_ERROR_STREAM(task_name <<" task failed, exiting");
00459 return false;
00460 }
00461 break;
00462
00463 case Feedback::RETRIEVING_WORKPIECE:
00464
00465 task_name = "RETRIEVING_WORKPIECE";
00466 ROS_INFO_STREAM(task_name <<" task in progress");
00467 arm_pickup_client_ptr_->sendGoal(material_unload_pickup_goal_);
00468 if(arm_pickup_client_ptr_->waitForResult(ros::Duration(DURATION_WAIT_RESULT)))
00469 {
00470 if(arm_pickup_client_ptr_->getResult()->manipulation_result.value != object_manipulation_msgs::ManipulationResult::SUCCESS)
00471 {
00472 ROS_ERROR_STREAM(task_name <<" task failed with status '"<< arm_pickup_client_ptr_->getResult()->manipulation_result.value
00473 <<"', exiting");
00474 return false;
00475 }
00476
00477 ROS_INFO_STREAM(task_name <<" task completed");
00478 }
00479 else
00480 {
00481 ROS_ERROR_STREAM(task_name <<" task failed, exiting");
00482 return false;
00483 }
00484 break;
00485
00486 case Feedback::WAITING_ON_EXTERNAL_DEVICE:
00487 break;
00488 case Feedback::CLEARING_WORK_AREA:
00489
00490 task_name = "CLEARING_WORK_AREA";
00491 ROS_INFO_STREAM(task_name <<" task in progress");
00492
00493
00494 move_arm_goal_.motion_plan_request.goal_constraints.joint_constraints.clear();
00495 joint_wait_pos_.toJointConstraints(DEFAULT_JOINT_ERROR_TOLERANCE,DEFAULT_JOINT_ERROR_TOLERANCE,
00496 move_arm_goal_.motion_plan_request.goal_constraints.joint_constraints);
00497
00498 move_arm_client_ptr_->sendGoal(move_arm_goal_);
00499 if(move_arm_client_ptr_->waitForResult(ros::Duration(DURATION_WAIT_RESULT)))
00500 {
00501 if(move_arm_client_ptr_->getResult()->error_code.val != arm_navigation_msgs::ArmNavigationErrorCodes::SUCCESS)
00502 {
00503 ROS_ERROR_STREAM(task_name <<" task failed, exiting");
00504 return false;
00505 }
00506
00507 ROS_INFO_STREAM(task_name <<" task completed");
00508 }
00509 else
00510 {
00511 ROS_ERROR_STREAM(task_name <<" task failed, exiting");
00512 return false;
00513 }
00514 break;
00515
00516
00517 }
00518 }
00519 return true;
00520 }
00521
00522 void publishTimerCallback(const ros::TimerEvent &evnt)
00523 {
00524
00525 robot_state_msg_.header.stamp = ros::Time::now();
00526 robot_spindle_msg_.header.stamp = ros::Time::now();
00527
00528
00529 robot_states_pub_.publish(robot_state_msg_);
00530 robot_spindle_pub_.publish(robot_spindle_msg_);
00531 }
00532
00533 protected:
00534
00535
00536 MaterialLoadServerPtr material_load_server_ptr_;
00537 MaterialUnloadServerPtr material_unload_server_ptr_;
00538
00539
00540 MoveArmClientPtr move_arm_client_ptr_;
00541 MovePickupClientPtr arm_pickup_client_ptr_;
00542 MovePlaceClientPtr arm_place_client_ptr_;
00543 CncOpenDoorClientPtr open_door_client_ptr_;
00544 CncCloseDoorClientPtr close_door_client_ptr_;
00545 CncOpenChuckClientPtr open_chuck_client_ptr_;
00546 CncCloseChuckClientPtr close_chuck_client_ptr_;
00547
00548
00549 ros::Publisher robot_states_pub_;
00550 ros::Publisher robot_spindle_pub_;
00551
00552
00553 mtconnect_msgs::RobotStates robot_state_msg_;
00554 mtconnect_msgs::RobotSpindle robot_spindle_msg_;
00555
00556
00557 ros::Timer publish_timer_;
00558
00559
00560 std::string arm_group_;
00561 arm_navigation_msgs::MoveArmGoal move_arm_goal_;
00562 move_arm_utils::PickupGoalInfo material_load_pickup_goal_;
00563 move_arm_utils::PlaceGoalInfo material_load_place_goal_;
00564 move_arm_utils::PickupGoalInfo material_unload_pickup_goal_;
00565 move_arm_utils::PlaceGoalInfo material_unload_place_goal_;
00566 move_arm_utils::JointStateInfo joint_home_pos_;
00567 move_arm_utils::JointStateInfo joint_wait_pos_;
00568
00569 };
00570
00571 int main(int argc,char** argv)
00572 {
00573 ros::init(argc,argv,"material_hanlding_server_test");
00574
00575 SimpleMaterialHandlingServer material_handling_server;
00576 material_handling_server.run();
00577 return 0;
00578 }