RokubiminiFactory.cpp

Go to the documentation of this file.

 
 
#include <rokubimini/setup/Setup.hpp>
 
#include <rokubimini_factory/RokubiminiFactory.hpp>
#include <rokubimini_serial/RokubiminiSerial.hpp>
#include <rokubimini_serial/RokubiminiSerialBusManager.hpp>
#include <rokubimini_ethercat/RokubiminiEthercat.hpp>
#include <rokubimini_ethercat/RokubiminiEthercatBusManager.hpp>
#include <utility>
 
using namespace std;
using namespace rokubimini::ethercat;
using namespace rokubimini::serial;
 
namespace rokubimini
{
std::shared_ptr<rokubimini::setup::Rokubimini>
createRokubiminiSetup(const std::string& rokubiminiName,
                      std::vector<std::unique_ptr<RokubiminiBusManager>>& busManagers, const NodeHandlePtr& nh)
{
  std::shared_ptr<rokubimini::setup::Rokubimini> rokubimini_setup;
  busType product_code;
  std::string product_code_string = rokubiminiName + "/setup/product_code";
  if (nh->hasParam(product_code_string))
  {
    product_code = parseProductCode(product_code_string, nh);
    switch (product_code)
    {
      case ETHERCAT:
      {
        auto ethercat_setup = make_shared<rokubimini::ethercat::setup::RokubiminiEthercat>();
        ethercat_setup->load(rokubiminiName, nh);
        // check TODO in doxygen.
        bool bus_manager_found = false;
        for (auto& bus_manager : busManagers)
        {
          // To check if the busManager is a
          // RokubiminiEthercatBusManager, we use it's first
          // attached RokubiminiSetup's productCode.
          if (bus_manager->getRokubiminiSetups()[0]->productCode_ == ETHERCAT)
          {
            bus_manager->addRokubiminiSetupToList(ethercat_setup);
            bus_manager_found = true;
            break;
          };
        }
        if (!bus_manager_found)
        {
          auto bus_manager = make_unique<RokubiminiEthercatBusManager>();
          bus_manager->addRokubiminiSetupToList(ethercat_setup);
          busManagers.push_back(move(bus_manager));
        }
        rokubimini_setup = static_pointer_cast<rokubimini::setup::Rokubimini>(ethercat_setup);
        break;
      }
      case SERIAL:
      {
        auto serial_setup = make_shared<rokubimini::serial::setup::RokubiminiSerial>();
        serial_setup->load(rokubiminiName, nh);
        // check TODO in doxygen.
        bool bus_manager_found = false;
        for (auto& bus_manager : busManagers)
        {
          // To check if the busManager is a
          // RokubiminiSerialBusManager, we use it's first
          // attached RokubiminiSetup's productCode.
          if (bus_manager->getRokubiminiSetups()[0]->productCode_ == SERIAL)
          {
            bus_manager->addRokubiminiSetupToList(serial_setup);
            bus_manager_found = true;
            break;
          };
        }
        if (!bus_manager_found)
        {
          auto bus_manager = make_unique<RokubiminiSerialBusManager>();
          bus_manager->addRokubiminiSetupToList(serial_setup);
          busManagers.push_back(move(bus_manager));
        }
        rokubimini_setup = static_pointer_cast<rokubimini::setup::Rokubimini>(serial_setup);
        break;
      }
      default:
        ROS_ERROR("Product code of rokubimini is not valid");
        rokubimini_setup = nullptr;
        break;
    }
  }
  else
  {
    ROS_ERROR("Could not find product code of one Rokubimini in the Bus Setup");
    rokubimini_setup = nullptr;
  }
  return rokubimini_setup;
}
 
busType parseProductCode(const std::string& productCode, const NodeHandlePtr& nh)
{
  int product_code;
  nh->getParam(productCode, product_code);
  return (busType) static_cast<std::uint32_t>(product_code);
}
 
std::shared_ptr<rokubimini::Rokubimini>
createRokubiminiFactory(const std::shared_ptr<rokubimini::setup::Rokubimini>& rokubiminiSetup, const NodeHandlePtr& nh)
{
  std::shared_ptr<rokubimini::Rokubimini> rokubimini = nullptr;
  auto product_code = rokubiminiSetup->productCode_;
  switch (product_code)
  {
    case ETHERCAT:
    {
      rokubimini = make_shared<RokubiminiEthercat>();
      rokubimini->setNodeHandle(nh);
      break;
    }
    case SERIAL:
    {
      rokubimini = make_shared<RokubiminiSerial>();
      rokubimini->setNodeHandle(nh);
      break;
    }
    default:
      ROS_ERROR("Product code of rokubimini with name '%s' is not valid: %u", rokubiminiSetup->name_.c_str(),
                rokubiminiSetup->productCode_);
      break;
  }
  return rokubimini;
}
bool loadBusSetupFromParamServer(const rokubimini::setup::SetupPtr& setup,
                                 std::vector<unique_ptr<RokubiminiBusManager>>& busManagers, const NodeHandlePtr& nh)
{
  ROS_INFO("Loading bus setup");
  try
  {
    std::vector<std::string> keys;
    std::string rokubimini_name;
    // Clear the rokubiminis first.
    setup->rokubiminis_.clear();
    nh->getParamNames(keys);
 
    for (const auto& key : keys)
    {
      // append only parameter names starting with "/rokubimini" and have "/setup/name"
      if (key.find("/rokubimini") == 0 && key.find("/setup/name") != std::string::npos)
      {
        nh->getParam(key, rokubimini_name);
        ROS_DEBUG("Key is: %s and name is: %s", key.c_str(), rokubimini_name.c_str());
        auto rokubimini = createRokubiminiSetup(rokubimini_name, busManagers, nh);
        ROS_DEBUG("New rokubimini with name: %s and product code: %u", rokubimini->name_.c_str(),
                  rokubimini->productCode_);
        setup->rokubiminis_.push_back(rokubimini);
      }
    }
    ROS_DEBUG("Number of bus managers: %zu", busManagers.size());
    ROS_DEBUG("Number of RokubiminiSetups: %zu", setup->rokubiminis_.size());
  }
  catch (const ros::Exception& exception)
  {
    ROS_ERROR_STREAM("Caught an exception while loading the setup: " << exception.what());
    return false;
  }
  return true;
}
 
rokubimini::setup::SetupPtr createSetup()
{
  return rokubimini::setup::SetupPtr(new rokubimini::setup::Setup());
}
rokubimini::setup::SetupPtr loadBusSetup(std::vector<std::unique_ptr<RokubiminiBusManager>>& busManagers,
                                         const NodeHandlePtr& nh)
{
  auto setup = createSetup();
  if (!loadBusSetupFromParamServer(setup, busManagers, nh))
  {
    ROS_ERROR("Could not load Bus Setup!");
    return nullptr;
  }
  return setup;
}
 
}  // namespace rokubimini