tibi_dabo_head_driver.cpp

Go to the documentation of this file.

#include "tibi_dabo_head_driver.h"
#include "exceptions.h"
#include <stdlib.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>
#include <dirent.h>

std::string head_name="tibi_dabo_head";

std::string default_config_file="head_config.xml";
std::string default_motion_file="head_home.xml";

std::string config_path=std::string("xml/config/");
std::string motion_path=std::string("xml/motion/");

TibiDaboHeadDriver::TibiDaboHeadDriver()
{
  //setDriverId(driver string id);
  this->head_driver=NULL;
  this->feedback_rate=10;

  this->xml_path="./";
  this->head_config_file=this->xml_path + config_path + default_config_file;
}

void TibiDaboHeadDriver::scan_XML_files(void)
{
  std::string full_path;
  struct dirent *dp;
  struct stat st;
  DIR *dir;

  full_path=this->xml_path + config_path;
  // check wether the config XML files folder exists or not
  if(stat(full_path.c_str(),&st)==0)// the folder exists
  {
    this->config_files.clear();
    dir=opendir(full_path.c_str());
    while((dp=readdir(dir)) != NULL) 
    {
      if(strstr(dp->d_name,".xml")!=NULL)
        this->config_files.push_back(full_path + dp->d_name);
    }
    closedir(dir);
  }
  ROS_INFO("Found %d configuration files in %s",this->config_files.size(),full_path.c_str());
  full_path=this->xml_path + motion_path;
  if(stat(full_path.c_str(),&st)==0)// the folder exists
  {
    this->motion_seq_files.clear();
    dir=opendir(full_path.c_str());
    while((dp=readdir(dir)) != NULL) 
    {
      if(strstr(dp->d_name,".xml")!=NULL)
        this->motion_seq_files.push_back(full_path + dp->d_name);
    }
    closedir(dir);
  }
  ROS_INFO("Found %d motion sequence files in %s",this->motion_seq_files.size(),full_path.c_str());
}

bool TibiDaboHeadDriver::openDriver(void)
{
  //setDriverId(driver string id);
  try{
    this->lock();
    this->head_driver=new CSegwayHead(head_name);
    this->head_driver->load_config(this->head_config_file);
    this->head_driver->enable();
    this->unlock();
    return true;
  }catch(CException &e){
    if(this->head_driver!=NULL)
    { 
      delete this->head_driver;
      this->head_driver=NULL;
    }
    std::cout << e.what() << std::endl;
    ROS_INFO("Impossible to configure the head with the given parameters");
    this->unlock();
    return false;
  }

  return false;
}

bool TibiDaboHeadDriver::closeDriver(void)
{
  if(this->head_driver!=NULL)
  {
    delete this->head_driver;
    this->head_driver=NULL;
  }

  return true;
}

bool TibiDaboHeadDriver::startDriver(void)
{
  return true;
}

bool TibiDaboHeadDriver::stopDriver(void)
{
  return true;
}

void TibiDaboHeadDriver::config_update(Config& new_cfg, uint32_t level)
{
  this->lock();
  
  // depending on current state
  // update driver with new_cfg data
  switch(this->getState())
  {
    case TibiDaboHeadDriver::CLOSED:
      this->xml_path=new_cfg.XML_path;
      this->scan_XML_files();
      // get the configuration file
      this->head_config_file=this->xml_path + config_path + new_cfg.config_file;
      // get the XML file path and update the xml files
      // configure the node
      break;

    case TibiDaboHeadDriver::OPENED:
      break;

    case TibiDaboHeadDriver::RUNNING:
      // stop the current motion if necessary
      this->head_driver->stop_sequence();
      // configure the robot
      // update the feedback rate
      this->feedback_rate=new_cfg.feedback_rate;
      // update the XML files
      this->xml_path=new_cfg.XML_path;
      this->scan_XML_files();
      break;
  }

  // save the current configuration
  this->config_=new_cfg;

  this->unlock();
}

// API for the sequences
void TibiDaboHeadDriver::start_motion_sequence(std::string &filename)
{
  std::string full_path;

  full_path=this->xml_path + motion_path + filename;
  try{
    this->head_driver->load_sequence(full_path);
    this->head_driver->start_sequence();
  }catch(CException &e){
    std::cout << e.what() << std::endl;
  }
}

void TibiDaboHeadDriver::start_motion_sequence(std::vector<TDynamixelMotionStep> &seq)
{
  unsigned int i=0;

  try{
    this->head_driver->clear_sequence();
    for(i=0;i<seq.size();i++)
      this->head_driver->add_sequence_step(seq[i].position,seq[i].velocity,seq[i].delay);
    this->head_driver->start_sequence();
  }catch(CException &e){
    std::cout << e.what() << std::endl;
  }
}

