Subscriber.hpp

/*
 * Copyright 2017 ONERA
 *
 * This file is part of the MAUVE ROS project.
 *
 * MAUVE ROS is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License version 3 as
 * published by the Free Software Foundation.
 *
 * MAUVE ROS is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with MAUVE.  If not, see <https://www.gnu.org/licenses/lgpl-3.0>.
 */
#ifndef MAUVE_ROS_SUBSCRIBER_HPP
#define MAUVE_ROS_SUBSCRIBER_HPP

#include <string>
#include <functional>

#include <ros/ros.h>

#include <mauve/runtime.hpp>
#include <mauve/base/PeriodicStateMachine.hpp>

#include "Connector.hpp"
#include "Shell.hpp"

namespace mauve
{
  namespace ros
  {

    template <typename ROS_T, typename T>
    struct SubscriberCore : public runtime::Core< RosShell<ROS_T, T> >
    {
      bool configure_hook() final override;
      void cleanup_hook() final override;
      inline T read_value() {
        if (status == runtime::DataStatus::NEW_DATA)
          status = runtime::DataStatus::OLD_DATA;
        return value;
      };
      inline runtime::StatusValue<T> read() {
        auto sv = runtime::StatusValue<T> {status, value};
        if (status == runtime::DataStatus::NEW_DATA)
          status = runtime::DataStatus::OLD_DATA;
        return sv;
      };
      inline runtime::DataStatus read_status() { return status; };
      void update();

    private:
      void callback(const ROS_T& msg);
      ::ros::Subscriber subscriber;
      T value;
      runtime::DataStatus status;
      runtime::RtMutex mutex;
    };

    template <typename ROS_T, typename T>
    struct SubscriberInterface :
      public runtime::Interface< RosShell<ROS_T, T>, SubscriberCore<ROS_T, T> >
    {
      runtime::ReadService<runtime::StatusValue<T>> & read = this->template mk_read_service<runtime::StatusValue<T>>("read", &SubscriberCore<ROS_T, T>::read);
      runtime::ReadService<T> & read_value = this->template mk_read_service<T>("read_value", &SubscriberCore<ROS_T, T>::read_value);
      runtime::ReadService<runtime::DataStatus> & read_status = this->template mk_read_service<runtime::DataStatus>("read_status", &SubscriberCore<ROS_T, T>::read_status);
    };

    template <typename ROS_T, typename T>
    using SubscriberResource = runtime::Resource< RosShell<ROS_T, T>,
      SubscriberCore<ROS_T, T>, SubscriberInterface<ROS_T, T> >;

    template <typename ROS_T, typename T>
    using SubscriberComponent = runtime::Component< RosShell<ROS_T, T>,
        SubscriberCore<ROS_T, T>,
        base::PeriodicStateMachine<RosShell<ROS_T, T>,
          SubscriberCore<ROS_T, T>> >;

}} // namespaces

#include "ipp/Subscriber.ipp"

#endif