state_machine.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (Apache License)
3  *
4  * Copyright (c) 2018 Plus One Robotics
5  *
6  * Licensed under the Apache License, Version 2.0 (the "License");
7  * you may not use this file except in compliance with the License.
8  * You may obtain a copy of the License at
9  *
10  * http://www.apache.org/licenses/LICENSE-2.0
11  *
12  * Unless required by applicable law or agreed to in writing, software
13  * distributed under the License is distributed on an "AS IS" BASIS,
14  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15  * See the License for the specific language governing permissions and
16  * limitations under the License.
17  */
18 
19 #include <gtest/gtest.h>
20 #include "state_machine_visited_states_queue.h"
21 #include "packml_sm/common.h"
22 #include "packml_sm/abstract_state_machine.h"
23 #include "packml_sm/boost/packml_state_machine_single_cycle.h"
24 #include "packml_sm/boost/packml_state_machine_continuous.h"
25 #include "ros/duration.h"
26 #include "ros/console.h"
27 #include "ros/rate.h"
28 
29 namespace packml_sm_test
30 {
31 using namespace packml_sm;
32 
39 bool waitForState(StatesEnum state, StateMachineVisitedStatesQueue& queue)
40 {
41  auto i = 0;
42  do
43  {
44  auto nextState = queue.nextState();
45 
46  if (nextState == static_cast<int>(state))
47  {
48  return true;
49  }
50  else if (nextState != -1)
51  {
52  return false;
53  }
54 
55  ROS_WARN_STREAM("Waiting for state to change to " << state);
56  ros::Duration(0.05).sleep();
57  i++;
58  } while (i < 25);
59 
60  return false;
61 }
62 
63 int success()
64 {
65  ROS_INFO_STREAM("Beginning success() method");
66  ros::Duration(1.0).sleep();
67  ROS_INFO_STREAM("Execute method complete");
68  return 0;
69 }
70 
71 int fail()
72 {
73  ROS_INFO_STREAM("Beginning fail method");
74  ros::Duration(1.0).sleep();
75  ROS_INFO_STREAM("Execute method complete");
76  return -1;
77 }
78 
79 TEST(Packml_SM, set_execute)
80 {
81  std::shared_ptr<AbstractStateMachine> sm = PackmlStateMachineSingleCycle::spawn();
82  StateMachineVisitedStatesQueue queue(sm);
83  sm->setExecute(std::bind(success));
84  sm->activate();
85  ros::Duration(1.0).sleep(); // give time to start
86  ASSERT_TRUE(waitForState(StatesEnum::ABORTED, queue));
87  ASSERT_TRUE(sm->clear());
88  ASSERT_TRUE(waitForState(StatesEnum::CLEARING, queue));
89  ASSERT_TRUE(waitForState(StatesEnum::STOPPED, queue));
90  ASSERT_TRUE(sm->reset());
91  ASSERT_TRUE(waitForState(StatesEnum::RESETTING, queue));
92  ASSERT_TRUE(waitForState(StatesEnum::IDLE, queue));
93  ASSERT_TRUE(sm->start());
94  ASSERT_TRUE(waitForState(StatesEnum::STARTING, queue));
95  ASSERT_TRUE(waitForState(StatesEnum::EXECUTE, queue));
96  ASSERT_TRUE(waitForState(StatesEnum::COMPLETING, queue));
97  ASSERT_TRUE(waitForState(StatesEnum::COMPLETE, queue));
98  ASSERT_TRUE(sm->reset());
99  ASSERT_TRUE(waitForState(StatesEnum::RESETTING, queue));
100  ASSERT_TRUE(waitForState(StatesEnum::IDLE, queue));
101  sm->setExecute(std::bind(fail));
102  ASSERT_TRUE(sm->start());
103  ASSERT_TRUE(waitForState(StatesEnum::STARTING, queue));
104  ASSERT_TRUE(waitForState(StatesEnum::EXECUTE, queue));
105  ASSERT_TRUE(waitForState(StatesEnum::ABORTING, queue));
106  ASSERT_TRUE(waitForState(StatesEnum::ABORTED, queue));
107 }
108 
109 TEST(Packml_SC, state_diagram)
110 {
111  ROS_INFO_STREAM("SINGLE CYCLE::State diagram");
112  std::shared_ptr<AbstractStateMachine> sm = PackmlStateMachineSingleCycle::spawn();
113  StateMachineVisitedStatesQueue queue(sm);
114 
115  EXPECT_FALSE(sm->isActive());
116  sm->setExecute(std::bind(success));
117  sm->activate();
118  ros::Duration(1.0).sleep(); // give time to start
119  EXPECT_TRUE(sm->isActive());
120 
121  ASSERT_TRUE(waitForState(StatesEnum::ABORTED, queue));
122  ASSERT_TRUE(sm->isActive());
123 
124  ASSERT_TRUE(sm->clear());
125  ASSERT_TRUE(waitForState(StatesEnum::CLEARING, queue));
126  ASSERT_TRUE(waitForState(StatesEnum::STOPPED, queue));
127 
128  ASSERT_TRUE(sm->reset());
129  ASSERT_TRUE(waitForState(StatesEnum::RESETTING, queue));
130  ASSERT_TRUE(waitForState(StatesEnum::IDLE, queue));
131 
132  ASSERT_TRUE(sm->start());
133  ASSERT_TRUE(waitForState(StatesEnum::STARTING, queue));
134  ASSERT_TRUE(waitForState(StatesEnum::EXECUTE, queue));
135  ASSERT_TRUE(waitForState(StatesEnum::COMPLETING, queue));
136  ASSERT_TRUE(waitForState(StatesEnum::COMPLETE, queue));
137 
138  ASSERT_TRUE(sm->reset());
139  ASSERT_TRUE(waitForState(StatesEnum::RESETTING, queue));
140  ASSERT_TRUE(waitForState(StatesEnum::IDLE, queue));
141 
142  ASSERT_TRUE(sm->start());
143  ASSERT_TRUE(waitForState(StatesEnum::STARTING, queue));
144  ASSERT_TRUE(waitForState(StatesEnum::EXECUTE, queue));
145  ASSERT_TRUE(sm->hold());
146  ASSERT_TRUE(waitForState(StatesEnum::HOLDING, queue));
147  ASSERT_TRUE(waitForState(StatesEnum::HELD, queue));
148  ASSERT_TRUE(sm->unhold());
149  ASSERT_TRUE(waitForState(StatesEnum::UNHOLDING, queue));
150  ASSERT_TRUE(waitForState(StatesEnum::EXECUTE, queue));
151 
152  ASSERT_TRUE(sm->suspend());
153  ASSERT_TRUE(waitForState(StatesEnum::SUSPENDING, queue));
154  ASSERT_TRUE(waitForState(StatesEnum::SUSPENDED, queue));
155  ASSERT_TRUE(sm->unsuspend());
156  ASSERT_TRUE(waitForState(StatesEnum::UNSUSPENDING, queue));
157  ASSERT_TRUE(waitForState(StatesEnum::EXECUTE, queue));
158 
159  ASSERT_TRUE(sm->stop());
160  ASSERT_TRUE(waitForState(StatesEnum::STOPPING, queue));
161  ASSERT_TRUE(waitForState(StatesEnum::STOPPED, queue));
162 
163  ASSERT_TRUE(sm->abort());
164  ASSERT_TRUE(waitForState(StatesEnum::ABORTING, queue));
165  ASSERT_TRUE(waitForState(StatesEnum::ABORTED, queue));
166 
167  sm->deactivate();
168  ros::Duration(1).sleep();
169  EXPECT_FALSE(sm->isActive());
170  ROS_INFO_STREAM("State diagram test complete");
171 }
172 
173 TEST(Packml_CC, state_diagram)
174 {
175  ROS_INFO_STREAM("CONTINUOUS CYCLE::State diagram");
176  std::shared_ptr<AbstractStateMachine> sm = PackmlStateMachineContinuous::spawn();
177  StateMachineVisitedStatesQueue queue(sm);
178  EXPECT_FALSE(sm->isActive());
179  sm->setExecute(std::bind(success));
180  sm->activate();
181  ros::Duration(1.0).sleep(); // give time to start
182  EXPECT_TRUE(sm->isActive());
183 
184  ASSERT_TRUE(waitForState(StatesEnum::ABORTED, queue));
185  ASSERT_TRUE(sm->isActive());
186 
187  ASSERT_TRUE(sm->clear());
188  ASSERT_TRUE(waitForState(StatesEnum::CLEARING, queue));
189  ASSERT_TRUE(waitForState(StatesEnum::STOPPED, queue));
190 
191  ASSERT_TRUE(sm->reset());
192  ASSERT_TRUE(waitForState(StatesEnum::RESETTING, queue));
193  ASSERT_TRUE(waitForState(StatesEnum::IDLE, queue));
194 
195  ASSERT_TRUE(sm->start());
196  ASSERT_TRUE(waitForState(StatesEnum::STARTING, queue));
197  ASSERT_TRUE(waitForState(StatesEnum::EXECUTE, queue));
198  ASSERT_FALSE(waitForState(StatesEnum::COMPLETING, queue));
199  ASSERT_FALSE(waitForState(StatesEnum::COMPLETE, queue));
200 
201  ASSERT_TRUE(sm->hold());
202  ASSERT_TRUE(waitForState(StatesEnum::HOLDING, queue));
203  ASSERT_TRUE(waitForState(StatesEnum::HELD, queue));
204 
205  ASSERT_TRUE(sm->unhold());
206  ASSERT_TRUE(waitForState(StatesEnum::UNHOLDING, queue));
207  ASSERT_TRUE(waitForState(StatesEnum::EXECUTE, queue));
208 
209  ASSERT_TRUE(sm->suspend());
210  ASSERT_TRUE(waitForState(StatesEnum::SUSPENDING, queue));
211  ASSERT_TRUE(waitForState(StatesEnum::SUSPENDED, queue));
212 
213  ASSERT_TRUE(sm->unsuspend());
214  ASSERT_TRUE(waitForState(StatesEnum::UNSUSPENDING, queue));
215  ASSERT_TRUE(waitForState(StatesEnum::EXECUTE, queue));
216 
217  ASSERT_TRUE(sm->stop());
218  ASSERT_TRUE(waitForState(StatesEnum::STOPPING, queue));
219  ASSERT_TRUE(waitForState(StatesEnum::STOPPED, queue));
220 
221  ASSERT_TRUE(sm->abort());
222  ASSERT_TRUE(waitForState(StatesEnum::ABORTING, queue));
223  ASSERT_TRUE(waitForState(StatesEnum::ABORTED, queue));
224 
225  sm->deactivate();
226  ros::Duration(1).sleep();
227  EXPECT_FALSE(sm->isActive());
228  ROS_INFO_STREAM("State diagram test complete");
229 }
230 }
packml_sm::PackmlStateMachineSingleCycle::spawn
static std::shared_ptr< PackmlStateMachineSingleCycle > spawn()
Definition: packml_state_machine_single_cycle.h:28
packml_state_machine_continuous.h
ros::Duration::sleep
bool sleep() const
abstract_state_machine.h
packml_sm_test
Definition: state_machine.cpp:29
packml_sm_test::success
int success()
Definition: state_machine.cpp:63
packml_sm
Definition: abstract_state_machine.h:29
packml_sm_test::TEST
TEST(Packml_SM, set_execute)
Definition: state_machine.cpp:79
packml_sm::StatesEnum
StatesEnum
Definition: common.h:35
ROS_WARN_STREAM
#define ROS_WARN_STREAM(args)
packml_sm_test::waitForState
bool waitForState(StatesEnum state, StateMachineVisitedStatesQueue &queue)
waitForState - returns true if the current state of the state machine (sm) matches the queried state ...
Definition: state_machine.cpp:39
packml_sm::StatesEnum::IDLE
ROS_INFO_STREAM
#define ROS_INFO_STREAM(args)
packml_sm::PackmlStateMachineContinuous::spawn
static std::shared_ptr< PackmlStateMachineContinuous > spawn()
Definition: packml_state_machine_continuous.h:27
state_machine_visited_states_queue.h
packml_sm_test::fail
int fail()
Definition: state_machine.cpp:71
packml_state_machine_single_cycle.h

packml_sm
Author(s): Shaun Edwards
autogenerated on Fri Jul 12 2019 03:30:55