Gazebo Gazebo

API Reference

6.16.0
ElevatorStateMachineImpl.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 <deque>
24 #include <memory>
25 #include <sstream>
26 #include <utility>
27 
28 #include <gz/common/Console.hh>
29 
30 #include "../ElevatorStateMachine.hh"
31 
32 #include "../ElevatorCommonPrivate.hh"
33 
34 namespace ignition
35 {
36 namespace gazebo
37 {
38 // Inline bracket to help doxygen filtering
39 inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
40 namespace systems
41 {
43 {
48 
53  public: void EnqueueNewTarget(double _target);
54 
58  public: void SendCmd(transport::Node::Publisher &_pub, double _cmd);
59 
62 
65 };
66 
68 ElevatorStateMachinePrivate::ElevatorStateMachinePrivate(
70  : system(_system)
71 {
72 }
73 
76 {
77  // Ignore duplicate targets
78  auto it = std::find(this->targets.cbegin(), this->targets.cend(), _target);
79  if (it != this->targets.cend())
80  return;
81 
82  // Prioritize target in the queue
83  bool enqueued = false;
84  int32_t prevTarget = this->system->state;
85  for (it = this->targets.cbegin(); it != this->targets.cend(); ++it)
86  {
87  int32_t lower = prevTarget;
88  int32_t upper = *it;
89  if (upper < lower) std::swap(lower, upper);
90  if (_target >= lower && _target < upper)
91  {
92  this->targets.insert(it, _target);
93  enqueued = true;
94  break;
95  }
96  prevTarget = *it;
97  }
98  if (!enqueued)
99  this->targets.push_back(_target);
100 
102  ss << "The elevator enqueued target " << _target << " [ ";
103  for (const auto &target : this->targets) ss << target << " ";
104  ignmsg << ss.str() << "]" << std::endl;
105 }
106 
109  double _cmd)
110 {
111  msgs::Double msg;
112  msg.set_data(_cmd);
113  _pub.Publish(msg);
114 }
115 
119  : dataPtr(std::make_unique<ElevatorStateMachinePrivate>(_system))
120 {
121 }
122 
125 
126 } // namespace systems
127 } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE
128 } // namespace gazebo
129 } // namespace ignition
130 
131 #include "EventsImpl.hh"
132 
133 #include "ActionsImpl.hh"
134 #include "GuardsImpl.hh"
135 #include "StatesImpl.hh"
ElevatorStateMachineDef(const std::shared_ptr< ElevatorCommonPrivate > &_system)
Constructor.
Definition: ElevatorStateMachineImpl.hh:117
This library is part of the Gazebo project.
ElevatorStateMachinePrivate(const std::shared_ptr< ElevatorCommonPrivate > &_system)
Constructor.
Definition: ElevatorStateMachineImpl.hh:68
T find(T... args)
void EnqueueNewTarget(double _target)
Adds a new target to the queue.
Definition: ElevatorStateMachineImpl.hh:75
T push_back(T... args)
void SendCmd(transport::Node::Publisher &_pub, double _cmd)
Publishes a command message.
Definition: ElevatorStateMachineImpl.hh:108
void set_data(double value)
std::shared_ptr< ElevatorCommonPrivate > system
Data of the enclosing system.
Definition: ElevatorStateMachineImpl.hh:61
T swap(T... args)
#define ignmsg
T endl(T... args)
Definition: ElevatorStateMachineImpl.hh:42
T cbegin(T... args)
STL namespace.
T insert(T... args)
T str(T... args)
bool Publish(const ProtoMsg &_msg)
T cend(T... args)
std::deque< int32_t > targets
Floor target queue.
Definition: ElevatorStateMachineImpl.hh:64