Program Listing for File lely_driver_bridge.hpp

↰ Return to documentation for file (/tmp/ws/src/ros2_canopen/canopen_base_driver/include/canopen_base_driver/lely_driver_bridge.hpp)

//    Copyright 2022 Christoph Hellmann Santos
//
//    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.

#ifndef CANOPEN_BASE_DRIVER__LELY_BRIDGE_HPP_
#define CANOPEN_BASE_DRIVER__LELY_BRIDGE_HPP_

#include <sys/stat.h>

#include <lely/co/dcf.hpp>
#include <lely/co/dev.hpp>
#include <lely/co/obj.hpp>
#include <lely/coapp/fiber_driver.hpp>
#include <lely/coapp/master.hpp>
#include <lely/coapp/sdo_error.hpp>
#include <lely/ev/co_task.hpp>
#include <lely/ev/future.hpp>

#include <atomic>
#include <condition_variable>
#include <future>
#include <memory>
#include <mutex>
#include <rclcpp/rclcpp.hpp>
#include <string>
#include <system_error>
#include <thread>
#include <vector>

#include <boost/lockfree/queue.hpp>
#include <boost/optional.hpp>
#include <boost/thread.hpp>

#include "canopen_core/exchange.hpp"

using namespace std::chrono_literals;
using namespace lely;

namespace ros2_canopen
{
struct pdo_mapping
{
  bool is_tpdo;
  bool is_rpdo;
};

typedef std::map<uint16_t, std::map<uint8_t, pdo_mapping>> PDOMap;
class DriverDictionary : public lely::CODev
{
public:
  DriverDictionary(std::string eds_file) : lely::CODev(eds_file.c_str()) {}
  ~DriverDictionary()
  {
    // lely::CODev::~CODev();
  }

  std::shared_ptr<PDOMap> createPDOMapping()
  {
    std::shared_ptr<PDOMap> pdo_map = std::make_shared<PDOMap>();
    fetchRPDO(pdo_map);
    fetchTPDO(pdo_map);
    return pdo_map;
    // COObj * first = co_dev_first_obj((lely::CODev *)this);
    // COObj * last = co_dev_last_obj((lely::CODev *)this);
    // if (first == nullptr || last == nullptr)
    // {
    //   std::cout << "No objects found in dictionary" << std::endl;
    //   return pdo_map;
    // }
    // COObj * current = first;
    // while (current != last)
    // {
    //   uint16_t index = co_obj_get_idx(current);
    //   auto subids = current->getSubidx();
    //   bool created = false;
    //   for (auto subid : subids)
    //   {
    //     bool is_rpdo = checkObjRPDO(index, subid);
    //     bool is_tpdo = checkObjTPDO(index, subid);
    //     std::cout << "Found subobject: index=" << std::hex << (int)index
    //               << " subindex=" << (int)subid << (is_rpdo ? " rpdo" : "")
    //               << (is_tpdo ? " tpdo" : "") << std::endl;
    //     if (is_rpdo || is_tpdo)
    //     {
    //       pdo_mapping mapping;
    //       mapping.is_rpdo = is_rpdo;
    //       mapping.is_tpdo = is_tpdo;
    //       if (!created)
    //       {
    //         pdo_map->emplace(index, std::map<uint8_t, pdo_mapping>());
    //         created = true;
    //       }
    //       (*pdo_map)[index].emplace(subid, mapping);
    //     }
    //   }
    //   current = co_obj_next(current);
    // }
    // return pdo_map;
  }