void TibiDaboHeadDriver::pause_motion_sequence(void)
{
  if(this->head_driver!=NULL)
    this->head_driver->pause_sequence();
}

void TibiDaboHeadDriver::resume_motion_sequence(void)
{
  if(this->head_driver!=NULL)
    this->head_driver->resume_sequence();
}

void TibiDaboHeadDriver::stop_motion_sequence(void)
{
  if(this->head_driver!=NULL)
    this->head_driver->stop_sequence();
}

std::string TibiDaboHeadDriver::get_motion_sequence_complete_event_id(void)
{
  if(this->head_driver!=NULL)
    return this->head_driver->get_sequence_complete_event_id();
  else 
    return std::string("");
}

std::string TibiDaboHeadDriver::get_motion_sequence_error_event_id(void)
{
  if(this->head_driver!=NULL)
    return this->head_driver->get_sequence_error_event_id();
  else
    return std::string("");
}

std::string TibiDaboHeadDriver::get_motion_sequence_error_message(void)
{
  if(this->head_driver!=NULL)
    return this->head_driver->get_sequence_error_message();
  else
    return std::string("");
}

float TibiDaboHeadDriver::get_motion_sequence_completed_percentage(void)
{
  if(this->head_driver!=NULL)
    return this->head_driver->get_sequence_completed_percentage();
  else
    return 0.0;
}

// API for the discete motions
void TibiDaboHeadDriver::move_absolute_angle(std::vector<double> &position,std::vector<double> &velocity)
{
  try{
    if(this->head_driver!=NULL)
      this->head_driver->move_absolute_angle(position,velocity);
  }catch(CException &e){
    std::cout << e.what() << std::endl;
    //throw;  
  }
}

void TibiDaboHeadDriver::move_relative_angle(std::vector<double> &position,std::vector<double> &velocity)
{
  try{
    if(this->head_driver!=NULL)
      this->head_driver->move_relative_angle(position,velocity);
  }catch(CException &e){
    std::cout << e.what() << std::endl;
    //throw;  
  }
}

void TibiDaboHeadDriver::stop(void)
{
  if(this->head_driver!=NULL)
    this->head_driver->stop();
}

void TibiDaboHeadDriver::get_position(std::vector<double> &position)
{
  if(this->head_driver!=NULL)
    this->head_driver->get_current_angle(position);
}

void TibiDaboHeadDriver::get_velocity(std::vector<double> &velocity)
{
  if(this->head_driver!=NULL)
    this->head_driver->get_current_speed(velocity);
}

// face expressions API
void TibiDaboHeadDriver::set_face_expression(std::string &expression)
{
  if(this->head_driver!=NULL)
  {
    if(expression=="stand_by")
      this->head_driver->stand_by();
    else if(expression=="happy")
      this->head_driver->happy();
    else if(expression=="sad")
      this->head_driver->sad();
    else if(expression=="angry")
      this->head_driver->angry();
    else if(expression=="confused")
      this->head_driver->confused();
    else if(expression=="shy")
      this->head_driver->shy();
    else if(expression=="ashamed")
      this->head_driver->ashamed();
    else if(expression=="speak")
      this->head_driver->speak();
  }
}

void TibiDaboHeadDriver::set_face_expression(int index)
{
  if(this->head_driver!=NULL)
  {
    switch(index)
    {
      case 0: this->head_driver->stand_by();
              break;
      case 1: this->head_driver->happy();
              break;
      case 2: this->head_driver->sad();
              break;
      case 3: this->head_driver->angry();
              break;
      case 4: this->head_driver->confused();
              break;
      case 5: this->head_driver->shy();
              break;
      case 6: this->head_driver->ashamed();
              break;
      case 7: this->head_driver->speak();
              break;
      default: /* no nothing */
               break;
    }
  }
}

void TibiDaboHeadDriver::set_brightness(float brightness)
{
  if(this->head_driver!=NULL)
    this->head_driver->set_brightness(brightness);
}

unsigned int TibiDaboHeadDriver::get_num_config_files(void)
{
  return this->config_files.size();
}

std::string TibiDaboHeadDriver::get_config_file(unsigned int index)
{
  if(index > this->config_files.size())
  {
    /* handle exceptions */
  }
  return this->config_files[index];
}

unsigned int TibiDaboHeadDriver::get_num_motion_seq_files(void)
{
  return this->motion_seq_files.size();
}

std::string TibiDaboHeadDriver::get_motion_seq_file(unsigned int index)
{
  if(index > this->motion_seq_files.size())
  {
    /* handle exceptions */
  }
  return this->motion_seq_files[index];
}

int TibiDaboHeadDriver::get_feedback_rate(void)
{
  return this->feedback_rate;
}

TibiDaboHeadDriver::~TibiDaboHeadDriver()
{
  if(this->head_driver!=NULL)
  {
    delete this->head_driver;
    this->head_driver=NULL;
  }
}