dabo_arm_node_driver.cpp
Go to the documentation of this file.
00001 #include "dabo_arm_node_driver.h"
00002 #include "exceptions.h"
00003 #include <sys/types.h>
00004 #include <sys/stat.h>
00005 #include <unistd.h>
00006 #include <dirent.h>
00007 
00008 std::string arm_name="dabo_arm";
00009 std::string arm_motion_sequence_name="arm_motion_sequence";
00010 
00011 std::string default_config_file="dabo_arm_config.xml";
00012 std::string default_motion_file="motion1.xml";
00013 
00014 std::string config_path=std::string("xml/config/");
00015 std::string motion_path=std::string("xml/motion/");
00016 
00017 
00018 DaboArmNodeDriver::DaboArmNodeDriver(void)
00019 {
00020   //setDriverId(driver string id);
00021   this->arm_driver=NULL;
00022   this->feedback_rate=10;
00023 
00024   this->xml_path="./";
00025   this->arm_config_file=this->xml_path + config_path + default_config_file;
00026 }
00027 
00028 void DaboArmNodeDriver::scan_XML_files(void)
00029 {
00030   std::string full_path;
00031   struct dirent *dp;
00032   struct stat st;
00033   DIR *dir;
00034 
00035   full_path=this->xml_path + config_path;
00036   // check wether the config XML files folder exists or not
00037   if(stat(full_path.c_str(),&st)==0)// the folder exists
00038   {
00039     this->config_files.clear();
00040     dir=opendir(full_path.c_str());
00041     while((dp=readdir(dir)) != NULL)
00042     {
00043       if(strstr(dp->d_name,".xml")!=NULL)
00044         this->config_files.push_back(full_path + dp->d_name);
00045     }
00046     closedir(dir);
00047   }
00048   ROS_INFO("Found %d configuration files in %s",this->config_files.size(),full_path.c_str());
00049   full_path=this->xml_path + motion_path;
00050   if(stat(full_path.c_str(),&st)==0)// the folder exists
00051   {
00052     this->motion_seq_files.clear();
00053     dir=opendir(full_path.c_str());
00054     while((dp=readdir(dir)) != NULL)
00055     {
00056       if(strstr(dp->d_name,".xml")!=NULL)
00057         this->motion_seq_files.push_back(full_path + dp->d_name);
00058     }
00059     closedir(dir);
00060   }
00061   ROS_INFO("Found %d motion sequence files files in %s",this->motion_seq_files.size(),full_path.c_str());
00062 }
00063 
00064 bool DaboArmNodeDriver::openDriver(void)
00065 {
00066   //setDriverId(driver string id);
00067   try{
00068     this->lock();
00069     this->arm_driver=new CSegwayDynArm(arm_name);
00070     this->arm_driver->load_config(this->arm_config_file);
00071     this->arm_driver->enable();
00072     this->unlock();
00073     return true;
00074   }catch(CException &e){
00075     if(this->arm_driver!=NULL)
00076     {
00077       delete this->arm_driver;
00078       this->arm_driver=NULL;
00079     }
00080     std::cout << e.what() << std::endl;
00081     ROS_INFO("Impossible to configure the head with the given parameters");
00082     this->unlock();
00083     return false;
00084   }
00085 
00086   return false;
00087 }
00088 
00089 bool DaboArmNodeDriver::closeDriver(void)
00090 {
00091   if(this->arm_driver!=NULL)
00092   {
00093     delete this->arm_driver;
00094     this->arm_driver=NULL;
00095   }
00096 
00097   return true;
00098 }
00099 
00100 bool DaboArmNodeDriver::startDriver(void)
00101 {
00102   return true;
00103 }
00104 
00105 bool DaboArmNodeDriver::stopDriver(void)
00106 {
00107   return true;
00108 }
00109 
00110 void DaboArmNodeDriver::config_update(Config& new_cfg, uint32_t level)
00111 {
00112   this->lock();
00113 
00114   // depending on current state
00115   // update driver with new_cfg data
00116   switch(this->getState())
00117   {
00118     case DaboArmNodeDriver::CLOSED:
00119       this->xml_path=new_cfg.XML_path;
00120       this->scan_XML_files();
00121       // get the configuration file
00122       this->arm_config_file=this->xml_path + config_path + new_cfg.config_file;
00123       // get the XML file path and update the xml files
00124       // configure the node
00125       break;
00126 
00127     case DaboArmNodeDriver::OPENED:
00128       break;
00129 
00130     case DaboArmNodeDriver::RUNNING:
00131       // stop the current motion if necessary
00132       this->arm_driver->stop_sequence();
00133       // configure the robot
00134       // update the feedback rate
00135       this->feedback_rate=new_cfg.feedback_rate;
00136       // update the XML files
00137       this->xml_path=new_cfg.XML_path;
00138       this->scan_XML_files();
00139       break;
00140   }
00141 
00142   // save the current configuration
00143   this->config_=new_cfg;
00144 
00145   this->unlock();
00146 }
00147 
00148 // API for the sequences
00149 void DaboArmNodeDriver::start_motion_sequence(std::string &filename)
00150 {
00151   std::string full_path;
00152 
00153   full_path=this->xml_path + motion_path + filename;
00154   try{
00155     this->arm_driver->load_sequence(full_path);
00156     this->arm_driver->start_sequence();
00157   }catch(CException &e){
00158     std::cout << e.what() << std::endl;
00159   }
00160 }
00161 
00162 void DaboArmNodeDriver::start_motion_sequence(std::vector<TDynamixelMotionStep> &seq)
00163 {
00164   unsigned int i=0;
00165 
00166   try{
00167     this->arm_driver->clear_sequence();
00168     for(i=0;i<seq.size();i++)
00169       this->arm_driver->add_sequence_step(seq[i].position,seq[i].velocity,seq[i].delay);
00170     this->arm_driver->start_sequence();
00171   }catch(CException &e){
00172     std::cout << e.what() << std::endl;
00173   }
00174 }
00175 
00176 void DaboArmNodeDriver::pause_motion_sequence(void)
00177 {
00178   if(this->arm_driver!=NULL)
00179     this->arm_driver->pause_sequence();
00180 }
00181 
00182 void DaboArmNodeDriver::resume_motion_sequence(void)
00183 {
00184   if(this->arm_driver!=NULL)
00185     this->arm_driver->resume_sequence();
00186 }
00187 
00188 void DaboArmNodeDriver::stop_motion_sequence(void)
00189 {
00190   if(this->arm_driver!=NULL)
00191     this->arm_driver->stop_sequence();
00192 }
00193 
00194 std::string DaboArmNodeDriver::get_motion_sequence_complete_event_id(void)
00195 {
00196   if(this->arm_driver!=NULL)
00197     return this->arm_driver->get_sequence_complete_event_id();
00198   else
00199     return std::string("");
00200 }
00201 
00202 std::string DaboArmNodeDriver::get_motion_sequence_error_event_id(void)
00203 {
00204   if(this->arm_driver!=NULL)
00205     return this->arm_driver->get_sequence_error_event_id();
00206   else
00207     return std::string("");
00208 }
00209 
00210 std::string DaboArmNodeDriver::get_motion_sequence_error_message(void)
00211 {
00212   if(this->arm_driver!=NULL)
00213     return this->arm_driver->get_sequence_error_message();
00214   else
00215     return std::string("");
00216 }
00217 
00218 float DaboArmNodeDriver::get_motion_sequence_completed_percentage(void)
00219 {
00220   if(this->arm_driver!=NULL)
00221     return this->arm_driver->get_sequence_completed_percentage();
00222   else
00223     return 0.0;
00224 }
00225 
00226 // API for the discete motions
00227 void DaboArmNodeDriver::move_absolute_angle(std::vector<double> &position,std::vector<double> &velocity)
00228 {
00229   try{
00230     if(this->arm_driver!=NULL)
00231       this->arm_driver->move_absolute_angle(position,velocity);
00232   }catch(CException &e){
00233     std::cout << e.what() << std::endl;
00234     //throw;  
00235   }
00236 }
00237 
00238 void DaboArmNodeDriver::move_relative_angle(std::vector<double> &position,std::vector<double> &velocity)
00239 {
00240   try{
00241     if(this->arm_driver!=NULL)
00242       this->arm_driver->move_relative_angle(position,velocity);
00243   }catch(CException &e){
00244     std::cout << e.what() << std::endl;
00245     //throw;  
00246   }
00247 }
00248 
00249 void DaboArmNodeDriver::stop(void)
00250 {
00251   if(this->arm_driver!=NULL)
00252     this->arm_driver->stop();
00253 }
00254 
00255 void DaboArmNodeDriver::get_position(std::vector<double> &position)
00256 {
00257   if(this->arm_driver!=NULL)
00258     this->arm_driver->get_current_angle(position);
00259 }
00260 
00261 void DaboArmNodeDriver::get_velocity(std::vector<double> &velocity)
00262 {
00263   if(this->arm_driver!=NULL)
00264     this->arm_driver->get_current_speed(velocity);
00265 }
00266 
00267 unsigned int DaboArmNodeDriver::get_num_config_files(void)
00268 {
00269   return this->config_files.size();
00270 }
00271 
00272 std::string DaboArmNodeDriver::get_config_file(unsigned int index)
00273 {
00274   if(index > this->config_files.size())
00275   {
00276     /* handle exceptions */
00277   }
00278   return this->config_files[index];
00279 }
00280 
00281 unsigned int DaboArmNodeDriver::get_num_motion_seq_files(void)
00282 {
00283   return this->motion_seq_files.size();
00284 }
00285 
00286 std::string DaboArmNodeDriver::get_motion_seq_file(unsigned int index)
00287 {
00288   if(index > this->motion_seq_files.size())
00289   {
00290     /* handle exceptions */
00291   }
00292   return this->motion_seq_files[index];
00293 }
00294 
00295 int DaboArmNodeDriver::get_feedback_rate(void)
00296 {
00297   return this->feedback_rate;
00298 }
00299 
00300 DaboArmNodeDriver::~DaboArmNodeDriver(void)
00301 {
00302   this->lock();
00303   if(this->arm_driver!=NULL)
00304   {
00305     delete this->arm_driver;
00306     this->arm_driver=NULL;
00307   }
00308   this->unlock();
00309 }


dabo_arm_node
Author(s): Sergi Hernandez Juan
autogenerated on Fri Dec 6 2013 21:40:30