  void fetchRPDO(std::shared_ptr<PDOMap> map)
  {
    for (int index = 0; index < 256; index++)
    {
      for (int subindex = 1; subindex < 9; subindex++)
      {
        auto obj = find(0x1600 + index, subindex);
        if (obj == nullptr)
        {
          continue;
        }
        uint32_t data;
        {
          data = obj->getVal<CO_DEFTYPE_UNSIGNED32>();
        }
        uint8_t tmps = (data >> 8) & 0xFF;
        uint16_t tmpi = (data >> 16) & 0xFFFF;
        if (tmpi == 0U)
        {
          continue;
        }
        pdo_mapping mapping;
        mapping.is_rpdo = true;
        mapping.is_tpdo = false;
        (*map)[tmpi][tmps] = mapping;
        std::cout << "Found rpdo mapped object: index=" << std::hex << (int)tmpi
                  << " subindex=" << (int)tmps << std::endl;
      }
    }
  }
  void fetchTPDO(std::shared_ptr<PDOMap> map)
  {
    for (int index = 0; index < 256; index++)
    {
      for (int subindex = 1; subindex < 9; subindex++)
      {
        auto obj = find(0x1A00 + index, subindex);
        if (obj == nullptr)
        {
          continue;
        }
        uint32_t data;
        {
          data = obj->getVal<CO_DEFTYPE_UNSIGNED32>();
        }
        uint8_t tmps = (data >> 8) & 0xFF;
        uint16_t tmpi = (data >> 16) & 0xFFFF;
        if (tmpi == 0U)
        {
          continue;
        }

        pdo_mapping mapping;
        mapping.is_rpdo = false;
        mapping.is_tpdo = true;
        (*map)[tmpi][tmps] = mapping;
        std::cout << "Found tpdo mapped object: index=" << std::hex << (int)tmpi
                  << " subindex=" << (int)tmps << std::endl;
      }
    }
  }

  bool checkObjRPDO(uint16_t idx, uint8_t subidx)
  {
    // std::cout << "Checking for rpo mapping of object: index=" << std::hex << (int)idx
    //           << " subindex=" << (int)subidx << std::endl;
    for (int i = 0; i < 256; i++)
    {
      if (this->checkObjInPDO(i, 0x1600, idx, subidx))
      {
        return true;
      }
    }
    return false;
  }

  bool checkObjTPDO(uint16_t idx, uint8_t subidx)
  {
    // std::cout << "Checking for rpo mapping of object: index=" << std::hex << (int)idx
    //          << " subindex=" << (int)subidx << std::endl;
    for (int i = 0; i < 256; i++)
    {
      if (this->checkObjInPDO(i, 0x1A00, idx, subidx))
      {
        return true;
      }
    }
    return false;
  }

  bool checkObjInPDO(uint8_t pdo, uint16_t mapping_idx, uint16_t idx, uint8_t subindex)
  {
    for (int i = 1; i < 9; i++)
    {
      auto obj = find(mapping_idx + pdo, i);
      if (obj == nullptr)
      {
        return false;
      }
      uint32_t data;
      {
        data = obj->getVal<CO_DEFTYPE_UNSIGNED32>();
      }
      uint8_t tmps = (data >> 8) & 0xFF;
      uint16_t tmpi = (data >> 16) & 0xFFFF;

      if (tmps == subindex && tmpi == idx)
      {
        std::cout << "Found object in pdo: " << (int)pdo << std::endl;
        return true;
      }
    }

    return false;
  }
};

enum class LelyBridgeErrc
{
  NotListedDevice = 'A',
  NoResponseOnDeviceType = 'B',
  DeviceTypeDifference = 'C',
  VendorIdDifference = 'D',
  HeartbeatIssue = 'E',
  NodeGuardingIssue = 'F',
  InconsistentProgramDownload = 'G',
  SoftwareUpdateRequired = 'H',
  SoftwareDownloadFailed = 'I',
  ConfigurationDownloadFailed = 'J',
  StartErrorControlFailed = 'K',
  NmtSlaveInitiallyOperational = 'L',
  ProductCodeDifference = 'M',
  RevisionCodeDifference = 'N',
  SerialNumberDifference = 'O'
};

struct LelyBridgeErrCategory : std::error_category
{
  const char * name() const noexcept override;
  std::string message(int ev) const override;
};
}  // namespace ros2_canopen

namespace std
{
template <>
struct is_error_code_enum<ros2_canopen::LelyBridgeErrc> : true_type
{
};
std::error_code make_error_code(ros2_canopen::LelyBridgeErrc);
}  // namespace std

namespace ros2_canopen
{
class LelyDriverBridge : public canopen::FiberDriver
{
protected:
  // Dictionary for driver based on DCF and BIN files.
  std::unique_ptr<DriverDictionary> dictionary_;
  std::mutex dictionary_mutex_;
  std::shared_ptr<PDOMap> pdo_map_;

  // SDO Read synchronisation items
  std::shared_ptr<std::promise<COData>> sdo_read_data_promise;
  std::shared_ptr<std::promise<bool>> sdo_write_data_promise;
  std::mutex sdo_mutex;
  bool running;
  std::condition_variable sdo_cond;

  // NMT synchronisation items
  std::promise<canopen::NmtState> nmt_state_promise;
  std::atomic<bool> nmt_state_is_set;
  std::mutex nmt_mtex;

