pipeline.h

Go to the documentation of this file.

/*
 * Copyright 2019, FZI Forschungszentrum Informatik (templating)
 *
 * Copyright 2017, 2018 Simon Rasmussen (refactor)
 *
 * Copyright 2015, 2016 Thomas Timm Andersen (original version)
 *
 * 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.
 */

#pragma once

#include "ur_client_library/comm/package.h"
#include "ur_client_library/log.h"
#include "ur_client_library/queue/readerwriterqueue.h"
#include <atomic>
#include <chrono>
#include <thread>
#include <vector>
#include <fstream>

namespace urcl
{
namespace comm
{
template <typename T>
class IConsumer
{
public:
  virtual void setupConsumer()
  {
  }
  virtual void teardownConsumer()
  {
    stopConsumer();
  }
  virtual void stopConsumer()
  {
  }
  virtual void onTimeout()
  {
  }

  virtual bool consume(std::shared_ptr<T> product) = 0;
};

template <typename T>
class MultiConsumer : public IConsumer<T>
{
private:
  std::vector<IConsumer<T>*> consumers_;

public:
  MultiConsumer(std::vector<IConsumer<T>*> consumers) : consumers_(consumers)
  {
  }

  virtual void setupConsumer()
  {
    for (auto& con : consumers_)
    {
      con->setupConsumer();
    }
  }
  virtual void teardownConsumer()
  {
    for (auto& con : consumers_)
    {
      con->teardownConsumer();
    }
  }
  virtual void stopConsumer()
  {
    for (auto& con : consumers_)
    {
      con->stopConsumer();
    }
  }
  virtual void onTimeout()
  {
    for (auto& con : consumers_)
    {
      con->onTimeout();
    }
  }

  bool consume(std::shared_ptr<T> product)
  {
    bool res = true;
    for (auto& con : consumers_)
    {
      if (!con->consume(product))
        res = false;
    }
    return res;
  }
};

template <typename T>
class IProducer
{
public:
  virtual void setupProducer()
  {
  }
  virtual void teardownProducer()
  {
    stopProducer();
  }
  virtual void stopProducer()
  {
  }

  virtual void startProducer()
  {
  }

  virtual bool tryGet(std::vector<std::unique_ptr<T>>& products) = 0;
};

class INotifier
{
public:
  virtual void started(std::string name)
  {
  }
  virtual void stopped(std::string name)
  {
  }
};

template <typename T>
class Pipeline
{
public:
  typedef std::chrono::high_resolution_clock Clock;
  typedef Clock::time_point Time;
  Pipeline(IProducer<T>& producer, IConsumer<T>* consumer, std::string name, INotifier& notifier)
    : producer_(producer), consumer_(consumer), name_(name), notifier_(notifier), queue_{ 32 }, running_{ false }
  {
  }
  Pipeline(IProducer<T>& producer, std::string name, INotifier& notifier)
    : producer_(producer), consumer_(nullptr), name_(name), notifier_(notifier), queue_{ 32 }, running_{ false }
  {
  }

  virtual ~Pipeline()
  {
    URCL_LOG_DEBUG("Destructing pipeline");
    stop();
  }

  void init()
  {
    producer_.setupProducer();
    if (consumer_ != nullptr)
      consumer_->setupConsumer();
  }

  void run()
  {
    if (running_)
      return;

    running_ = true;
    producer_.startProducer();
    pThread_ = std::thread(&Pipeline::runProducer, this);
    if (consumer_ != nullptr)
      cThread_ = std::thread(&Pipeline::runConsumer, this);
    notifier_.started(name_);
  }

  void stop()
  {
    if (!running_)
      return;

    URCL_LOG_DEBUG("Stopping pipeline! <%s>", name_.c_str());

    running_ = false;

    producer_.stopProducer();
    if (pThread_.joinable())
    {
      pThread_.join();
    }
    if (cThread_.joinable())
    {
      cThread_.join();
    }
    notifier_.stopped(name_);
  }

  bool getLatestProduct(std::unique_ptr<T>& product, std::chrono::milliseconds timeout)
  {
    return queue_.waitDequeTimed(product, timeout);
  }

private:
  IProducer<T>& producer_;
  IConsumer<T>* consumer_;
  std::string name_;
  INotifier& notifier_;
  moodycamel::BlockingReaderWriterQueue<std::unique_ptr<T>> queue_;
  std::atomic<bool> running_;
  std::thread pThread_, cThread_;

  void runProducer()
  {
    URCL_LOG_DEBUG("Starting up producer");
    std::ifstream realtime_file("/sys/kernel/realtime", std::ios::in);
    bool has_realtime;
    realtime_file >> has_realtime;
    if (has_realtime)
    {
      const int max_thread_priority = sched_get_priority_max(SCHED_FIFO);
      if (max_thread_priority != -1)
      {
        // We'll operate on the currently running thread.
        pthread_t this_thread = pthread_self();

        // struct sched_param is used to store the scheduling priority
        struct sched_param params;

        // We'll set the priority to the maximum.
        params.sched_priority = max_thread_priority;

        int ret = pthread_setschedparam(this_thread, SCHED_FIFO, &params);
        if (ret != 0)
        {
          URCL_LOG_ERROR("Unsuccessful in setting producer thread realtime priority. Error code: %d", ret);
        }
        // Now verify the change in thread priority
        int policy = 0;
        ret = pthread_getschedparam(this_thread, &policy, &params);
        if (ret != 0)
        {
          std::cout << "Couldn't retrieve real-time scheduling paramers" << std::endl;
        }

        // Check the correct policy was applied
        if (policy != SCHED_FIFO)
        {
          URCL_LOG_ERROR("Producer thread: Scheduling is NOT SCHED_FIFO!");
        }
        else
        {
          URCL_LOG_INFO("Producer thread: SCHED_FIFO OK");
        }

        // Print thread scheduling priority
        URCL_LOG_INFO("Thread priority is %d", params.sched_priority);
      }
      else
      {
        URCL_LOG_ERROR("Could not get maximum thread priority for producer thread");
      }
    }
    else
    {
      URCL_LOG_WARN("No realtime capabilities found. Consider using a realtime system for better performance");
    }
    std::vector<std::unique_ptr<T>> products;
    while (running_)
    {
      if (!producer_.tryGet(products))
      {
        producer_.teardownProducer();
        running_ = false;
        break;
      }

      for (auto& p : products)
      {
        if (!queue_.tryEnqueue(std::move(p)))
        {
          URCL_LOG_ERROR("Pipeline producer overflowed! <%s>", name_.c_str());
        }
      }

      products.clear();
    }
    URCL_LOG_DEBUG("Pipeline producer ended! <%s>", name_.c_str());
    notifier_.stopped(name_);
  }

  void runConsumer()
  {
    std::unique_ptr<T> product;
    while (running_)
    {
      // timeout was chosen because we should receive messages
      // at roughly 125hz (every 8ms) and have to update
      // the controllers (i.e. the consumer) with *at least* 125Hz
      // So we update the consumer more frequently via onTimeout
      if (!queue_.waitDequeTimed(product, std::chrono::milliseconds(8)))
      {
        consumer_->onTimeout();
        continue;
      }

      if (!consumer_->consume(std::move(product)))
      {
        consumer_->teardownConsumer();
        running_ = false;
        break;
      }
    }
    consumer_->stopConsumer();
    URCL_LOG_DEBUG("Pipeline consumer ended! <%s>", name_.c_str());
    notifier_.stopped(name_);
  }
};
}  // namespace comm
}  // namespace urcl