tibi_arm_node_driver.cpp

Go to the documentation of this file.

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

std::string arm_name="tibi_arm";
std::string arm_motion_sequence_name="arm_motion_sequence";

std::string default_config_file="tibi_arm_config.xml";
std::string default_motion_file="motion1.xml";

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

TibiArmNodeDriver::TibiArmNodeDriver(void)
{
  //setDriverId(driver string id);
  this->arm_driver=NULL;
  this->feedback_rate=10;

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

void TibiArmNodeDriver::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 files in %s",this->motion_seq_files.size(),full_path.c_str());
}


bool TibiArmNodeDriver::openDriver(void)
{
  //setDriverId(driver string id);
  try{
    this->lock();
    this->arm_driver=new CPTUD46(arm_name);
    this->arm_driver->load_config(this->arm_config_file);
    this->unlock();
    return true;
  }catch(CException &e){
    if(this->arm_driver!=NULL)
    {
      delete this->arm_driver;
      this->arm_driver=NULL;
    }
    std::cout << e.what() << std::endl;
    ROS_INFO("Impossible to configure the head with the given parameters");
    this->unlock();
    return false;
  }catch(...){
    std::cout << "unknown exception!!!" << std::endl;
    this->unlock();
  }

  return false;
}

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

  return true;
}

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

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

void TibiArmNodeDriver::config_update(Config& new_cfg, uint32_t level)
{
  this->lock();

  // depending on current state
  // update driver with new_cfg data
  switch(this->getState())
  {
    case TibiArmNodeDriver::CLOSED:
      this->xml_path=new_cfg.XML_path;
      this->scan_XML_files();
      // get the configuration file
      this->arm_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 TibiArmNodeDriver::OPENED:
      break;

    case TibiArmNodeDriver::RUNNING:
      // stop the current motion if necessary
      this->arm_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 TibiArmNodeDriver::start_motion_sequence(std::string &filename)
{
  std::string full_path;

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

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

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

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

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

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

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

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

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

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

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

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

void TibiArmNodeDriver::stop(void)
{
  if(this->arm_driver!=NULL)
    this->arm_driver->stop();
}

void TibiArmNodeDriver::get_position(std::vector<double> &position)
{
  if(this->arm_driver!=NULL)
    position=this->arm_driver->get_current_angles();
}

void TibiArmNodeDriver::get_velocity(std::vector<double> &velocity)
{
  if(this->arm_driver!=NULL)
    velocity=this->arm_driver->get_current_speeds();
}

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

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

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

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

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

TibiArmNodeDriver::~TibiArmNodeDriver(void)
{
  this->lock();
  if(this->arm_driver!=NULL)
  {
    delete this->arm_driver;
    this->arm_driver=NULL;
  }
  this->unlock();
}