  // RPDO synchronisation items
  std::promise<COData> rpdo_promise;
  std::atomic<bool> rpdo_is_set;
  std::mutex pdo_mtex;
  std::shared_ptr<SafeQueue<COData>> rpdo_queue;

  // EMCY synchronisation items
  std::promise<COEmcy> emcy_promise;
  std::atomic<bool> emcy_is_set;
  std::mutex emcy_mtex;
  std::shared_ptr<SafeQueue<COEmcy>> emcy_queue;

  // BOOT synchronisation items
  std::atomic<bool> booted;
  char boot_status;
  std::string boot_what;
  canopen::NmtState boot_state;
  std::condition_variable boot_cond;
  std::mutex boot_mtex;

  uint8_t nodeid;
  std::string name_;

  std::function<void()> on_sync_function_;

  // void set_sync_function(std::function<void()> on_sync_function)
  // {
  //   on_sync_function_ = on_sync_function;
  // }

  // void unset_sync_function()
  // {
  //   on_sync_function_ = std::function<void()>();
  // }

  void OnSync(uint8_t cnt, const time_point & t) noexcept override
  {
    if (on_sync_function_ != nullptr)
    {
      try
      {
        on_sync_function_();
      }
      catch (...)
      {
      }
    }
  }

  void OnState(canopen::NmtState state) noexcept override;

  virtual void OnBoot(canopen::NmtState st, char es, const ::std::string & what) noexcept override;

  void OnRpdoWrite(uint16_t idx, uint8_t subidx) noexcept override;

  void OnEmcy(uint16_t eec, uint8_t er, uint8_t msef[5]) noexcept override;

public:
  using FiberDriver::FiberDriver;

  LelyDriverBridge(
    ev_exec_t * exec, canopen::AsyncMaster & master, uint8_t id, std::string name, std::string eds,
    std::string bin)
  : FiberDriver(exec, master, id),
    rpdo_queue(new SafeQueue<COData>()),
    emcy_queue(new SafeQueue<COEmcy>())
  {
    nodeid = id;
    running = false;
    name_ = name;
    dictionary_ = std::make_unique<DriverDictionary>(eds.c_str());
    struct stat buffer;
    if (stat(bin.c_str(), &buffer) == 0)
    {
      co_unsigned16_t * a = NULL;
      co_unsigned16_t * b = NULL;
      dictionary_->readDCF(a, b, bin.c_str());
    }
    pdo_map_ = dictionary_->createPDOMapping();
  }

  std::future<bool> async_sdo_write(COData data);

  template <typename T>
  std::future<bool> async_sdo_write_typed(uint16_t idx, uint8_t subidx, T value)
  {
    std::unique_lock<std::mutex> lck(sdo_mutex);
    if (running)
    {
      sdo_cond.wait(lck);
    }
    running = true;

    auto prom = std::make_shared<std::promise<bool>>();
    lely::COSub * sub = this->dictionary_->find(idx, subidx);
    if (sub == nullptr)
    {
      std::cout << "async_sdo_write_typed: id=" << (unsigned int)this->get_id() << " index=0x"
                << std::hex << (unsigned int)idx << " subindex=" << (unsigned int)subidx
                << " object does not exist" << std::endl;
      prom->set_value(false);
      this->running = false;
      this->sdo_cond.notify_one();
      return prom->get_future();
    }

    this->SubmitWrite(
      idx, subidx, value,
      [this, value, prom](uint8_t id, uint16_t idx, uint8_t subidx, ::std::error_code ec) mutable
      {
        if (ec)
        {
          prom->set_exception(
            lely::canopen::make_sdo_exception_ptr(id, idx, subidx, ec, "AsyncDownload"));
        }
        else
        {
          std::scoped_lock<std::mutex> lck(this->dictionary_mutex_);
          this->dictionary_->setVal<T>(idx, subidx, value);
          prom->set_value(true);
        }
        std::unique_lock<std::mutex> lck(this->sdo_mutex);
        this->running = false;
        this->sdo_cond.notify_one();
      },
      20ms);
    return prom->get_future();
  }

