StatesImpl.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 <memory>
#include <string>
 
#include "../ElevatorStateMachine.hh"
 
#include <gz/common/Console.hh>
 
namespace gz
{
namespace sim
{
// Inline bracket to help doxygen filtering
inline namespace GZ_SIM_VERSION_NAMESPACE {
namespace systems
{
struct ElevatorStateMachineDef::IdleState : state<IdleState>
{
  public: template <typename Event, typename FSM>
  void on_enter(const Event &, FSM &)
  {
    gzmsg << "The elevator is idling" << std::endl;
  }
};
 
template <typename E>
struct ElevatorStateMachineDef::DoorState : state<DoorState<E>>
{
  public: DoorState(double _posEps = 2e-2, double _velEps = 1e-2)
      : posEps(_posEps), velEps(_velEps)
  {
  }
 
  public: virtual ~DoorState()
  {
  }
 
  public: template <typename Event, typename FSM>
  void on_enter(const Event &, FSM &_fsm)
  {
    const auto &data = _fsm.dataPtr;
    int32_t floorTarget = data->system->state;
    gzmsg << "The elevator is " << this->Report(data) << std::endl;
 
    double jointTarget = this->JointTarget(data, floorTarget);
    data->SendCmd(data->system->doorJointCmdPubs[floorTarget], jointTarget);
    this->triggerEvent = [&_fsm] { _fsm.process_event(E()); };
    data->system->SetDoorMonitor(
        floorTarget, jointTarget, this->posEps, this->velEps,
        std::bind(&DoorState::OnJointTargetReached, this));
  }
 
  private: virtual std::string Report(
      const std::unique_ptr<ElevatorStateMachinePrivate> &_data) const = 0;
 
  private: virtual double JointTarget(
      const std::unique_ptr<ElevatorStateMachinePrivate> &_data,
      int32_t _floorTarget) const = 0;
 
  private: void OnJointTargetReached()
  {
    this->triggerEvent();
  }
 
  private: double posEps;
 
  private: double velEps;
 
  private: std::function<void()> triggerEvent;
};
 
struct ElevatorStateMachineDef::OpenDoorState : DoorState<events::DoorOpen>
{
  public: using deferred_events = type_tuple<events::EnqueueNewTarget>;
 
  public: OpenDoorState() = default;
 
  // Documentation inherited
  private: virtual std::string Report(
      const std::unique_ptr<ElevatorStateMachinePrivate> &_data) const override
  {
    return "opening door " + std::to_string(_data->system->state);
  }
 
  // Documentation inherited
  private: virtual double JointTarget(
      const std::unique_ptr<ElevatorStateMachinePrivate> &_data,
      int32_t _floorTarget) const override
  {
    return _data->system->doorTargets[_floorTarget];
  }
};
 
struct ElevatorStateMachineDef::CloseDoorState : DoorState<events::DoorClosed>
{
  public: CloseDoorState() = default;
 
  // Documentation inherited
  private: virtual std::string Report(
      const std::unique_ptr<ElevatorStateMachinePrivate> &_data) const override
  {
    return "closing door " + std::to_string(_data->system->state);
  }
 
  // Documentation inherited
  private: virtual double JointTarget(
      const std::unique_ptr<ElevatorStateMachinePrivate> & /*_data*/,
      int32_t /*_floorTarget*/) const override
  {
    return 0.0;
  }
};
 
struct ElevatorStateMachineDef::WaitState : state<WaitState>
{
  public: using deferred_events = type_tuple<events::EnqueueNewTarget>;
 
  public: template <typename Event, typename FSM>
  void on_enter(const Event &, FSM &_fsm)
  {
    const auto &data = _fsm.dataPtr;
    gzmsg << "The elevator is waiting to close door " << data->system->state
           << std::endl;
 
    this->triggerEvent = [&_fsm] { _fsm.process_event(events::Timeout()); };
    data->system->StartDoorTimer(data->system->state,
                                 std::bind(&WaitState::OnTimeout, this));
  }
 
  private: void OnTimeout()
  {
    this->triggerEvent();
  }
 
  private: std::function<void()> triggerEvent;
};
 
struct ElevatorStateMachineDef::MoveCabinState : state<MoveCabinState>
{
  public: using deferred_events = type_tuple<events::EnqueueNewTarget>;
 
  public: MoveCabinState(double _posEps = 15e-2, double _velEps = 1e-2)
      : posEps(_posEps), velEps(_velEps)
  {
  }
 
  public: template <typename Event, typename FSM>
  void on_enter(const Event &, FSM &_fsm)
  {
    const auto &data = _fsm.dataPtr;
    int32_t floorTarget = data->targets.front();
    gzmsg << "The elevator is moving the cabin [ " << data->system->state
           << " -> " << floorTarget << " ]" << std::endl;
 
    double jointTarget = data->system->cabinTargets[floorTarget];
    data->SendCmd(data->system->cabinJointCmdPub, jointTarget);
    this->triggerEvent = [&_fsm] {
      _fsm.process_event(events::CabinAtTarget());
    };
    data->system->SetCabinMonitor(
        floorTarget, jointTarget, this->posEps, this->velEps,
        std::bind(&MoveCabinState::OnJointTargetReached, this));
  }
 
  private: void OnJointTargetReached()
  {
    this->triggerEvent();
  }
 
  private: double posEps;
 
  private: double velEps;
 
  private: std::function<void()> triggerEvent;
};
 
}  // namespace systems
}  // namespace GZ_SIM_VERSION_NAMESPACE
}  // namespace sim
}  // namespace gz