Gazebo Gazebo

API Reference

6.16.0
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 namespace systems
37 {
39 struct ElevatorStateMachineDef::IdleState : state<IdleState>
40 {
42  public: template <typename Event, typename FSM>
43  void on_enter(const Event &, FSM &)
44  {
45  ignmsg << "The elevator is idling" << std::endl;
46  }
47 };
48 
50 template <typename E>
51 struct ElevatorStateMachineDef::DoorState : state<DoorState<E>>
52 {
58  public: DoorState(double _posEps = 2e-2, double _velEps = 1e-2)
59  : posEps(_posEps), velEps(_velEps)
60  {
61  }
62 
64  public: virtual ~DoorState()
65  {
66  }
67 
71  public: template <typename Event, typename FSM>
72  void on_enter(const Event &, FSM &_fsm)
73  {
74  const auto &data = _fsm.dataPtr;
75  int32_t floorTarget = data->system->state;
76  ignmsg << "The elevator is " << this->Report(data) << std::endl;
77 
78  double jointTarget = this->JointTarget(data, floorTarget);
79  data->SendCmd(data->system->doorJointCmdPubs[floorTarget], jointTarget);
80  this->triggerEvent = [&_fsm] { _fsm.process_event(E()); };
81  data->system->SetDoorMonitor(
82  floorTarget, jointTarget, this->posEps, this->velEps,
83  std::bind(&DoorState::OnJointTargetReached, this));
84  }
85 
89  private: virtual std::string Report(
90  const std::unique_ptr<ElevatorStateMachinePrivate> &_data) const = 0;
91 
96  private: virtual double JointTarget(
98  int32_t _floorTarget) const = 0;
99 
101  private: void OnJointTargetReached()
102  {
103  this->triggerEvent();
104  }
105 
107  private: double posEps;
108 
110  private: double velEps;
111 
113  private: std::function<void()> triggerEvent;
114 };
115 
117 struct ElevatorStateMachineDef::OpenDoorState : DoorState<events::DoorOpen>
118 {
120  public: using deferred_events = type_tuple<events::EnqueueNewTarget>;
121 
123  public: OpenDoorState() = default;
124 
125  // Documentation inherited
126  private: virtual std::string Report(
127  const std::unique_ptr<ElevatorStateMachinePrivate> &_data) const override
128  {
129  return "opening door " + std::to_string(_data->system->state);
130  }
131 
132  // Documentation inherited
133  private: virtual double JointTarget(
135  int32_t _floorTarget) const override
136  {
137  return _data->system->doorTargets[_floorTarget];
138  }
139 };
140 
142 struct ElevatorStateMachineDef::CloseDoorState : DoorState<events::DoorClosed>
143 {
145  public: CloseDoorState() = default;
146 
147  // Documentation inherited
148  private: virtual std::string Report(
149  const std::unique_ptr<ElevatorStateMachinePrivate> &_data) const override
150  {
151  return "closing door " + std::to_string(_data->system->state);
152  }
153 
154  // Documentation inherited
155  private: virtual double JointTarget(
157  int32_t /*_floorTarget*/) const override
158  {
159  return 0.0;
160  }
161 };
162 
164 struct ElevatorStateMachineDef::WaitState : state<WaitState>
165 {
167  public: using deferred_events = type_tuple<events::EnqueueNewTarget>;
168 
171  public: template <typename Event, typename FSM>
172  void on_enter(const Event &, FSM &_fsm)
173  {
174  const auto &data = _fsm.dataPtr;
175  ignmsg << "The elevator is waiting to close door " << data->system->state
176  << std::endl;
177 
178  this->triggerEvent = [&_fsm] { _fsm.process_event(events::Timeout()); };
179  data->system->StartDoorTimer(data->system->state,
180  std::bind(&WaitState::OnTimeout, this));
181  }
182 
184  private: void OnTimeout()
185  {
186  this->triggerEvent();
187  }
188 
190  private: std::function<void()> triggerEvent;
191 };
192 
194 struct ElevatorStateMachineDef::MoveCabinState : state<MoveCabinState>
195 {
197  public: using deferred_events = type_tuple<events::EnqueueNewTarget>;
198 
204  public: MoveCabinState(double _posEps = 15e-2, double _velEps = 1e-2)
205  : posEps(_posEps), velEps(_velEps)
206  {
207  }
208 
212  public: template <typename Event, typename FSM>
213  void on_enter(const Event &, FSM &_fsm)
214  {
215  const auto &data = _fsm.dataPtr;
216  int32_t floorTarget = data->targets.front();
217  ignmsg << "The elevator is moving the cabin [ " << data->system->state
218  << " -> " << floorTarget << " ]" << std::endl;
219 
220  double jointTarget = data->system->cabinTargets[floorTarget];
221  data->SendCmd(data->system->cabinJointCmdPub, jointTarget);
222  this->triggerEvent = [&_fsm] {
223  _fsm.process_event(events::CabinAtTarget());
224  };
225  data->system->SetCabinMonitor(
226  floorTarget, jointTarget, this->posEps, this->velEps,
227  std::bind(&MoveCabinState::OnJointTargetReached, this));
228  }
229 
231  private: void OnJointTargetReached()
232  {
233  this->triggerEvent();
234  }
235 
237  private: double posEps;
238 
240  private: double velEps;
241 
243  private: std::function<void()> triggerEvent;
244 };
245 
246 } // namespace systems
247 } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE
248 } // namespace gazebo
249 } // namespace ignition
T bind(T... args)
This library is part of the Gazebo project.
MoveCabinState(double _posEps=15e-2, double _velEps=1e-2)
Constructor.
Definition: StatesImpl.hh:204
STL class.
void on_enter(const Event &, FSM &)
Logs the entry to the state.
Definition: StatesImpl.hh:43
void on_enter(const Event &, FSM &_fsm)
Starts the timer that keeps the door at the target floor level open.
Definition: StatesImpl.hh:172
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:213
State at which the elevator is moving the cabin to the target floor.
Definition: StatesImpl.hh:194
type_tuple< events::EnqueueNewTarget > deferred_events
This state defers EnqueueNewTargetEvent events.
Definition: StatesImpl.hh:197
State at which the elevator is waiting with a door open.
Definition: StatesImpl.hh:164
T to_string(T... args)
State at which the elevator is idling.
Definition: StatesImpl.hh:39
Event that signifies the cabin has reached the target floor level.
Definition: EventsImpl.hh:70
State at which the elevator is closing a door.
Definition: StatesImpl.hh:142
#define ignmsg
Event that signifies the door at the target floor level has remained open for the required amount of ...
Definition: EventsImpl.hh:65
T endl(T... args)
State at which the elevator is opening a door.
Definition: StatesImpl.hh:117
type_tuple< events::EnqueueNewTarget > deferred_events
This state defers EnqueueNewTargetEvent events.
Definition: StatesImpl.hh:167
STL class.
type_tuple< events::EnqueueNewTarget > deferred_events
This state defers EnqueueNewTargetEvent events.
Definition: StatesImpl.hh:120