  template <typename T>
  bool sync_sdo_write_typed(
    uint16_t idx, uint8_t subidx, T value, std::chrono::milliseconds timeout)
  {
    auto fut = async_sdo_write_typed(idx, subidx, value);
    auto wait_res = fut.wait_for(timeout);
    if (wait_res == std::future_status::timeout)
    {
      std::cout << "sync_sdo_write_typed: id=" << (unsigned int)this->get_id() << " index=0x"
                << std::hex << (unsigned int)idx << " subindex=" << (unsigned int)subidx
                << " timed out." << std::endl;
      return false;
    }
    bool res = false;
    try
    {
      res = fut.get();
    }
    catch (std::exception & e)
    {
      RCLCPP_ERROR(rclcpp::get_logger(name_), e.what());
    }
    return res;
  }

  std::future<COData> async_sdo_read(COData data);

  template <typename T>
  std::future<T> async_sdo_read_typed(uint16_t idx, uint8_t subidx)
  {
    std::unique_lock<std::mutex> lck(sdo_mutex);
    if (running)
    {
      sdo_cond.wait(lck);
    }
    running = true;

    auto prom = std::make_shared<std::promise<T>>();
    lely::COSub * sub = this->dictionary_->find(idx, subidx);
    if (sub == nullptr)
    {
      std::cout << "async_sdo_read: id=" << (unsigned int)this->get_id() << " index=0x" << std::hex
                << (unsigned int)idx << " subindex=" << (unsigned int)subidx
                << " object does not exist" << std::endl;
      try
      {
        throw lely::canopen::SdoError(this->get_id(), idx, subidx, lely::canopen::SdoErrc::NO_OBJ);
      }
      catch (...)
      {
        prom->set_exception(std::current_exception());
      }
      this->running = false;
      this->sdo_cond.notify_one();
      return prom->get_future();
    }
    this->SubmitRead<T>(
      idx, subidx,
      [this, prom](uint8_t id, uint16_t idx, uint8_t subidx, ::std::error_code ec, T value) mutable
      {
        if (ec)
        {
          prom->set_exception(
            lely::canopen::make_sdo_exception_ptr(id, idx, subidx, ec, "AsyncUpload"));
        }
        else
        {
          std::scoped_lock<std::mutex> lck(this->dictionary_mutex_);
          this->dictionary_->setVal<T>(idx, subidx, value);
          prom->set_value(value);
        }
        std::unique_lock<std::mutex> lck(this->sdo_mutex);
        this->running = false;
        this->sdo_cond.notify_one();
      },
      20ms);
    return prom->get_future();
  }

  template <typename T>
  bool sync_sdo_read_typed(
    uint16_t idx, uint8_t subidx, T & value, std::chrono::milliseconds timeout)
  {
    auto fut = async_sdo_read_typed<T>(idx, subidx);
    auto wait_res = fut.wait_for(timeout);
    if (wait_res == std::future_status::timeout)
    {
      std::cout << "sync_sdo_read_typed: id=" << (unsigned int)this->get_id() << " index=0x"
                << std::hex << (unsigned int)idx << " subindex=" << (unsigned int)subidx
                << " timed out." << std::endl;
      return false;
    }
    bool res = false;
    try
    {
      value = fut.get();
      res = true;
    }
    catch (std::exception & e)
    {
      RCLCPP_ERROR(rclcpp::get_logger(name_), e.what());
      res = false;
    }
    return res;
  }

  std::future<canopen::NmtState> async_request_nmt();

  std::shared_ptr<SafeQueue<COData>> get_rpdo_queue();

  std::shared_ptr<SafeQueue<COEmcy>> get_emcy_queue();

  void tpdo_transmit(COData data);

  void nmt_command(canopen::NmtCommand command);

  uint8_t get_id();

  bool wait_for_boot()
  {
    if (booted.load())
    {
      return true;
    }
    std::unique_lock<std::mutex> lck(boot_mtex);
    boot_cond.wait(lck);
    if ((boot_status != 0) && (boot_status != 'L'))
    {
      throw std::system_error(boot_status, LelyBridgeErrCategory(), "Boot Issue");
    }
    else
    {
      booted.store(true);
      return true;
    }
    return false;
  }

  void set_sync_function(std::function<void()> on_sync_function)
  {
    on_sync_function_ = on_sync_function;
  }

  void unset_sync_function() { on_sync_function_ = std::function<void()>(); }

  void Boot()
  {
    booted.store(false);
    FiberDriver::Boot();
  }

  bool is_booted() { return booted.load(); }

