dabo_arm_node_driver.cpp

Go to the documentation of this file.

#include "dabo_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="dabo_arm";
std::string arm_motion_sequence_name="arm_motion_sequence";

std::string default_config_file="dabo_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/");


DaboArmNodeDriver::DaboArmNodeDriver(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 DaboArmNodeDriver::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 DaboArmNodeDriver::openDriver(void)
{
  //setDriverId(driver string id);
  try{
    this->lock();
    this->arm_driver=new CSegwayDynArm(arm_name);
    this->arm_driver->load_config(this->arm_config_file);
    this->arm_driver->enable();
    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;
  }

  return false;
}

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

  return true;
}

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

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

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

  // depending on current state
  // update driver with new_cfg data
  switch(this->getState())
  {
    case DaboArmNodeDriver::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 DaboArmNodeDriver::OPENED:
      break;

    case DaboArmNodeDriver::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 DaboArmNodeDriver::start_motion_sequence(std::string &filename)
{
  std::string full_path;

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

void DaboArmNodeDriver::start_motion_sequence(std::vector<TDynamixelMotionStep> &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 DaboArmNodeDriver::pause_motion_sequence(void)
{
  if(this->arm_driver!=NULL)
    this->arm_driver->pause_sequence();
}

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

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

std::string DaboArmNodeDriver::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 DaboArmNodeDriver::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 DaboArmNodeDriver::get_motion_sequence_error_message(void)
{
  if(this->arm_driver!=NULL)
    return this->arm_driver->get_sequence_error_message();
  else
    return std::string("");
}

float DaboArmNodeDriver::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 DaboArmNodeDriver::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 DaboArmNodeDriver::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 DaboArmNodeDriver::stop(void)
{
  if(this->arm_driver!=NULL)
    this->arm_driver->stop();
}

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

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

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

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

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

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

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

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