BotaDeviceDriver.cpp

Go to the documentation of this file.

#include <bota_device_driver/BotaDeviceDriver.hpp>

#include <string>
#include <utility>

namespace bota_device_driver
{
BotaDeviceDriver::BotaDeviceDriver(const bota_node::Node::NodeHandlePtr& nh) : bota_node::Node(nh), nh_(nh)
{
}

bool BotaDeviceDriver::init()
{
  const bool is_standalone = false;
  const bool install_signal_handler = false;
  double time_step;
  std::string setup_file;
  nh_->param<double>("time_step", time_step, 0.01);
  nh_->param<std::string>("rokubimini_setup_file", setup_file, "");

  rokubiminiManager_ = std::make_unique<RokubiminiManager>(is_standalone, install_signal_handler, time_step, nh_);

  if (!rokubiminiManager_->loadSetup())
  {
    ROS_ERROR_STREAM("Could not load setup file");
  }

  if (!rokubiminiManager_->startup())
  {
    ROS_WARN_STREAM("Manager could not startup");
    return false;
  }
  rokubiminiManager_->setNodeHandle(nh_);
  rokubiminiManager_->createRokubiminiRosPublishers();
  rokubiminiManager_->createRokubiminiRosServices();
  std::vector<std::shared_ptr<Rokubimini>> rokubiminis;
  // Load all available rokubiminis
  rokubiminis = rokubiminiManager_->getRokubiminis();
  if (rokubiminis.empty())
  {
    ROS_ERROR_STREAM("No rokubimini available");
    return false;
  }

  bota_worker::WorkerOptions options;
  options.callback_ = std::bind(&BotaDeviceDriver::update, this, std::placeholders::_1);
  options.defaultPriority_ = 10;  // this has high priority
  options.name_ = "BotaDeviceDriver::updateWorker";

  int minimumft_sampling_rate = std::numeric_limits<int>::max();
  /* the following function has to be after setting the filter in order Sampling rate Object to be updated.
   * The filters are set inside the startup function above */
  for (const auto& rokubimini : rokubiminis)
  {
    int ft_sampling_rate;
    rokubimini->getForceTorqueSamplingRate(ft_sampling_rate);
    minimumft_sampling_rate = std::min(minimumft_sampling_rate, ft_sampling_rate);
  }

  /* never compare with equality floats */
  if (time_step <= 0.000001)
  {
    options.timeStep_ = 1.0 / static_cast<float>(minimumft_sampling_rate);
    ROS_WARN_STREAM("Starting Worker at " << 1 / options.timeStep_ << " Hz, based on force/torque sampling rate.");
  }
  else
  {
    options.timeStep_ = time_step;
    ROS_WARN_STREAM("Starting Worker at " << 1 / options.timeStep_ << " Hz, based on selected time step.");
  }

  if (!this->addWorker(options))
  {
    ROS_ERROR_STREAM("[BotaDeviceDriver] Worker " << options.name_ << "could not be added!");
    return false;
  }
  ROS_DEBUG("Finished initializing device driver");
  return true;
}

bool BotaDeviceDriver::update(const bota_worker::WorkerEvent& event)
{
  ROS_DEBUG("[BotaDeviceDriver]: update called");
  rokubiminiManager_->updateCommunicationManagerReadMessages();
  rokubiminiManager_->updateProcessReadings();

  rokubiminiManager_->updateCommunicationManagerWriteMessages();

  rokubiminiManager_->publishRosMessages();
  return true;
}

void BotaDeviceDriver::cleanup()
{
  rokubiminiManager_->shutdown();
}

}  // namespace bota_device_driver