Executor.hpp

Go to the documentation of this file.

#ifndef AIKIDO_CONTROL_EXECUTOR_HPP_
#define AIKIDO_CONTROL_EXECUTOR_HPP_
 
#include <chrono>
#include <set>
#include <vector>
 
#include <dart/dart.hpp>
 
#include "aikido/common/ExecutorThread.hpp"
#include "aikido/common/pointers.hpp"
 
namespace aikido {
namespace control {
 
AIKIDO_DECLARE_POINTERS(Executor)
 
enum class ExecutorType
{
  STATE = 0,
  POSITION = 1,
  VELOCITY = 2,
  EFFORT = 3,
  TRAJECTORY = 4,
  MODE = 5,
  READONLY = 6
};
 
constexpr std::chrono::milliseconds defaultThreadRate{10};
 
class Executor
{
public:
  Executor(
      const std::set<ExecutorType>& types,
      const std::vector<dart::dynamics::DegreeOfFreedom*>& dofs,
      const std::chrono::milliseconds threadRate = defaultThreadRate);
 
  Executor(
      const ExecutorType type,
      const std::vector<dart::dynamics::DegreeOfFreedom*>& dofs,
      std::chrono::milliseconds threadRate = defaultThreadRate);
 
  virtual ~Executor();
 
  std::set<ExecutorType> getTypes() const;
 
  const std::vector<dart::dynamics::DegreeOfFreedom*> getDofs() const;
 
  virtual void step(
      const std::chrono::system_clock::time_point& /* timepoint */)
  {
    // Do nothing
  }
 
  void start();
 
  void stop();
 
  bool registerDofs();
 
  void releaseDofs();
 
  virtual void cancel()
  {
    // Do nothing
  }
 
private:
  void spin()
  {
    if (mThread->isRunning())
    {
      step(std::chrono::system_clock::now());
    }
  }
 
  std::chrono::milliseconds mThreadRate;
 
  bool mDofsRegistered{false};
 
  static std::
      unordered_map<ExecutorType, std::set<dart::dynamics::DegreeOfFreedom*>>
          mDofManager;
 
  static std::mutex mMutex;
 
  std::unique_ptr<aikido::common::ExecutorThread> mThread;
 
protected:
  std::set<ExecutorType> mTypes;
 
  std::vector<dart::dynamics::DegreeOfFreedom*> mDofs;
};
 
} // namespace control
} // namespace aikido
 
#endif