  template <typename T>
  void submit_write(COData data)
  {
    T value = 0;
    std::memcpy(&value, &data.data_, sizeof(value));

    this->SubmitWrite(
      data.index_, data.subindex_, value,
      [this, value](uint8_t id, uint16_t idx, uint8_t subidx, ::std::error_code ec) mutable
      {
        if (ec)
        {
          this->sdo_write_data_promise->set_exception(
            lely::canopen::make_sdo_exception_ptr(id, idx, subidx, ec, "AsyncDownload"));
        }
        else
        {
          std::scoped_lock<std::mutex> lck(this->dictionary_mutex_);
          this->dictionary_->setVal<T>(idx, subidx, value);
          this->sdo_write_data_promise->set_value(true);
        }
        std::unique_lock<std::mutex> lck(this->sdo_mutex);
        this->running = false;
        this->sdo_cond.notify_one();
      },
      20ms);
  }

  template <typename T>
  void submit_read(COData data)
  {
    this->SubmitRead<T>(
      data.index_, data.subindex_,
      [this](uint8_t id, uint16_t idx, uint8_t subidx, ::std::error_code ec, T value) mutable
      {
        if (ec)
        {
          this->sdo_read_data_promise->set_exception(
            lely::canopen::make_sdo_exception_ptr(id, idx, subidx, ec, "AsyncUpload"));
        }
        else
        {
          std::scoped_lock<std::mutex> lck(this->dictionary_mutex_);
          this->dictionary_->setVal<T>(idx, subidx, value);
          COData d = {idx, subidx, 0};
          std::memcpy(&d.data_, &value, sizeof(T));
          this->sdo_read_data_promise->set_value(d);
        }
        std::unique_lock<std::mutex> lck(this->sdo_mutex);
        this->running = false;
        this->sdo_cond.notify_one();
      },
      20ms);
  }

  template <typename T>
  const T universal_get_value(uint16_t index, uint8_t subindex)
  {
    T value = 0;
    bool is_tpdo = false;
    if (this->pdo_map_->find(index) != this->pdo_map_->end())
    {
      auto object = this->pdo_map_->at(index);
      if (object.find(subindex) != object.end())
      {
        auto entry = object.at(subindex);
        is_tpdo = entry.is_tpdo;
      }
    }
    if (!is_tpdo)
    {
      if (sync_sdo_read_typed<T>(index, subindex, value, std::chrono::milliseconds(20)))
      {
        return value;
      }
    }

    std::scoped_lock<std::mutex> lck(this->dictionary_mutex_);
    if (typeid(T) == typeid(uint8_t))
    {
      value = this->dictionary_->getVal<CO_DEFTYPE_UNSIGNED8>(index, subindex);
    }
    if (typeid(T) == typeid(uint16_t))
    {
      value = this->dictionary_->getVal<CO_DEFTYPE_UNSIGNED16>(index, subindex);
    }
    if (typeid(T) == typeid(uint32_t))
    {
      value = this->dictionary_->getVal<CO_DEFTYPE_UNSIGNED32>(index, subindex);
    }
    if (typeid(T) == typeid(int8_t))
    {
      value = this->dictionary_->getVal<CO_DEFTYPE_INTEGER8>(index, subindex);
    }
    if (typeid(T) == typeid(int16_t))
    {
      value = this->dictionary_->getVal<CO_DEFTYPE_INTEGER16>(index, subindex);
    }
    if (typeid(T) == typeid(int32_t))
    {
      value = this->dictionary_->getVal<CO_DEFTYPE_INTEGER32>(index, subindex);
    }

    return value;
  }

  template <typename T>
  void universal_set_value(uint16_t index, uint8_t subindex, T value)
  {
    bool is_rpdo = false;
    if (this->pdo_map_->find(index) != this->pdo_map_->end())
    {
      auto object = this->pdo_map_->at(index);
      if (object.find(subindex) != object.end())
      {
        auto entry = object.at(subindex);
        is_rpdo = entry.is_rpdo;
      }
    }
    if (is_rpdo)
    {
      std::scoped_lock<std::mutex> lck(this->dictionary_mutex_);
      this->dictionary_->setVal<T>(index, subindex, value);
      this->tpdo_mapped[index][subindex] = value;
      this->tpdo_mapped[index][subindex].WriteEvent();
    }
    else
    {
      sync_sdo_write_typed(index, subindex, value, std::chrono::milliseconds(20));
    }
  }
};

}  // namespace ros2_canopen

#endif  // CANOPEN_BASE_DRIVER__LELY_BRIDGE_HPP_