Gazebo Gazebo

API Reference

3.15.1
StatesImpl.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2021 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16  */
17 
18 /*
19  * \author Nick Lamprianidis <nlamprian@gmail.com>
20  * \date January 2021
21  */
22 
23 #include <memory>
24 #include <string>
25 
26 #include "../ElevatorStateMachine.hh"
27 
28 #include <gz/common/Console.hh>
29 
30 namespace ignition
31 {
32 namespace gazebo
33 {
34 // Inline bracket to help doxygen filtering
35 inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE
36 {
37 namespace systems
38 {
40 struct ElevatorStateMachineDef::IdleState : state<IdleState>
41 {
43  public: template <typename Event, typename FSM>
44  void on_enter(const Event &, FSM &)
45  {
46  ignmsg << "The elevator is idling" << std::endl;
47  }
48 };
49 
51 template <typename E>
52 struct ElevatorStateMachineDef::DoorState : state<DoorState<E>>
53 {
59  public: DoorState(double _posEps = 2e-2, double _velEps = 1e-2)
60  : posEps(_posEps), velEps(_velEps)
61  {
62  }
63 
65  public: virtual ~DoorState()
66  {
67  }
68 
72  public: template <typename Event, typename FSM>
73  void on_enter(const Event &, FSM &_fsm)
74  {
75  const auto &data = _fsm.dataPtr;
76  int32_t floorTarget = data->system->state;
77  ignmsg << "The elevator is " << this->Report(data) << std::endl;
78 
79  double jointTarget = this->JointTarget(data, floorTarget);
80  data->SendCmd(data->system->doorJointCmdPubs[floorTarget], jointTarget);
81  this->triggerEvent = [&_fsm] { _fsm.process_event(E()); };
82  data->system->SetDoorMonitor(
83  floorTarget, jointTarget, this->posEps, this->velEps,
84  std::bind(&DoorState::OnJointTargetReached, this));
85  }
86 
90  private: virtual std::string Report(
91  const std::unique_ptr<ElevatorStateMachinePrivate> &_data) const = 0;
92 
97  private: virtual double JointTarget(
99  int32_t _floorTarget) const = 0;
100 
102  private: void OnJointTargetReached()
103  {
104  this->triggerEvent();
105  }
106 
108  private: double posEps;
109 
111  private: double velEps;
112 
114  private: std::function<void()> triggerEvent;
115 };
116 
118 struct ElevatorStateMachineDef::OpenDoorState : DoorState<events::DoorOpen>
119 {
121  public: using deferred_events = type_tuple<events::EnqueueNewTarget>;
122 
124  public: OpenDoorState() = default;
125 
126  // Documentation inherited
127  private: virtual std::string Report(
128  const std::unique_ptr<ElevatorStateMachinePrivate> &_data) const override
129  {
130  return "opening door " + std::to_string(_data->system->state);
131  }
132 
133  // Documentation inherited
134  private: virtual double JointTarget(
136  int32_t _floorTarget) const override
137  {
138  return _data->system->doorTargets[_floorTarget];
139  }
140 };
141 
143 struct ElevatorStateMachineDef::CloseDoorState : DoorState<events::DoorClosed>
144 {
146  public: CloseDoorState() = default;
147 
148  // Documentation inherited
149  private: virtual std::string Report(
150  const std::unique_ptr<ElevatorStateMachinePrivate> &_data) const override
151  {
152  return "closing door " + std::to_string(_data->system->state);
153  }
154 
155  // Documentation inherited
156  private: virtual double JointTarget(
158  int32_t /*_floorTarget*/) const override
159  {
160  return 0.0;
161  }
162 };
163 
165 struct ElevatorStateMachineDef::WaitState : state<WaitState>
166 {
168  public: using deferred_events = type_tuple<events::EnqueueNewTarget>;
169 
172  public: template <typename Event, typename FSM>
173  void on_enter(const Event &, FSM &_fsm)
174  {
175  const auto &data = _fsm.dataPtr;
176  ignmsg << "The elevator is waiting to close door " << data->system->state
177  << std::endl;
178 
179  this->triggerEvent = [&_fsm] { _fsm.process_event(events::Timeout()); };
180  data->system->StartDoorTimer(data->system->state,
181  std::bind(&WaitState::OnTimeout, this));
182  }
183 
185  private: void OnTimeout()
186  {
187  this->triggerEvent();
188  }
189 
191  private: std::function<void()> triggerEvent;
192 };
193 
195 struct ElevatorStateMachineDef::MoveCabinState : state<MoveCabinState>
196 {
198  public: using deferred_events = type_tuple<events::EnqueueNewTarget>;
199 
205  public: MoveCabinState(double _posEps = 15e-2, double _velEps = 1e-2)
206  : posEps(_posEps), velEps(_velEps)
207  {
208  }
209 
213  public: template <typename Event, typename FSM>
214  void on_enter(const Event &, FSM &_fsm)
215  {
216  const auto &data = _fsm.dataPtr;
217  int32_t floorTarget = data->targets.front();
218  ignmsg << "The elevator is moving the cabin [ " << data->system->state
219  << " -> " << floorTarget << " ]" << std::endl;
220 
221  double jointTarget = data->system->cabinTargets[floorTarget];
222  data->SendCmd(data->system->cabinJointCmdPub, jointTarget);
223  this->triggerEvent = [&_fsm] {
224  _fsm.process_event(events::CabinAtTarget());
225  };
226  data->system->SetCabinMonitor(
227  floorTarget, jointTarget, this->posEps, this->velEps,
228  std::bind(&MoveCabinState::OnJointTargetReached, this));
229  }
230 
232  private: void OnJointTargetReached()
233  {
234  this->triggerEvent();
235  }
236 
238  private: double posEps;
239 
241  private: double velEps;
242 
244  private: std::function<void()> triggerEvent;
245 };
246 
247 } // namespace systems
248 } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE
249 } // namespace gazebo
250 } // namespace ignition
type_tuple< events::EnqueueNewTarget > deferred_events
This state defers EnqueueNewTargetEvent events.
Definition: StatesImpl.hh:198
T bind(T... args)
This library is part of the Ignition Robotics project.
STL class.
MoveCabinState(double _posEps=15e-2, double _velEps=1e-2)
Constructor.
Definition: StatesImpl.hh:205
Event that signifies the cabin has reached the target floor level.
Definition: EventsImpl.hh:71
type_tuple< events::EnqueueNewTarget > deferred_events
This state defers EnqueueNewTargetEvent events.
Definition: StatesImpl.hh:121
void on_enter(const Event &, FSM &)
Logs the entry to the state.
Definition: StatesImpl.hh:44
State at which the elevator is moving the cabin to the target floor.
Definition: StatesImpl.hh:195
State at which the elevator is waiting with a door open.
Definition: StatesImpl.hh:165
T to_string(T... args)
type_tuple< events::EnqueueNewTarget > deferred_events
This state defers EnqueueNewTargetEvent events.
Definition: StatesImpl.hh:168
#define ignmsg
State at which the elevator is closing a door.
Definition: StatesImpl.hh:143
T endl(T... args)
void on_enter(const Event &, FSM &_fsm)
Sends the command that moves the cabin joint to the target floor and configures the monitor that chec...
Definition: StatesImpl.hh:214
State at which the elevator is opening a door.
Definition: StatesImpl.hh:118
void on_enter(const Event &, FSM &_fsm)
Starts the timer that keeps the door at the target floor level open.
Definition: StatesImpl.hh:173
State at which the elevator is idling.
Definition: StatesImpl.hh:40
STL class.
Event that signifies the door at the target floor level has remained open for the required amount of ...
Definition: EventsImpl.hh:66