group.cpp

Go to the documentation of this file.

#include "group.hpp"
#include "group_command.hpp"
#include "group_feedback.hpp"
#include "group_info.hpp"
#include "log_file.hpp"

namespace hebi {

#ifndef DOXYGEN_OMIT_INTERNAL
void callbackWrapper(HebiGroupFeedbackPtr group_feedback, void* user_data)
{
  reinterpret_cast<Group*>(user_data)->callAttachedHandlers(group_feedback);
}
#endif // DOXYGEN_OMIT_INTERNAL

void Group::callAttachedHandlers(HebiGroupFeedbackPtr group_feedback)
{
  // Wrap this:
  GroupFeedback wrapped_fbk(group_feedback);
  // Call handlers:
  std::lock_guard<std::mutex> lock_guard(handler_lock_);
  for (unsigned int i = 0; i < handlers_.size(); i++)
  {
    GroupFeedbackHandler handler = handlers_[i];
    try
    {
      handler(wrapped_fbk);
    }
    catch (...)
    {

    }
  }
}

Group::Group(HebiGroupPtr group,
             float initial_feedback_frequency,
             int32_t initial_command_lifetime)
  : internal_(group), number_of_modules_(hebiGroupGetSize(internal_))
{
  if (initial_feedback_frequency != 0)
    setFeedbackFrequencyHz(initial_feedback_frequency);
  if (initial_command_lifetime != 0)
    setCommandLifetimeMs(initial_command_lifetime);
}

std::shared_ptr<Group> Group::createImitation(size_t size)
{
  return std::make_shared<Group>(hebiGroupCreateImitation(size));
}

Group::~Group() noexcept
{
  // Cleanup group object allocated by the C library
  if (internal_ != nullptr)
    hebiGroupRelease(internal_);
}

int Group::size()
{
  return number_of_modules_;
}

bool Group::setCommandLifetimeMs(int32_t ms)
{
  return (hebiGroupSetCommandLifetime(internal_, ms) == HebiStatusSuccess);
}

bool Group::sendCommand(const GroupCommand& group_command)
{
  return (hebiGroupSendCommand(internal_, group_command.internal_) == HebiStatusSuccess);
}

bool Group::sendCommandWithAcknowledgement(const GroupCommand& group_command, int32_t timeout_ms)
{
  return (hebiGroupSendCommandWithAcknowledgement(internal_, group_command.internal_, timeout_ms) == HebiStatusSuccess);
}

bool Group::sendFeedbackRequest()
{
  return (hebiGroupSendFeedbackRequest(internal_) == HebiStatusSuccess);
}

bool Group::getNextFeedback(GroupFeedback& feedback, int32_t timeout_ms)
{
  return (hebiGroupGetNextFeedback(internal_, feedback.internal_, timeout_ms) == HebiStatusSuccess);
}

bool Group::requestInfo(GroupInfo& info, int32_t timeout_ms)
{
  return (hebiGroupRequestInfo(internal_, info.internal_, timeout_ms) == HebiStatusSuccess);
}

std::string Group::startLog(const std::string& dir)
{
  HebiStringPtr str;
  if (hebiGroupStartLog(internal_, dir.c_str(), nullptr, &str) == HebiStatusSuccess)
  {
    assert(str);
    size_t len;

    hebiStringGetString(str, nullptr, &len);
    char* buffer = new char[len];
    hebiStringGetString(str, buffer, &len);
    std::string ret(buffer, --len);

    delete[] buffer;
    hebiStringRelease(str);

    return ret;
  }
  return "";
}

std::string Group::startLog(const std::string& dir, const std::string& file)
{
  HebiStringPtr str;
  if (hebiGroupStartLog(internal_, dir.c_str(), file.c_str(), &str) == HebiStatusSuccess)
  {
    assert(str);
    size_t len;

    hebiStringGetString(str, nullptr, &len);
    char* buffer = new char[len];
    hebiStringGetString(str, buffer, &len);
    std::string ret(buffer, --len);

    delete[] buffer;
    hebiStringRelease(str);

    return ret;
  }
  return "";
}

std::shared_ptr<LogFile> Group::stopLog()
{
  auto internal = hebiGroupStopLog(internal_);
  if (internal == nullptr) {
    return std::shared_ptr<LogFile>();
  }

  return std::shared_ptr<LogFile>(
    new LogFile(internal, hebiLogFileGetNumberOfModules(internal))
  );
}

bool Group::setFeedbackFrequencyHz(float frequency)
{
  return (hebiGroupSetFeedbackFrequencyHz(internal_, frequency) == HebiStatusSuccess);
}

float Group::getFeedbackFrequencyHz()
{
  return hebiGroupGetFeedbackFrequencyHz(internal_);
}

void Group::addFeedbackHandler(GroupFeedbackHandler handler)
{
  std::lock_guard<std::mutex> lock_guard(handler_lock_);
  handlers_.push_back(handler);
  if (handlers_.size() == 1) // (i.e., this was the first one)
    hebiGroupRegisterFeedbackHandler(internal_, callbackWrapper, reinterpret_cast<void*>(this));
}

void Group::clearFeedbackHandlers()
{
  std::lock_guard<std::mutex> lock_guard(handler_lock_);
  hebiGroupClearFeedbackHandlers(internal_);
  handlers_.clear();
}

} // namespace hebi