Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008 #include <mtconnect_cnc_robot_example/state_machine/state_machine_interface.h>
00009 using namespace mtconnect_cnc_robot_example::state_machine;
00010
00011 #define SET_AND_WAIT_UNTIL_ACTIVE(sm,st) sm->set_active_state(st);while(sm->get_active_state() != st){};ros::Duration(0.5f).sleep()
00012
00013 int main(int argc,char** argv)
00014 {
00015 ros::init(argc,argv,"test_state_machine");
00016 ros::NodeHandle nh;
00017
00018 StateMachineInterface::Ptr s(new StateMachineInterface());
00019
00020 boost::thread state_machine_thread = boost::thread(&StateMachineInterface::run,s.get());
00021
00022
00023 ros::Duration(0.5f).sleep();
00024 while(s->get_active_state()!= states::READY){};
00025
00026 std::cout<<"Setting "<<states::STATE_MAP[states::MATERIAL_LOAD_STARTED]<<" active"<<std::endl;
00027
00028 SET_AND_WAIT_UNTIL_ACTIVE(s,states::MATERIAL_LOAD_STARTED);
00029
00030 std::cout<<"Setting "<<states::STATE_MAP[states::ROBOT_MOVING]<<" active"<<std::endl;
00031
00032 SET_AND_WAIT_UNTIL_ACTIVE(s,states::ROBOT_MOVING);
00033
00034
00035
00036 std::cout<<"Setting "<<states::STATE_MAP[states::CNC_MOVING]<<" active"<<std::endl;
00037 SET_AND_WAIT_UNTIL_ACTIVE(s,states::CNC_MOVING);
00038
00039
00040 std::cout<<"Setting "<<states::STATE_MAP[states::MATERIAL_LOAD_COMPLETED]<<" active"<<std::endl;
00041 SET_AND_WAIT_UNTIL_ACTIVE(s,states::MATERIAL_LOAD_COMPLETED);
00042
00043
00044 std::cout<<"Setting "<<states::STATE_MAP[states::CNC_FAULT]<<" active"<<std::endl;
00045 SET_AND_WAIT_UNTIL_ACTIVE(s,states::CNC_FAULT);
00046
00047 std::cout<<"Setting "<<states::STATE_MAP[states::ROBOT_FAULT]<<" active"<<std::endl;
00048 SET_AND_WAIT_UNTIL_ACTIVE(s,states::ROBOT_FAULT);
00049
00050 std::cout<<"Setting "<<states::STATE_MAP[states::READY]<<" active"<<std::endl;
00051 SET_AND_WAIT_UNTIL_ACTIVE(s,states::READY);
00052
00053 if(s->get_active_state() == states::READY)
00054 {
00055 std::cout<<states::STATE_MAP[states::READY]<< " state validation passed"<<std::endl;
00056 }
00057
00058 s->set_active_state(states::EXIT);
00059 state_machine_thread.join();
00060
00061 return 0;
00062 }
00063