00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include <mtconnect_cnc_robot_example/state_machine/state_machine.h>
00018 #include <ros/topic.h>
00019
00020 static const std::string PARAM_ARM_GROUP = "arm_group";
00021 static const std::string PARAM_UNLOAD_PICKUP_GOAL = "unload_pickup_goal";
00022 static const std::string PARAM_UNLOAD_PLACE_GOAL = "unload_place_goal";
00023 static const std::string PARAM_LOAD_PICKUP_GOAL = "load_pickup_goal";
00024 static const std::string PARAM_LOAD_PLACE_GOAL = "load_place_goal";
00025 static const std::string PARAM_JOINT_HOME_POSITION = "joint_home_position";
00026 static const std::string PARAM_JOINT_WAIT_POSITION = "joint_wait_position";
00027 static const std::string PARAM_TRAJ_ARBITRARY_MOVE = "traj_arbitrary";
00028 static const std::string PARAM_TRAJ_APPROACH_CNC = "traj_approach_cnc";
00029 static const std::string PARAM_TRAJ_ENTER_CNC = "traj_enter_cnc";
00030 static const std::string PARAM_TRAJ_MOVE_TO_CHUCK = "traj_move_to_chuck";
00031 static const std::string PARAM_TRAJ_RETREAT_FROM_CHUCK = "traj_retreat_from_chuck";
00032 static const std::string PARAM_TRAJ_EXIT_CNC = "traj_exit_cnc";
00033 static const std::string PARAM_FORCE_ROBOT_FAULT = "force_robot_fault";
00034 static const std::string PARAM_FORCE_CNC_FAULT = "force_cnc_fault";
00035 static const std::string PARAM_FORCE_GRIPPER_FAULT = "force_gripper_fault";
00036 static const std::string PARAM_FORCE_FAULT_ON_TASK = "force_fault_on_task";
00037 static const std::string PARAM_TASK_DESCRIPTION = "task_description";
00038 static const std::string PARAM_USE_TASK_MOTION = "use_task_motion";
00039
00040
00041 static const std::string DEFAULT_MOVE_ARM_ACTION = "move_arm_action";
00042 static const std::string DEFAULT_PICKUP_ACTION = "pickup_action_service";
00043 static const std::string DEFAULT_PLACE_ACTION = "place_action_service";
00044 static const std::string DEFAULT_GRASP_ACTION = "grasp_action_service";
00045 static const std::string DEFAULT_VISE_ACTION = "vise_action_service";
00046 static const std::string DEFAULT_MATERIAL_LOAD_ACTION= "material_load_action";
00047 static const std::string DEFAULT_MATERIAL_UNLOAD_ACTION= "material_unload_action";
00048 static const std::string DEFAULT_CNC_OPEN_DOOR_ACTION = "cnc_open_door_action";
00049 static const std::string DEFAULT_CNC_CLOSE_DOOR_ACTION = "cnc_close_door_action";
00050 static const std::string DEFAULT_CNC_OPEN_CHUCK_ACTION = "cnc_open_chuck_action";
00051 static const std::string DEFAULT_CNC_CLOSE_CHUCK_ACTION = "cnc_close_chuck_action";
00052 static const std::string DEFAULT_JOINT_TRAJ_ACTION = "joint_trajectory_action";
00053 static const std::string DEFAULT_ROBOT_STATES_TOPIC = "robot_states";
00054 static const std::string DEFAULT_ROBOT_SPINDLE_TOPIC = "robot_spindle";
00055 static const std::string DEFAULT_ROBOT_STATUS_TOPIC = "robot_status";
00056 static const std::string DEFAULT_JOINT_STATE_TOPIC = "joint_states";
00057 static const std::string DEFAULT_EXTERNAL_COMMAND_SERVICE = "external_command";
00058 static const std::string DEFAULT_MATERIAL_LOAD_SET_STATE_SERVICE = "/MaterialLoad/set_mtconnect_state";
00059 static const std::string DEFAULT_MATERIAL_UNLOAD_SET_STATE_SERVICE = "/MaterialUnload/set_mtconnect_state";
00060 static const std::string DEFAULT_TRAJECTORY_FILTER_SERVICE = "filter_trajectory_with_constraints";
00061
00062 static const std::string CNC_ACTION_ACTIVE_FLAG = "ACTIVE";
00063 static const double DEFAULT_JOINT_ERROR_TOLERANCE = 0.01f;
00064 static const int DEFAULT_PATH_PLANNING_ATTEMPTS = 2;
00065 static const std::string DEFAULT_PATH_PLANNER = "/ompl_planning/plan_kinematic_path";
00066 static const double DURATION_LOOP_PAUSE = 0.5f;
00067 static const double DURATION_TIMER_INTERVAL = 4.0f;
00068 static const double DURATION_WAIT_SERVER = 2.0f;
00069 static const double DURATION_PLANNING_TIME = 5.0f;
00070 static const double DURATION_WAIT_RESULT = 40.0f;
00071 static const double DURATION_PATH_COMPLETION = 2.0f;
00072 static const double DURATION_JOINT_MESSAGE_TIMEOUT = 10.0f;
00073
00074
00075 using namespace mtconnect_cnc_robot_example::state_machine;
00076
00077 StateMachine::StateMachine()
00078 {
00079
00080
00081 }
00082
00083 StateMachine::~StateMachine()
00084 {
00085
00086 }
00087
00088 bool StateMachine::fetch_parameters(std::string name_space)
00089 {
00090 ros::NodeHandle ph("~");
00091 return ph.getParam(PARAM_TASK_DESCRIPTION, task_desc_) &&
00092 ph.getParam(PARAM_USE_TASK_MOTION, use_task_desc_motion) &&
00093 ph.getParam(PARAM_ARM_GROUP,arm_group_) &&
00094 material_load_pickup_goal_.fetchParameters(PARAM_LOAD_PICKUP_GOAL) &&
00095 material_unload_place_goal_.fetchParameters(PARAM_UNLOAD_PLACE_GOAL) &&
00096 joint_home_pos_.fetchParameters(PARAM_JOINT_HOME_POSITION) &&
00097 joint_wait_pos_.fetchParameters(PARAM_JOINT_WAIT_POSITION) &&
00098 traj_approach_cnc_.fetchParameters(PARAM_TRAJ_APPROACH_CNC) &&
00099 traj_enter_cnc_.fetchParameters(PARAM_TRAJ_ENTER_CNC) &&
00100 traj_move_to_chuck_.fetchParameters(PARAM_TRAJ_MOVE_TO_CHUCK) &&
00101 traj_retreat_from_chuck_.fetchParameters(PARAM_TRAJ_RETREAT_FROM_CHUCK) &&
00102 traj_exit_cnc_.fetchParameters(PARAM_TRAJ_EXIT_CNC) &&
00103 traj_arbitrary_move_.fetchParameters(PARAM_TRAJ_ARBITRARY_MOVE);
00104 return true;
00105 }
00106 bool StateMachine::setup()
00107 {
00108 using namespace industrial_msgs;
00109
00110 ros::NodeHandle nh;
00111 int wait_attempts = 0;
00112
00113 if(fetch_parameters())
00114 {
00115 ROS_INFO_STREAM("Read all parameters successfully");
00116 }
00117 else
00118 {
00119 ROS_ERROR_STREAM("Failed to read parameters, exiting");
00120 return false;
00121 }
00122
00123
00124 if (!parseTaskXml(task_desc_, joint_paths_))
00125 {
00126 ROS_ERROR_STREAM("Failed to initialize task xml");
00127 return false;
00128 }
00129
00130
00131 if(!MoveArmActionClient::setup())
00132 {
00133 ROS_ERROR_STREAM("Failed to initialize MoveArmActionClient");
00134 return false;
00135 }
00136
00137
00138 material_load_server_ptr_ = MaterialLoadServerPtr(new MaterialLoadServer(nh,DEFAULT_MATERIAL_LOAD_ACTION,false));
00139 material_load_server_ptr_->registerGoalCallback(boost::bind(&StateMachine::material_load_goalcb,this));
00140 material_unload_server_ptr_ = MaterialUnloadServerPtr(new MaterialUnloadServer(nh,DEFAULT_MATERIAL_UNLOAD_ACTION,false));
00141 material_unload_server_ptr_->registerGoalCallback(boost::bind(&StateMachine::material_unload_goalcb,this));
00142
00143
00144 move_pickup_client_ptr_ = MovePickupClientPtr(new MovePickupClient(DEFAULT_PICKUP_ACTION,true));
00145 move_place_client_ptr_ = MovePlaceClientPtr(new MovePlaceClient(DEFAULT_PLACE_ACTION,true));
00146 open_door_client_ptr_ =CncOpenDoorClientPtr(new CncOpenDoorClient(DEFAULT_CNC_OPEN_DOOR_ACTION,true));
00147 close_door_client_ptr_ =CncCloseDoorClientPtr(new CncCloseDoorClient(DEFAULT_CNC_CLOSE_DOOR_ACTION,true));
00148 open_chuck_client_ptr_ =CncOpenChuckClientPtr(new CncOpenChuckClient(DEFAULT_CNC_OPEN_CHUCK_ACTION,true));
00149 close_chuck_client_ptr_ =CncCloseChuckClientPtr(new CncCloseChuckClient(DEFAULT_CNC_CLOSE_CHUCK_ACTION,true));
00150 grasp_action_client_ptr_ = GraspActionClientPtr(new GraspActionClient(DEFAULT_GRASP_ACTION,true));
00151 vise_action_client_ptr_ = GraspActionClientPtr(new GraspActionClient(DEFAULT_VISE_ACTION,true));
00152 joint_traj_client_ptr_ = JointTractoryClientPtr(new JointTractoryClient(DEFAULT_JOINT_TRAJ_ACTION,true));
00153
00154
00155 robot_states_pub_ = nh.advertise<mtconnect_msgs::RobotStates>(DEFAULT_ROBOT_STATES_TOPIC,1);
00156 robot_spindle_pub_ = nh.advertise<mtconnect_msgs::RobotSpindle>(DEFAULT_ROBOT_SPINDLE_TOPIC,1);
00157
00158
00159 robot_status_sub_ = nh.subscribe(DEFAULT_ROBOT_STATUS_TOPIC,1,&StateMachine::ros_status_subs_cb,this);
00160
00161
00162 external_command_srv_ = nh.advertiseService(DEFAULT_EXTERNAL_COMMAND_SERVICE,&StateMachine::external_command_cb,this);
00163
00164
00165 material_load_set_state_client_ = nh.serviceClient<mtconnect_msgs::SetMTConnectState>(DEFAULT_MATERIAL_LOAD_SET_STATE_SERVICE);
00166 material_unload_set_state_client_ = nh.serviceClient<mtconnect_msgs::SetMTConnectState>(DEFAULT_MATERIAL_UNLOAD_SET_STATE_SERVICE);
00167
00168 trajectory_filter_client_ =
00169 nh.serviceClient<arm_navigation_msgs::FilterJointTrajectoryWithConstraints>(
00170 DEFAULT_TRAJECTORY_FILTER_SERVICE);
00171
00172
00173 mat_load_set_state_.request.state_flag = mtconnect_msgs::SetMTConnectState::Request::READY;
00174 mat_load_set_state_.response.accepted = false;
00175
00176 mat_unload_set_state_.request.state_flag = mtconnect_msgs::SetMTConnectState::Request::READY;
00177 mat_unload_set_state_.response.accepted = false;
00178
00179
00180
00181 robot_state_msg_.avail.val = TriState::ENABLED;
00182 robot_state_msg_.mode.val = RobotMode::AUTO;
00183 robot_state_msg_.rexec.val = TriState::HIGH;
00184 robot_spindle_msg_.c_unclamp.val = TriState::HIGH;
00185 robot_spindle_msg_.s_inter.val = TriState::HIGH;
00186
00187
00188 move_arm_joint_goal_.motion_plan_request.group_name = arm_group_;
00189 move_arm_joint_goal_.motion_plan_request.num_planning_attempts = DEFAULT_PATH_PLANNING_ATTEMPTS;
00190 move_arm_joint_goal_.planner_service_name = DEFAULT_PATH_PLANNER;
00191 move_arm_joint_goal_.motion_plan_request.allowed_planning_time = ros::Duration(DURATION_PLANNING_TIME);
00192 move_arm_joint_goal_.motion_plan_request.planner_id = "";
00193 move_arm_joint_goal_.motion_plan_request.expected_path_duration = ros::Duration(DURATION_PATH_COMPLETION);
00194
00195
00196 robot_topics_timer_ = nh.createTimer(ros::Duration(DURATION_TIMER_INTERVAL),
00197 &StateMachine::publish_robot_topics_timercb,this,false,false);
00198
00199
00200 material_load_task_sequence_.clear();
00201 jm_material_load_task_sequence_.clear();
00202 material_unload_task_sequence_.clear();
00203 jm_material_unload_task_sequence_.clear();
00204
00205 using namespace state_machine::tasks;
00206 using namespace boost::assign;
00207 material_load_task_sequence_ = list_of((int)MATERIAL_LOAD_START)
00208 ((int)ROBOT_MOVE_HOME)
00209 ((int)CNC_OPEN_DOOR)
00210 ((int)VISE_OPEN)
00211 ((int)CNC_OPEN_CHUCK)
00212 ((int)ROBOT_PICKUP_MATERIAL)
00213 ((int)ROBOT_APPROACH_CNC)
00214 ((int)ROBOT_ENTER_CNC)
00215 ((int)ROBOT_MOVE_TO_CHUCK)
00216 ((int)CNC_CLOSE_CHUCK)
00217 ((int)VISE_CLOSE)
00218 ((int)GRIPPER_OPEN)
00219 ((int)ROBOT_RETREAT_FROM_CHUCK)
00220 ((int)ROBOT_EXIT_CNC)
00221 ((int)CNC_CLOSE_DOOR)
00222 ((int)MATERIAL_LOAD_END);
00223
00224
00225
00226 jm_material_load_task_sequence_ = list_of((int)MATERIAL_LOAD_START)
00227 ((int)JM_HOME_TO_READY)
00228 ((int)JM_READY_TO_APPROACH)
00229 ((int)GRIPPER_OPEN)
00230 ((int)JM_APPROACH_TO_PICK)
00231 ((int)GRIPPER_CLOSE)
00232 ((int)JM_PICK_TO_DOOR)
00233 ((int)CNC_OPEN_DOOR)
00234 ((int)VISE_OPEN)
00235 ((int)CNC_OPEN_CHUCK)
00236 ((int)JM_DOOR_TO_CHUCK)
00237 ((int)CNC_CLOSE_CHUCK)
00238 ((int)VISE_CLOSE)
00239 ((int)GRIPPER_OPEN)
00240 ((int)JM_CHUCK_TO_READY)
00241 ((int)CNC_CLOSE_DOOR)
00242 ((int)MATERIAL_LOAD_END);
00243
00244
00245 material_unload_task_sequence_ = list_of((int)MATERIAL_UNLOAD_START)
00246 ((int)ROBOT_APPROACH_CNC)
00247 ((int)GRIPPER_OPEN)
00248 ((int)CNC_OPEN_DOOR)
00249 ((int)ROBOT_ENTER_CNC)
00250 ((int)ROBOT_MOVE_TO_CHUCK)
00251 ((int)GRIPPER_CLOSE)
00252 ((int)VISE_OPEN)
00253 ((int)CNC_OPEN_CHUCK)
00254 ((int)ROBOT_RETREAT_FROM_CHUCK)
00255 ((int)ROBOT_EXIT_CNC)
00256 ((int)ROBOT_PLACE_WORKPIECE)
00257 ((int)CNC_CLOSE_CHUCK)
00258 ((int)VISE_CLOSE)
00259 ((int)CNC_CLOSE_DOOR)
00260 ((int)MATERIAL_UNLOAD_END);
00261
00262 jm_material_unload_task_sequence_ = list_of((int)MATERIAL_UNLOAD_START)
00263 ((int)JM_READY_TO_DOOR)
00264 ((int)GRIPPER_OPEN)
00265 ((int)CNC_OPEN_DOOR)
00266 ((int)JM_DOOR_TO_CHUCK)
00267 ((int)GRIPPER_CLOSE)
00268 ((int)VISE_OPEN)
00269 ((int)CNC_OPEN_CHUCK)
00270 ((int)JM_CHUCK_TO_READY)
00271 ((int)JM_READY_TO_APPROACH)
00272 ((int)JM_APPROACH_TO_PICK)
00273 ((int)GRIPPER_OPEN)
00274 ((int)JM_PICK_TO_HOME)
00275 ((int)MATERIAL_UNLOAD_END);
00276
00277
00278 while( ros::ok() && (
00279 (!move_arm_client_ptr_->isServerConnected() && !move_arm_client_ptr_->waitForServer(ros::Duration(DURATION_WAIT_SERVER))) ||
00280 (!move_pickup_client_ptr_->isServerConnected() && !move_pickup_client_ptr_->waitForServer(ros::Duration(DURATION_WAIT_SERVER))) ||
00281 (!move_place_client_ptr_->isServerConnected() && !move_place_client_ptr_->waitForServer(ros::Duration(DURATION_WAIT_SERVER))) ||
00282 (!grasp_action_client_ptr_->isServerConnected() && !grasp_action_client_ptr_->waitForServer(ros::Duration(DURATION_WAIT_SERVER))) ||
00283 (!vise_action_client_ptr_->isServerConnected() && !vise_action_client_ptr_->waitForServer(ros::Duration(DURATION_WAIT_SERVER))) ||
00284 (!open_door_client_ptr_->isServerConnected() && !open_door_client_ptr_->waitForServer(ros::Duration(DURATION_WAIT_SERVER))) ||
00285 (!close_door_client_ptr_->isServerConnected() && !close_door_client_ptr_->waitForServer(ros::Duration(DURATION_WAIT_SERVER))) ||
00286 (!open_chuck_client_ptr_->isServerConnected() && !open_chuck_client_ptr_->waitForServer(ros::Duration(DURATION_WAIT_SERVER))) ||
00287 (!close_chuck_client_ptr_->isServerConnected() && !close_chuck_client_ptr_->waitForServer(ros::Duration(DURATION_WAIT_SERVER))) ||
00288 (!joint_traj_client_ptr_->isServerConnected() && !joint_traj_client_ptr_->waitForServer(ros::Duration(DURATION_WAIT_SERVER)))
00289 ))
00290 {
00291 if(wait_attempts++ > MAX_WAIT_ATTEMPTS)
00292 {
00293 ROS_ERROR_STREAM("One or more action cnc/robot servers were not found, exiting");
00294 return false;
00295 }
00296 ROS_WARN_STREAM("Waiting for cnc/robot action servers");
00297 ros::Duration(DURATION_WAIT_SERVER).sleep();
00298 }
00299
00300
00301 robot_topics_timer_.start();
00302 material_load_server_ptr_->start();
00303 material_unload_server_ptr_->start();
00304
00305 return true;
00306 }
00307
00308 void StateMachine::run()
00309 {
00310 ros::NodeHandle nh;
00311 set_active_state(states::STARTUP);
00312
00313 int last_state = states::EMPTY;
00314 int active_state = get_active_state();
00315 print_current_state();
00316 while(ros::ok() && process_transition())
00317 {
00318 ros::spinOnce();
00319
00320
00321 get_param_force_fault_flags();
00322
00323
00324 get_param_state_override();
00325
00326
00327 active_state = get_active_state();
00328 if(active_state != last_state)
00329 {
00330 last_state = active_state;
00331 print_current_state();
00332 }
00333 }
00334 }
00335
00336 bool StateMachine::run_next_task()
00337 {
00338 using namespace mtconnect_cnc_robot_example::state_machine::tasks;
00339
00340 if(!all_action_servers_connected())
00341 {
00342 ROS_WARN_STREAM("One or more action servers are not ready, aborting task");
00343 current_task_sequence_.clear();
00344 current_task_index_ = 0;
00345 return false;
00346 }
00347
00348 if(++current_task_index_ < (int)current_task_sequence_.size() )
00349 {
00350 std::cout<<"\t - "<< TASK_MAP[current_task_sequence_[current_task_index_]] << " task running\n";
00351 run_task(current_task_sequence_[current_task_index_]);
00352 return true;
00353 }
00354 else
00355 {
00356
00357 current_task_sequence_.clear();
00358 current_task_index_ = 0;
00359 return false;
00360 }
00361 }
00362
00363 bool StateMachine::run_task(int task_id)
00364 {
00365 using namespace mtconnect_cnc_robot_example::state_machine::tasks;
00366
00367
00368 typedef object_manipulation_msgs::GraspHandPostureExecutionGoal GraspGoal;
00369 GraspGoal grasp_goal;
00370 GraspGoal vise_goal;
00371 mtconnect_msgs::OpenDoorGoal open_door_goal;
00372 mtconnect_msgs::CloseDoorGoal close_door_goal;
00373 mtconnect_msgs::OpenChuckGoal open_chuck_goal;
00374 mtconnect_msgs::CloseChuckGoal close_chuck_goal;
00375 open_door_goal.open_door = CNC_ACTION_ACTIVE_FLAG;
00376 close_door_goal.close_door = CNC_ACTION_ACTIVE_FLAG;
00377 open_chuck_goal.open_chuck = CNC_ACTION_ACTIVE_FLAG;
00378 close_chuck_goal.close_chuck = CNC_ACTION_ACTIVE_FLAG;
00379
00380
00381 cartesian_poses_.poses.clear();
00382 switch(task_id)
00383 {
00384 case MATERIAL_LOAD_END:
00385 set_active_state(states::MATERIAL_LOAD_COMPLETED);
00386 break;
00387
00388 case MATERIAL_UNLOAD_END:
00389 set_active_state(states::MATERIAL_UNLOAD_COMPLETED);
00390 break;
00391
00392 case TEST_TASK_END:
00393
00394 set_active_state(states::TEST_TASK_COMPLETED);
00395 break;
00396
00397 case ROBOT_APPROACH_CNC:
00398
00399 getTrajectoryInArmSpace(traj_approach_cnc_,cartesian_poses_);
00400 moveArm(cartesian_poses_);
00401 set_active_state(states::ROBOT_MOVING);
00402
00403 break;
00404 case ROBOT_ENTER_CNC:
00405
00406 getTrajectoryInArmSpace(traj_enter_cnc_,cartesian_poses_);
00407 moveArm(cartesian_poses_);
00408 set_active_state(states::ROBOT_MOVING);
00409
00410 break;
00411 case ROBOT_EXIT_CNC:
00412
00413 getTrajectoryInArmSpace(traj_exit_cnc_,cartesian_poses_);
00414 moveArm(cartesian_poses_);
00415 set_active_state(states::ROBOT_MOVING);
00416
00417 break;
00418 case ROBOT_MOVE_HOME:
00419
00420 moveArm(joint_home_pos_);
00421 set_active_state(states::ROBOT_MOVING);
00422 break;
00423
00424 case ROBOT_MOVE_TO_CHUCK:
00425
00426 getTrajectoryInArmSpace(traj_move_to_chuck_,cartesian_poses_);
00427 moveArm(cartesian_poses_);
00428 set_active_state(states::ROBOT_MOVING);
00429 break;
00430
00431 case ROBOT_RETREAT_FROM_CHUCK:
00432
00433 getTrajectoryInArmSpace(traj_retreat_from_chuck_,cartesian_poses_);
00434 moveArm(cartesian_poses_);
00435 set_active_state(states::ROBOT_MOVING);
00436 break;
00437
00438 case ROBOT_PICKUP_MATERIAL:
00439
00440 move_pickup_client_ptr_->sendGoal(material_load_pickup_goal_);
00441 set_active_state(states::ROBOT_MOVING);
00442 break;
00443
00444 case ROBOT_PLACE_WORKPIECE:
00445
00446 move_place_client_ptr_->sendGoal(material_unload_place_goal_);
00447 set_active_state(states::ROBOT_MOVING);
00448 break;
00449
00450 case ROBOT_CARTESIAN_MOVE:
00451
00452 getTrajectoryInArmSpace(traj_arbitrary_move_,cartesian_poses_);
00453 moveArm(cartesian_poses_);
00454 set_active_state(states::ROBOT_MOVING);
00455 break;
00456
00457 case CNC_CLOSE_CHUCK:
00458
00459 close_chuck_client_ptr_->sendGoal(close_chuck_goal);
00460 set_active_state(states::CNC_MOVING);
00461 break;
00462
00463 case CNC_OPEN_CHUCK:
00464
00465 open_chuck_client_ptr_->sendGoal(open_chuck_goal);
00466 set_active_state(states::CNC_MOVING);
00467 break;
00468
00469 case CNC_CLOSE_DOOR:
00470
00471 close_door_client_ptr_->sendGoal(close_door_goal);
00472 set_active_state(states::CNC_MOVING);
00473 break;
00474
00475 case CNC_OPEN_DOOR:
00476
00477 open_door_client_ptr_->sendGoal(open_door_goal);
00478 set_active_state(states::CNC_MOVING);
00479 break;
00480
00481 case GRIPPER_CLOSE:
00482
00483 grasp_goal.goal = GraspGoal::GRASP;
00484 grasp_action_client_ptr_->sendGoal(grasp_goal);
00485 set_active_state(states::GRIPPER_MOVING);
00486 break;
00487
00488 case GRIPPER_OPEN:
00489
00490 grasp_goal.goal = GraspGoal::RELEASE;
00491 grasp_action_client_ptr_->sendGoal(grasp_goal);
00492 set_active_state(states::GRIPPER_MOVING);
00493 break;
00494 case VISE_CLOSE:
00495
00496 vise_goal.goal = GraspGoal::GRASP;
00497 vise_action_client_ptr_->sendGoal(vise_goal);
00498 set_active_state(states::GRIPPER_MOVING);
00499 break;
00500
00501 case VISE_OPEN:
00502
00503 vise_goal.goal = GraspGoal::RELEASE;
00504 vise_action_client_ptr_->sendGoal(vise_goal);
00505 set_active_state(states::GRIPPER_MOVING);
00506 break;
00507
00508
00509 case JM_HOME_TO_READY:
00510 case JM_READY_TO_APPROACH:
00511 case JM_APPROACH_TO_PICK:
00512 case JM_PICK_TO_DOOR:
00513 case JM_DOOR_TO_CHUCK:
00514 case JM_READY_TO_DOOR:
00515 case JM_CHUCK_TO_READY:
00516 case JM_PICK_TO_HOME:
00517
00518
00519
00520 moveArm(TASK_MAP[task_id]);
00521 set_active_state(states::ROBOT_MOVING);
00522 break;
00523
00524 default:
00525
00526 break;
00527 }
00528
00529 return true;
00530 }
00531
00532 void StateMachine::cancel_active_material_requests()
00533 {
00534 if(material_load_server_ptr_->isActive())
00535 {
00536 MaterialLoadServer::Result res;
00537 res.load_state= "Failed";
00538 material_load_server_ptr_->setAborted(res,res.load_state);
00539 ROS_INFO_STREAM("Material Load goal aborted");
00540 }
00541
00542 if(material_unload_server_ptr_->isActive())
00543 {
00544 MaterialUnloadServer::Result res;
00545 res.unload_state = "Failed";
00546 material_unload_server_ptr_->setAborted(res,res.unload_state);
00547 ROS_INFO_STREAM("Material Unload goal aborted");
00548 }
00549 }
00550
00551 void StateMachine::cancel_active_action_goals()
00552 {
00553 move_pickup_client_ptr_->cancelAllGoals();
00554 move_arm_client_ptr_->cancelAllGoals();
00555 move_place_client_ptr_->cancelAllGoals();
00556 joint_traj_client_ptr_->cancelAllGoals();
00557
00558 open_door_client_ptr_->cancelAllGoals();
00559 open_chuck_client_ptr_->cancelAllGoals();
00560 close_door_client_ptr_->cancelAllGoals();
00561 close_chuck_client_ptr_->cancelAllGoals();
00562
00563 grasp_action_client_ptr_->cancelAllGoals();
00564 vise_action_client_ptr_->cancelAllGoals();
00565 }
00566
00567 bool StateMachine::all_action_servers_connected()
00568 {
00569
00570 if(!(move_pickup_client_ptr_->isServerConnected() && move_place_client_ptr_->isServerConnected()&&
00571 move_arm_client_ptr_->isServerConnected() && joint_traj_client_ptr_->isServerConnected()))
00572 {
00573 set_active_state(states::ROBOT_FAULT);
00574 return false;
00575 }
00576
00577 if(!(open_door_client_ptr_->isServerConnected() && close_door_client_ptr_->isServerConnected() &&
00578 open_chuck_client_ptr_->isServerConnected() && close_chuck_client_ptr_->isServerConnected()))
00579 {
00580 set_active_state(states::CNC_FAULT);
00581 return false;
00582 }
00583
00584 if(!grasp_action_client_ptr_->isServerConnected() && !vise_action_client_ptr_->isServerConnected())
00585 {
00586 set_active_state(states::GRIPPER_FAULT);
00587 return false;
00588 }
00589
00590 return true;
00591
00592 }
00593
00594
00595 bool StateMachine::on_startup()
00596 {
00597 return setup();
00598
00599 }
00600
00601 bool StateMachine::on_ready()
00602 {
00603
00604 if(!mat_load_set_state_.response.accepted)
00605 {
00606
00607 if(material_load_set_state_client_.exists() &&
00608 material_load_set_state_client_.call(mat_load_set_state_.request,mat_load_set_state_.response))
00609 {
00610 ROS_INFO_STREAM("set state service call "<< (mat_load_set_state_.response.accepted ? "accepted" : "rejected"));
00611 }
00612 else
00613 {
00614 ROS_WARN_STREAM("set state service call failed");
00615 ros::Duration(DURATION_LOOP_PAUSE).sleep();
00616 }
00617 }
00618
00619 if(!mat_unload_set_state_.response.accepted)
00620 {
00621
00622 if(material_unload_set_state_client_.exists() &&
00623 material_unload_set_state_client_.call(mat_unload_set_state_.request,mat_unload_set_state_.response))
00624 {
00625 ROS_INFO_STREAM("set state service call "<< (mat_unload_set_state_.response.accepted ? "accepted" : "rejected"));
00626 }
00627 else
00628 {
00629 ROS_WARN_STREAM("set state service call failed");
00630 ros::Duration(DURATION_LOOP_PAUSE).sleep();
00631 }
00632 }
00633
00634 return mat_load_set_state_.response.accepted && mat_unload_set_state_.response.accepted;
00635 }
00636
00637 bool StateMachine::on_robot_reset()
00638 {
00639
00640 mat_load_set_state_.response.accepted = false;
00641 mat_unload_set_state_.response.accepted = false;
00642 return true;
00643 }
00644
00645 bool StateMachine::on_material_load_started()
00646 {
00647 current_task_index_ = 0;
00648 if( use_task_desc_motion)
00649 {
00650 ROS_INFO("Executing NEW dumb joint move material load");
00651 current_task_sequence_.assign(jm_material_load_task_sequence_.begin(), jm_material_load_task_sequence_.end());
00652 }
00653
00654 else
00655 {
00656 ROS_INFO("Executing OLD smart path planning material load");
00657 current_task_sequence_.assign(material_load_task_sequence_.begin(),material_load_task_sequence_.end());
00658 }
00659 run_next_task();
00660 return true;
00661 }
00662
00663 bool StateMachine::on_test_task_started()
00664 {
00665
00666 cancel_active_action_goals();
00667 cancel_active_material_requests();
00668
00669 current_task_index_ = 0;
00670 current_task_sequence_.clear();
00671 current_task_sequence_.push_back(tasks::TEST_TASK_START);
00672 current_task_sequence_.push_back(test_task_id_);
00673 current_task_sequence_.push_back(tasks::TEST_TASK_END);
00674 run_next_task();
00675
00676 return true;
00677 }
00678
00679 bool StateMachine::on_test_task_completed()
00680 {
00681 return true;
00682 }
00683
00684 bool StateMachine::on_material_load_completed()
00685 {
00686 MaterialLoadServer::Result res;
00687 res.load_state= "Succeeded";
00688
00689 if(material_load_server_ptr_->isActive())
00690 {
00691 material_load_server_ptr_->setSucceeded(res);
00692 ROS_INFO_STREAM("MATERIAL LOAD request succeeded");
00693 }
00694
00695
00696 return true;
00697 }
00698
00699 bool StateMachine::on_material_unload_started()
00700 {
00701 current_task_index_ = 0;
00702
00703 if( use_task_desc_motion)
00704 {
00705 ROS_INFO("Executing NEW dumb joint move material unload");
00706 current_task_sequence_.assign(jm_material_unload_task_sequence_.begin(), jm_material_unload_task_sequence_.end());
00707 }
00708
00709 else
00710 {
00711 ROS_INFO("Executing OLD smart path planning material unload");
00712 current_task_sequence_.assign(material_unload_task_sequence_.begin(),material_unload_task_sequence_.end());
00713 }
00714
00715
00716 run_next_task();
00717 return true;
00718 }
00719
00720 bool StateMachine::on_material_unload_completed()
00721 {
00722 MaterialUnloadServer::Result res;
00723 res.unload_state= "Succeeded";
00724 if(material_unload_server_ptr_->isActive())
00725 {
00726 material_unload_server_ptr_->setSucceeded(res);
00727 ROS_INFO_STREAM("MATERIAL UNLOAD request succeeded");
00728 }
00729
00730 return true;
00731 }
00732
00733 bool StateMachine::on_cnc_reset()
00734 {
00735 return true;
00736 }
00737
00738 bool StateMachine::on_peripherals_reset()
00739 {
00740 return true;
00741 }
00742
00743 bool StateMachine::on_robot_moving()
00744 {
00745 using namespace mtconnect_cnc_robot_example::state_machine::tasks;
00746
00747
00748 if(get_param_fault_on_task_check(current_task_sequence_[current_task_index_]))
00749 {
00750 ROS_WARN_STREAM("Forcing fault on task "<<tasks::TASK_MAP[current_task_sequence_[current_task_index_]]);
00751 set_active_state(states::ROBOT_FAULT);
00752 return true;
00753 }
00754
00755 int state = actionlib::SimpleClientGoalState::ACTIVE;
00756 switch(current_task_sequence_[current_task_index_])
00757 {
00758 case ROBOT_PICKUP_MATERIAL:
00759
00760 state = move_pickup_client_ptr_->getState().state_;
00761 break;
00762
00763 case ROBOT_PLACE_WORKPIECE:
00764
00765 state = move_place_client_ptr_->getState().state_;
00766 break;
00767
00768 default:
00769
00770 if( use_task_desc_motion )
00771 {
00772 state = joint_traj_client_ptr_->getState().state_;
00773 }
00774 else
00775 {
00776 state = move_arm_client_ptr_->getState().state_;
00777 }
00778 break;
00779 }
00780
00781
00782 switch(state)
00783 {
00784 case actionlib::SimpleClientGoalState::ACTIVE:
00785 break;
00786
00787 case actionlib::SimpleClientGoalState::SUCCEEDED:
00788 run_next_task();
00789 break;
00790
00791 case actionlib::SimpleClientGoalState::LOST:
00792 case actionlib::SimpleClientGoalState::REJECTED:
00793 case actionlib::SimpleClientGoalState::ABORTED:
00794
00795 set_active_state(states::ROBOT_FAULT);
00796 break;
00797 }
00798
00799 return true;
00800 }
00801
00802 bool StateMachine::on_cnc_moving()
00803 {
00804 using namespace mtconnect_cnc_robot_example::state_machine::tasks;
00805
00806
00807 if(get_param_fault_on_task_check(current_task_sequence_[current_task_index_]))
00808 {
00809 ROS_WARN_STREAM("Forcing fault on task "<<tasks::TASK_MAP[current_task_sequence_[current_task_index_]]);
00810 set_active_state(states::CNC_FAULT);
00811 return true;
00812 }
00813
00814 int state = actionlib::SimpleClientGoalState::ACTIVE;
00815 switch(current_task_sequence_[current_task_index_])
00816 {
00817 case CNC_CLOSE_CHUCK:
00818
00819 state = close_chuck_client_ptr_->getState().state_;
00820 break;
00821
00822 case CNC_OPEN_CHUCK:
00823
00824 state = open_chuck_client_ptr_->getState().state_;
00825 break;
00826
00827 case CNC_CLOSE_DOOR:
00828
00829 state = close_door_client_ptr_->getState().state_;
00830 break;
00831
00832 case CNC_OPEN_DOOR:
00833
00834 state = open_door_client_ptr_->getState().state_;
00835 break;
00836 }
00837
00838
00839 switch(state)
00840 {
00841 case actionlib::SimpleClientGoalState::ACTIVE:
00842 break;
00843
00844 case actionlib::SimpleClientGoalState::SUCCEEDED:
00845 run_next_task();
00846 break;
00847
00848 case actionlib::SimpleClientGoalState::LOST:
00849 case actionlib::SimpleClientGoalState::REJECTED:
00850 case actionlib::SimpleClientGoalState::ABORTED:
00851
00852 set_active_state(states::CNC_FAULT);
00853 break;
00854 }
00855
00856 return true;
00857 }
00858
00859 bool StateMachine::on_gripper_moving()
00860 {
00861 using namespace mtconnect_cnc_robot_example::state_machine::tasks;
00862
00863
00864 if(get_param_fault_on_task_check(current_task_sequence_[current_task_index_]))
00865 {
00866 ROS_WARN_STREAM("Forcing fault on task "<<tasks::TASK_MAP[current_task_sequence_[current_task_index_]]);
00867 set_active_state(states::GRIPPER_FAULT);
00868 return true;
00869 }
00870
00871 int state = actionlib::SimpleClientGoalState::ACTIVE;
00872 switch(current_task_sequence_[current_task_index_])
00873 {
00874 case GRIPPER_CLOSE:
00875 case GRIPPER_OPEN:
00876 state = grasp_action_client_ptr_->getState().state_;
00877 break;
00878
00879 case VISE_CLOSE:
00880 case VISE_OPEN:
00881 state = vise_action_client_ptr_->getState().state_;
00882 break;
00883 }
00884
00885
00886 switch(state)
00887 {
00888 case actionlib::SimpleClientGoalState::ACTIVE:
00889 break;
00890
00891 case actionlib::SimpleClientGoalState::SUCCEEDED:
00892 run_next_task();
00893 break;
00894
00895 case actionlib::SimpleClientGoalState::LOST:
00896 case actionlib::SimpleClientGoalState::REJECTED:
00897 case actionlib::SimpleClientGoalState::ABORTED:
00898
00899 set_active_state(states::GRIPPER_FAULT);
00900 break;
00901 }
00902
00903 return true;
00904 }
00905
00906 bool StateMachine::on_robot_fault()
00907 {
00908 cancel_active_material_requests();
00909 cancel_active_action_goals();
00910 return true;
00911 }
00912
00913 bool StateMachine::on_cnc_fault()
00914 {
00915 cancel_active_material_requests();
00916 cancel_active_action_goals();
00917 return true;
00918 }
00919
00920 bool StateMachine::on_gripper_fault()
00921 {
00922 cancel_active_material_requests();
00923 cancel_active_action_goals();
00924 return true;
00925 }
00926
00927
00928 void StateMachine::get_param_force_fault_flags()
00929 {
00930 ros::NodeHandle nh("~");
00931
00932 bool force_robot_fault, force_cnc_fault, force_gripper_fault;
00933 if(nh.getParam(PARAM_FORCE_ROBOT_FAULT,force_robot_fault) && force_robot_fault)
00934 {
00935 ROS_INFO_STREAM("Forcing 'ROBOT_FAULT'");
00936 set_active_state(states::ROBOT_FAULT);
00937 nh.setParam(PARAM_FORCE_ROBOT_FAULT,false);
00938 }
00939
00940 if(nh.getParam(PARAM_FORCE_CNC_FAULT,force_cnc_fault) && force_cnc_fault)
00941 {
00942 ROS_INFO_STREAM("Forcing 'CNC_FAULT'");
00943 set_active_state(states::CNC_FAULT);
00944 nh.setParam(PARAM_FORCE_CNC_FAULT,false);
00945 }
00946
00947 if(nh.getParam(PARAM_FORCE_GRIPPER_FAULT,force_gripper_fault) && force_gripper_fault)
00948 {
00949 ROS_INFO_STREAM("Forcing 'GRIPPER_FAULT'");
00950 set_active_state(states::GRIPPER_FAULT);
00951 nh.setParam(PARAM_FORCE_GRIPPER_FAULT,false);
00952 }
00953
00954 }
00955
00956 bool StateMachine::get_param_fault_on_task_check(int task_id)
00957 {
00958 ros::NodeHandle nh("~");
00959 int fault_on_task_id;
00960
00961
00962 if(nh.getParam(PARAM_FORCE_FAULT_ON_TASK,fault_on_task_id) && fault_on_task_id != tasks::NO_TASK &&
00963 task_id == fault_on_task_id )
00964 {
00965
00966 return true;
00967 }
00968 else
00969 {
00970 return false;
00971 }
00972 }
00973
00974 bool StateMachine::check_arm_at_position(sensor_msgs::JointState &joints, double tolerance)
00975 {
00976
00977 sensor_msgs::JointStateConstPtr actual_joints_ptr;
00978 sensor_msgs::JointState actual_joints;
00979 bool success = true;
00980 int joint_index = 0;
00981
00982
00983 ros::NodeHandle nh;
00984 actual_joints_ptr = ros::topic::waitForMessage<sensor_msgs::JointState>(DEFAULT_JOINT_STATE_TOPIC,nh
00985 ,ros::Duration(DURATION_JOINT_MESSAGE_TIMEOUT));
00986
00987 if(actual_joints_ptr.get() != NULL )
00988 {
00989
00990 for(int i = 0; i < joints.name.size(); i++)
00991 {
00992 std::string &name = joints.name[i];
00993 std::vector<std::string>::const_iterator iter_pos = std::find(actual_joints_ptr->name.begin(),
00994 actual_joints_ptr->name.end(),name);
00995
00996 if(iter_pos != actual_joints_ptr->name.end())
00997 {
00998 joint_index = 0;
00999 while(actual_joints_ptr->name[joint_index].compare(name)!=0)
01000 {
01001 joint_index++;
01002 }
01003
01004
01005 std::vector<double> &vals = joints.position;
01006 const std::vector<double> &true_vals = actual_joints_ptr->position;
01007 if( (joint_index >= joints.name.size()) || (std::abs(vals[i] - true_vals[joint_index]) > tolerance))
01008 {
01009 success = false;
01010 break;
01011 }
01012 }
01013 else
01014 {
01015 ROS_ERROR_STREAM("Joint "<<name<<" not found, failed joint position check");
01016 success = false;
01017 break;
01018 }
01019 }
01020
01021 }
01022 else
01023 {
01024 ROS_ERROR_STREAM("Joint state message not received, failed joint position check");
01025 success = false;
01026 }
01027
01028 return success;
01029 }
01030
01031
01032 void StateMachine::material_load_goalcb()
01033 {
01034
01035 if(get_active_state()== states::READY)
01036 {
01037 material_load_server_ptr_->acceptNewGoal();
01038 set_active_state(states::MATERIAL_LOAD_STARTED);
01039 }
01040
01041 }
01042
01043 void StateMachine::material_unload_goalcb()
01044 {
01045 if(get_active_state()== states::READY)
01046 {
01047 material_unload_server_ptr_->acceptNewGoal();
01048 set_active_state(states::MATERIAL_UNLOAD_STARTED);
01049 }
01050 }
01051
01052 void StateMachine::ros_status_subs_cb(const industrial_msgs::RobotStatusConstPtr &msg)
01053 {
01054 if(msg->e_stopped.val == industrial_msgs::TriState::ENABLED)
01055 {
01056 ROS_INFO_STREAM("Received 'e_stopped' enabled, going to ROBOT_FAULT");
01057 set_active_state(states::ROBOT_FAULT);
01058 }
01059 }
01060
01061 bool StateMachine::external_command_cb(mtconnect_cnc_robot_example::Command::Request &req,
01062 mtconnect_cnc_robot_example::Command::Response &res)
01063 {
01064
01065 switch(req.command_flag)
01066 {
01067 case Command::Request::FAULT_RESET:
01068
01069 if(get_active_state() == states::ROBOT_FAULT && check_arm_at_position(joint_home_pos_,DEFAULT_JOINT_ERROR_TOLERANCE))
01070 {
01071 set_active_state(states::ROBOT_RESET);
01072 res.accepted = true;
01073 ROS_INFO_STREAM("Fault reset accepted");
01074 }
01075 else
01076 {
01077 res.accepted = false;
01078 ROS_INFO_STREAM("Fault reset rejected");
01079 }
01080
01081 break;
01082 case Command::Request::RUN_TASK:
01083 test_task_id_ = req.task_id;
01084 set_active_state(states::TEST_TASK_STARTED);
01085 res.accepted = true;
01086 ROS_INFO_STREAM("Run task accepted");
01087 break;
01088
01089 case Command::Request::START:
01090 break;
01091 case Command::Request::EXIT:
01092 break;
01093 default:
01094 break;
01095 }
01096
01097 return true;
01098 }
01099
01100 void StateMachine::publish_robot_topics_timercb(const ros::TimerEvent &evnt)
01101 {
01102
01103 robot_state_msg_.header.stamp = ros::Time::now();
01104 robot_spindle_msg_.header.stamp = ros::Time::now();
01105
01106
01107 robot_states_pub_.publish(robot_state_msg_);
01108 robot_spindle_pub_.publish(robot_spindle_msg_);
01109 }
01110
01111
01112 bool StateMachine::moveArm(move_arm_utils::JointStateInfo &joint_info)
01113 {
01114
01115 move_arm_joint_goal_.motion_plan_request.goal_constraints.joint_constraints.clear();
01116 joint_info.toJointConstraints(DEFAULT_JOINT_ERROR_TOLERANCE,DEFAULT_JOINT_ERROR_TOLERANCE,
01117 move_arm_joint_goal_.motion_plan_request.goal_constraints.joint_constraints);
01118
01119
01120 move_arm_client_ptr_->sendGoal(move_arm_joint_goal_);
01121 return true;
01122 }
01123
01124
01125 bool StateMachine::moveArm(std::string & move_name)
01126 {
01127 joint_traj_goal_.trajectory = *joint_paths_[move_name];
01128 ROS_INFO_STREAM("Sending a joint trajectory with " <<
01129 joint_traj_goal_.trajectory.points.size() << "points");
01130 trajectory_filter_.request.trajectory = joint_traj_goal_.trajectory;
01131 if(!joint_traj_goal_.trajectory.points.empty())
01132 if(trajectory_filter_client_.call(trajectory_filter_))
01133 {
01134 if (trajectory_filter_.response.error_code.val ==
01135 trajectory_filter_.response.error_code.SUCCESS)
01136 {
01137 ROS_INFO("Trajectory successfully filtered...sending goal");
01138 joint_traj_goal_.trajectory = trajectory_filter_.response.trajectory;
01139 ROS_INFO("Copying trajectory to back to goal");
01140 joint_traj_client_ptr_->sendGoal(joint_traj_goal_);
01141 }
01142 else
01143 {
01144 ROS_ERROR("Failed to process filter trajectory, entering fault state");
01145 set_active_state(states::ROBOT_FAULT);
01146 }
01147 }
01148
01149 else
01150 {
01151 ROS_ERROR("Failed to call filter trajectory, entering fault state");
01152 set_active_state(states::ROBOT_FAULT);
01153 }
01154 else
01155 {
01156 ROS_ERROR_STREAM(move_name << " trajectory is empty, failing");
01157 set_active_state(states::ROBOT_FAULT);
01158 }
01159
01160 return true;
01161 }
01162