Gazebo Gazebo

API Reference

3.15.1
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 {
41 namespace systems
42 {
44 {
49 
54  public: void EnqueueNewTarget(double _target);
55 
59  public: void SendCmd(transport::Node::Publisher &_pub, double _cmd);
60 
63 
66 };
67 
71  : system(_system)
72 {
73 }
74 
77 {
78  // Ignore duplicate targets
79  auto it = std::find(this->targets.cbegin(), this->targets.cend(), _target);
80  if (it != this->targets.cend())
81  return;
82 
83  // Prioritize target in the queue
84  bool enqueued = false;
85  int32_t prevTarget = this->system->state;
86  for (it = this->targets.cbegin(); it != this->targets.cend(); ++it)
87  {
88  int32_t lower = prevTarget;
89  int32_t upper = *it;
90  if (upper < lower) std::swap(lower, upper);
91  if (_target >= lower && _target < upper)
92  {
93  this->targets.insert(it, _target);
94  enqueued = true;
95  break;
96  }
97  prevTarget = *it;
98  }
99  if (!enqueued)
100  this->targets.push_back(_target);
101 
103  ss << "The elevator enqueued target " << _target << " [ ";
104  for (const auto &target : this->targets) ss << target << " ";
105  ignmsg << ss.str() << "]" << std::endl;
106 }
107 
110  double _cmd)
111 {
112  msgs::Double msg;
113  msg.set_data(_cmd);
114  _pub.Publish(msg);
115 }
116 
120  : dataPtr(std::make_unique<ElevatorStateMachinePrivate>(_system))
121 {
122 }
123 
126 
127 } // namespace systems
128 } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE
129 } // namespace gazebo
130 } // namespace ignition
131 
132 #include "EventsImpl.hh"
133 
134 #include "ActionsImpl.hh"
135 #include "GuardsImpl.hh"
136 #include "StatesImpl.hh"
This library is part of the Ignition Robotics project.
T find(T... args)
void EnqueueNewTarget(double _target)
Adds a new target to the queue.
Definition: ElevatorStateMachineImpl.hh:76
void SendCmd(transport::Node::Publisher &_pub, double _cmd)
Publishes a command message.
Definition: ElevatorStateMachineImpl.hh:109
ElevatorStateMachineDef(const std::shared_ptr< ElevatorCommonPrivate > &_system)
Constructor.
Definition: ElevatorStateMachineImpl.hh:118
std::deque< int32_t > targets
Floor target queue.
Definition: ElevatorStateMachineImpl.hh:65
T push_back(T... args)
void set_data(double value)
std::shared_ptr< ElevatorCommonPrivate > system
Data of the enclosing system.
Definition: ElevatorStateMachineImpl.hh:62
ElevatorStateMachinePrivate(const std::shared_ptr< ElevatorCommonPrivate > &_system)
Constructor.
Definition: ElevatorStateMachineImpl.hh:69
T swap(T... args)
#define ignmsg
T endl(T... args)
T cbegin(T... args)
STL namespace.
T insert(T... args)
T str(T... args)
bool Publish(const ProtoMsg &_msg)
T cend(T... args)