packml_sm: state_machine.cpp Source File

Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (Apache License)
00003  *
00004  * Copyright (c) 2016 Shaun Edwards
00005  *
00006  * Licensed under the Apache License, Version 2.0 (the "License");
00007  * you may not use this file except in compliance with the License.
00008  * You may obtain a copy of the License at
00009  *
00010  * http://www.apache.org/licenses/LICENSE-2.0
00011  *
00012  * Unless required by applicable law or agreed to in writing, software
00013  * distributed under the License is distributed on an "AS IS" BASIS,
00014  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00015  * See the License for the specific language governing permissions and
00016  * limitations under the License.
00017  */
00018 
00019 #include <gtest/gtest.h>
00020 #include "packml_sm/common.h"
00021 #include "packml_sm/events.h"
00022 #include "packml_sm/state_machine.h"
00023 #include "ros/duration.h"
00024 #include "ros/console.h"
00025 #include "ros/rate.h"
00026 
00027 namespace packml_sm_test
00028 {
00029 
00030 using namespace packml_sm;
00031 
00038 bool waitForState(StatesEnum state, StateMachine & sm)
00039 {
00040   const double TIMEOUT = 2.0;
00041   const int SAMPLES = 50;
00042   ros::Rate r(double(SAMPLES)/TIMEOUT);
00043   for(int ii=0; ii<SAMPLES; ++ii)
00044   {
00045     if(sm.getCurrentState() == static_cast<int>(state))
00046     {
00047       ROS_INFO_STREAM("State changed to " << state);
00048       return true;
00049     }
00050     ROS_DEBUG_STREAM("Waiting for state to change to " << state);
00051     r.sleep();
00052   }
00053   return false;
00054 }
00055 
00056 int success()
00057 {
00058   ROS_INFO_STREAM("Beginning success() method");
00059   ros::Duration(1.0).sleep();
00060   ROS_INFO_STREAM("Execute method complete");
00061   return 0;
00062 }
00063 
00064 int fail()
00065 {
00066   ROS_INFO_STREAM("Beginning fail method");
00067   ros::Duration(1.0).sleep();
00068   ROS_INFO_STREAM("Execute method complete");
00069   return -1;
00070 }
00071 
00072 
00073 TEST(Packml_SM, set_execute)
00074 {
00075   std::shared_ptr<StateMachine> sm = StateMachine::singleCyleSM();
00076   sm->setExecute(std::bind(success));
00077   sm->activate();
00078   ros::Duration(1.0).sleep();  //give time to start
00079   ASSERT_TRUE(sm->clear());
00080   ASSERT_TRUE(waitForState(StatesEnum::STOPPED, *sm));
00081   ASSERT_TRUE(sm->reset());
00082   ASSERT_TRUE(waitForState(StatesEnum::IDLE, *sm));
00083   ASSERT_TRUE(sm->start());
00084   ASSERT_TRUE(waitForState(StatesEnum::COMPLETE, *sm));
00085   ASSERT_TRUE(sm->reset());
00086   ASSERT_TRUE(waitForState(StatesEnum::IDLE, *sm));
00087   sm->setExecute(std::bind(fail));
00088   ASSERT_TRUE(sm->start());
00089   ASSERT_TRUE(waitForState(StatesEnum::ABORTED, *sm));
00090 }
00091 
00092 TEST(Packml_SC, state_diagram)
00093 {
00094   ROS_INFO_STREAM("SINGLE CYCLE::State diagram");
00095   SingleCycle sm;
00096   EXPECT_FALSE(sm.isActive());
00097   sm.setExecute(std::bind(success));
00098   sm.activate();
00099   ros::Duration(1.0).sleep();  //give time to start
00100   EXPECT_TRUE(sm.isActive());
00101 
00102   ASSERT_TRUE(waitForState(StatesEnum::ABORTED, sm));
00103   ASSERT_TRUE(sm.isActive());
00104 
00105   ASSERT_TRUE(sm.clear());
00106   ASSERT_TRUE(waitForState(StatesEnum::CLEARING, sm));
00107   ASSERT_TRUE(waitForState(StatesEnum::STOPPED, sm));
00108 
00109   ASSERT_TRUE(sm.reset());
00110   ASSERT_TRUE(waitForState(StatesEnum::RESETTING, sm));
00111   ASSERT_TRUE(waitForState(StatesEnum::IDLE, sm));
00112 
00113   ASSERT_TRUE(sm.start());
00114   ASSERT_TRUE(waitForState(StatesEnum::STARTING, sm));
00115   ASSERT_TRUE(waitForState(StatesEnum::EXECUTE, sm));
00116   ASSERT_TRUE(waitForState(StatesEnum::COMPLETING, sm));
00117   ASSERT_TRUE(waitForState(StatesEnum::COMPLETE, sm));
00118 
00119   ASSERT_TRUE(sm.reset());
00120   ASSERT_TRUE(waitForState(StatesEnum::RESETTING, sm));
00121   ASSERT_TRUE(waitForState(StatesEnum::IDLE, sm));
00122 
00123   ASSERT_TRUE(sm.start());
00124   ASSERT_TRUE(waitForState(StatesEnum::STARTING, sm));
00125   ASSERT_TRUE(waitForState(StatesEnum::EXECUTE, sm));
00126 
00127   ASSERT_TRUE(sm.hold());
00128   ASSERT_TRUE(waitForState(StatesEnum::HOLDING, sm));
00129   ASSERT_TRUE(waitForState(StatesEnum::HELD, sm));
00130 
00131   ASSERT_TRUE(sm.unhold());
00132   ASSERT_TRUE(waitForState(StatesEnum::UNHOLDING, sm));
00133   ASSERT_TRUE(waitForState(StatesEnum::EXECUTE, sm));
00134 
00135   ASSERT_TRUE(sm.suspend());
00136   ASSERT_TRUE(waitForState(StatesEnum::SUSPENDING, sm));
00137   ASSERT_TRUE(waitForState(StatesEnum::SUSPENDED, sm));
00138 
00139   ASSERT_TRUE(sm.unsuspend());
00140   ASSERT_TRUE(waitForState(StatesEnum::UNSUSPENDING, sm));
00141   ASSERT_TRUE(waitForState(StatesEnum::EXECUTE, sm));
00142 
00143   ASSERT_TRUE(sm.stop());
00144   ASSERT_TRUE(waitForState(StatesEnum::STOPPING, sm));
00145   ASSERT_TRUE(waitForState(StatesEnum::STOPPED, sm));
00146 
00147   ASSERT_TRUE(sm.abort());
00148   ASSERT_TRUE(waitForState(StatesEnum::ABORTING, sm));
00149   ASSERT_TRUE(waitForState(StatesEnum::ABORTED, sm));
00150 
00151   sm.deactivate();
00152   ros::Duration(1).sleep();
00153   EXPECT_FALSE(sm.isActive());
00154   ROS_INFO_STREAM("State diagram test complete");
00155 }
00156 
00157 
00158 TEST(Packml_CC, state_diagram)
00159 {
00160   ROS_INFO_STREAM("CONTINUOUS CYCLE::State diagram");
00161   ContinuousCycle sm;
00162   EXPECT_FALSE(sm.isActive());
00163   sm.setExecute(std::bind(success));
00164   sm.activate();
00165   ros::Duration(1.0).sleep();  //give time to start
00166   EXPECT_TRUE(sm.isActive());
00167 
00168   ASSERT_TRUE(waitForState(StatesEnum::ABORTED, sm));
00169   ASSERT_TRUE(sm.isActive());
00170 
00171   ASSERT_TRUE(sm.clear());
00172   ASSERT_TRUE(waitForState(StatesEnum::CLEARING, sm));
00173   ASSERT_TRUE(waitForState(StatesEnum::STOPPED, sm));
00174 
00175   ASSERT_TRUE(sm.reset());
00176   ASSERT_TRUE(waitForState(StatesEnum::RESETTING, sm));
00177   ASSERT_TRUE(waitForState(StatesEnum::IDLE, sm));
00178 
00179   ASSERT_TRUE(sm.start());
00180   ASSERT_TRUE(waitForState(StatesEnum::STARTING, sm));
00181   ASSERT_TRUE(waitForState(StatesEnum::EXECUTE, sm));
00182   ASSERT_FALSE(waitForState(StatesEnum::COMPLETING, sm));
00183   ASSERT_FALSE(waitForState(StatesEnum::COMPLETE, sm));
00184 
00185   ASSERT_TRUE(sm.hold());
00186   ASSERT_TRUE(waitForState(StatesEnum::HOLDING, sm));
00187   ASSERT_TRUE(waitForState(StatesEnum::HELD, sm));
00188 
00189   ASSERT_TRUE(sm.unhold());
00190   ASSERT_TRUE(waitForState(StatesEnum::UNHOLDING, sm));
00191   ASSERT_TRUE(waitForState(StatesEnum::EXECUTE, sm));
00192 
00193   ASSERT_TRUE(sm.suspend());
00194   ASSERT_TRUE(waitForState(StatesEnum::SUSPENDING, sm));
00195   ASSERT_TRUE(waitForState(StatesEnum::SUSPENDED, sm));
00196 
00197   ASSERT_TRUE(sm.unsuspend());
00198   ASSERT_TRUE(waitForState(StatesEnum::UNSUSPENDING, sm));
00199   ASSERT_TRUE(waitForState(StatesEnum::EXECUTE, sm));
00200 
00201   ASSERT_TRUE(sm.stop());
00202   ASSERT_TRUE(waitForState(StatesEnum::STOPPING, sm));
00203   ASSERT_TRUE(waitForState(StatesEnum::STOPPED, sm));
00204 
00205   ASSERT_TRUE(sm.abort());
00206   ASSERT_TRUE(waitForState(StatesEnum::ABORTING, sm));
00207   ASSERT_TRUE(waitForState(StatesEnum::ABORTED, sm));
00208 
00209   sm.deactivate();
00210   ros::Duration(1).sleep();
00211   EXPECT_FALSE(sm.isActive());
00212   ROS_INFO_STREAM("State diagram test complete");
00213 }
00214 
00215 
00216 
00217 }