ElevatorStateMachineImpl.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2021 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */
 
/*
 * \author Nick Lamprianidis <nlamprian@gmail.com>
 * \date January 2021
 */
 
#include <deque>
#include <memory>
#include <sstream>
#include <utility>
 
#include <gz/common/Console.hh>
 
#include "../ElevatorStateMachine.hh"
 
#include "../ElevatorCommonPrivate.hh"
 
namespace ignition
{
namespace gazebo
{
// Inline bracket to help doxygen filtering
inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE
{
namespace systems
{
class ElevatorStateMachinePrivate
{
  public: ElevatorStateMachinePrivate(
      const std::shared_ptr<ElevatorCommonPrivate> &_system);
 
  public: void EnqueueNewTarget(double _target);
 
  public: void SendCmd(transport::Node::Publisher &_pub, double _cmd);
 
  public: std::shared_ptr<ElevatorCommonPrivate> system;
 
  public: std::deque<int32_t> targets;
};
 
ElevatorStateMachinePrivate::ElevatorStateMachinePrivate(
    const std::shared_ptr<ElevatorCommonPrivate> &_system)
    : system(_system)
{
}
 
void ElevatorStateMachinePrivate::EnqueueNewTarget(double _target)
{
  // Ignore duplicate targets
  auto it = std::find(this->targets.cbegin(), this->targets.cend(), _target);
  if (it != this->targets.cend())
    return;
 
  // Prioritize target in the queue
  bool enqueued = false;
  int32_t prevTarget = this->system->state;
  for (it = this->targets.cbegin(); it != this->targets.cend(); ++it)
  {
    int32_t lower = prevTarget;
    int32_t upper = *it;
    if (upper < lower) std::swap(lower, upper);
    if (_target >= lower && _target < upper)
    {
      this->targets.insert(it, _target);
      enqueued = true;
      break;
    }
    prevTarget = *it;
  }
  if (!enqueued)
    this->targets.push_back(_target);
 
  std::ostringstream ss;
  ss << "The elevator enqueued target " << _target << " [ ";
  for (const auto &target : this->targets) ss << target << " ";
  ignmsg << ss.str() << "]" << std::endl;
}
 
void ElevatorStateMachinePrivate::SendCmd(transport::Node::Publisher &_pub,
                                          double _cmd)
{
  msgs::Double msg;
  msg.set_data(_cmd);
  _pub.Publish(msg);
}
 
ElevatorStateMachineDef::ElevatorStateMachineDef(
    const std::shared_ptr<ElevatorCommonPrivate> &_system)
    : dataPtr(std::make_unique<ElevatorStateMachinePrivate>(_system))
{
}
 
ElevatorStateMachineDef::~ElevatorStateMachineDef() = default;
 
}  // namespace systems
}  // namespace IGNITION_GAZEBO_VERSION_NAMESPACE
}  // namespace gazebo
}  // namespace ignition
 
#include "EventsImpl.hh"
 
#include "ActionsImpl.hh"
#include "GuardsImpl.hh"
#include "StatesImpl.hh"