tibi_dabo_arm_driver.cpp

Go to the documentation of this file.

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

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

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

TibiDaboArmDriver::TibiDaboArmDriver():arm_driver(arm_name)
{
  //setDriverId(driver string id);
  //setDriverId(driver string id);
  this->motion_sequence=NULL;
  this->motion_sequence=new CMotionSequence(arm_motion_sequence_name);
  this->feedback_rate=10;
  this->absolute_motion=true;
  this->position_control=true;


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

void TibiDaboArmDriver::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 TibiDaboArmDriver::openDriver(void)
{
  std::vector<bool> absolute(4);
  std::vector<bool> enabled;
  unsigned int i=0;

  //setDriverId(driver string id);
  try{
    this->lock();
    this->arm_driver.load_config(this->arm_config_file);
    for(i=0;i<this->arm_driver.get_num_motors();i++)
      enabled.push_back(true);
    this->arm_driver.enable(enabled);
    if(this->position_control)
      this->arm_driver.set_position_control();
    else
      this->arm_driver.set_velocity_control();
    if(this->absolute_motion)
    {
      for(i=0;i<this->arm_driver.get_num_motors();i++)
        absolute[i]=true;
    }
    else
    {
      for(i=0;i<this->arm_driver.get_num_motors();i++)
        absolute[i]=false;
    }
    this->arm_driver.set_absolute_motion(absolute);
    this->motion_sequence->set_motor_group(&this->arm_driver);
    this->motion_sequence->set_timeout_factor(1.5);
    this->motion_sequence->set_acceleration_factor(0.5);
    this->unlock();
    return true;
  }catch(CException &e){
    std::cout << e.what() << std::endl;
    ROS_INFO("Impossible to configure the arm with the given parameters");
    this->unlock();
    return false;
  }
}

bool TibiDaboArmDriver::closeDriver(void)
{
  return true;
}

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

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

void TibiDaboArmDriver::config_update(Config& new_cfg, uint32_t level)
{
  std::vector<bool> absolute(2);
  std::string motion_seq;
  unsigned int i=0;

  this->lock();

  std::cout << "config_update" << std::endl; 
  // depending on current state
  // update driver with new_cfg data
  switch(this->getState())
  {
    case TibiDaboArmDriver::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;
      if(new_cfg.motion_mode==0)// absolute motion
        this->absolute_motion=true;
      else
        this->absolute_motion=false;
      if(new_cfg.control_mode==0)// position control
        this->position_control=true;
      else
        this->position_control=false;
      break;

      break;

    case TibiDaboArmDriver::OPENED:
      break;

    case TibiDaboArmDriver::RUNNING:
      // stop the current motion if necessary
      if(this->motion_sequence!=NULL)
      {
        if(this->motion_sequence->get_current_state()==mtn_started ||
           this->motion_sequence->get_current_state()==mtn_paused)
          this->motion_sequence->stop_sequence();
      }
      // configure the robot
      if(new_cfg.control_mode==0)// position control
      {
        this->position_control=true;
        this->arm_driver.set_position_control();
      }
      else
      {
        this->position_control=false;
        this->arm_driver.set_velocity_control();
      }
      if(new_cfg.motion_mode==0)//absolute motion
        for(i=0;i<this->arm_driver.get_num_motors();i++)
          absolute[i]=true;
      else
        for(i=0;i<this->arm_driver.get_num_motors();i++)
          absolute[i]=false;
      this->arm_driver.set_absolute_motion(absolute);
      // 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 TibiDaboArmDriver::start_motion_sequence(std::string &filename)
{
  std::string full_path;

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

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

  try{
    if(this->motion_sequence==NULL)
    {
      this->motion_sequence=new CMotionSequence(arm_motion_sequence_name);
      this->motion_sequence->set_motor_group(&this->arm_driver);
      this->motion_sequence->set_timeout_factor(1.5);
      this->motion_sequence->set_acceleration_factor(0.5);
    }
    this->motion_sequence->clear_sequence();
    for(i=0;i<seq.size();i++)
      this->motion_sequence->add_step(seq[i].position,seq[i].velocity,seq[i].delay);
    this->motion_sequence->start_sequence();
  }catch(CException &e){
    std::cout << e.what() << std::endl;
  }
}

void TibiDaboArmDriver::pause_motion_sequence(void)
{
  this->motion_sequence->pause_sequence();
}

void TibiDaboArmDriver::resume_motion_sequence(void)
{
  this->motion_sequence->resume_sequence();
}

void TibiDaboArmDriver::stop_motion_sequence(void)
{
  this->motion_sequence->stop_sequence();
}

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

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

std::string TibiDaboArmDriver::get_motion_sequence_error_message(void)
{
  if(this->motion_sequence!=NULL)
    return this->motion_sequence->get_error_message();
  else
    return std::string("");
}

float TibiDaboArmDriver::get_motion_sequence_completed_percentage(void)
{
  if(this->motion_sequence!=NULL)
    return this->motion_sequence->get_completed_percentage();
  else
    return 0.0;
}

// API for the discete motions
std::string TibiDaboArmDriver::get_position_reached_event_id(void)
{
  return this->arm_driver.get_position_reached_event_id();
}

void TibiDaboArmDriver::move(std::vector<float> &position,std::vector<float> &velocity,std::vector<float> &acceleration)
{
  CEventServer *event_server=CEventServer::instance();
  std::list<std::string> events;

  try{
    if(this->motion_sequence!=NULL)
    {
      delete this->motion_sequence;
      this->motion_sequence=NULL;
      this->arm_driver.move(position,velocity,acceleration);
    }
    else
    {
      if(event_server->event_is_set(this->arm_driver.get_position_reached_event_id()))
      {
        events.push_back(this->arm_driver.get_position_reached_event_id());
        event_server->wait_all(events);
        this->arm_driver.move(position,velocity,acceleration);
      }
    }
  }catch(CException &e){
    std::cout << e.what() << std::endl;
    //throw;
  }
}

void TibiDaboArmDriver::stop(void)
{
  this->arm_driver.stop();
}

void TibiDaboArmDriver::get_position(std::vector<float> &position)
{
  this->arm_driver.get_position(position);
}

void TibiDaboArmDriver::get_velocity(std::vector<float> &velocity)
{
  this->arm_driver.get_velocity(velocity);
}

void TibiDaboArmDriver::set_absolute_motion(bool status)
{
  std::vector<bool> absolute;
  unsigned int i=0;

  try{
    for(i=0;i<this->arm_driver.get_num_motors();i++)
      absolute.push_back(status);
    this->arm_driver.set_absolute_motion(absolute);
  }catch(CException &e){
    std::cout << e.what() << std::endl;
    throw;
  }
}

void TibiDaboArmDriver::set_position_control(bool status)
{
  try{
    if(status==true)
      this->arm_driver.set_position_control();
    else
      this->arm_driver.set_velocity_control();
  }catch(CException &e){
    std::cout << e.what() << std::endl;
    throw;
  }
}

bool TibiDaboArmDriver::using_absolute_motion(void)
{
  return this->absolute_motion;
}

bool TibiDaboArmDriver::using_position_control(void)
{
  return this->position_control;
}

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

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

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

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

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

TibiDaboArmDriver::~TibiDaboArmDriver()
{
}