dynamixel_nc_gripper_driver.cpp

Go to the documentation of this file.

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

std::string config_path = std::string("/config/");

DynamixelNcGripperDriver::DynamixelNcGripperDriver(void):
    // dynamixel attributes (default values)
    gripper_config_file_(std::string("gripper_nc_config.xml")),
    xml_path_(std::string(".")),
    full_path_(xml_path_ + config_path + gripper_config_file_)
{
    //setDriverId(driver string id);
}

DynamixelNcGripperDriver::~DynamixelNcGripperDriver(void)
{
}

void DynamixelNcGripperDriver::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
  {
    dir=opendir(full_path.c_str());
    while((dp=readdir(dir)) != NULL) 
    {
      if(strstr(dp->d_name,".xml")!=NULL)
        this->full_path_ = full_path + dp->d_name;
    }
    closedir(dir);
  }
  ROS_INFO("Found gripper configuration file %s in %s", gripper_config_file_.c_str(), full_path.c_str());
}

bool DynamixelNcGripperDriver::openDriver(void)
{
    try
    {
        this->lock();
        this->gripper_ = boost::shared_ptr<CDynamixelGripper>(new CDynamixelGripper());
        gripper_->load_config(this->full_path_);
        std::cout << *gripper_ << std::endl;
        this->unlock();
        return true;
    }
    catch(CException &e)
    {
        std::cout << e.what() << std::endl;
        ROS_INFO("Impossible to configure the Gripper with the given parameters");
        this->unlock();
        return false;
    }

    return false;

}

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

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

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

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

    // depending on current state
    // update driver with new_cfg data
    switch(this->getState())
    {
        case DynamixelNcGripperDriver::CLOSED:
            // save the values into the class
            this->xml_path_ = new_cfg.XML_path;
            this->scan_XML_files();
            this->full_path_ = this->xml_path_ + config_path + new_cfg.config_file;
            break;

        case DynamixelNcGripperDriver::OPENED:
            break;

        case DynamixelNcGripperDriver::RUNNING:
            try
            {
                // set the torque
            }
            catch(CDynamixelMotorException &e)
            {
                ROS_ERROR("%s",e.what().c_str());
            }
            break;
    }

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

    this->unlock();
}


bool DynamixelNcGripperDriver::open_gripper(void)
{
//    return move_gripper(max_angle_);
    return this->gripper_->open_gripper();
    
}

bool DynamixelNcGripperDriver::close_gripper()
{
//    return move_gripper(min_angle_);
    return this->gripper_->close_gripper();
}

void DynamixelNcGripperDriver::stop_gripper()
{
    if (this->getState() != DynamixelNcGripperDriver::CLOSED)
    {
        try
        {
              this->gripper_->disable();
        }
        catch (CDynamixelMotorException &e)
        {
            ROS_ERROR("%s", e.what().c_str());
        }
    }
}

float DynamixelNcGripperDriver::get_current_effort()
{
    return this->gripper_->get_current_effort();
}

float DynamixelNcGripperDriver::get_current_angle()
{
    return this->gripper_->get_current_angle();
}

bool DynamixelNcGripperDriver::is_at_max_angle()
{
    return this->gripper_->is_gripper_opened();

}

bool DynamixelNcGripperDriver::is_at_min_angle()
{
    return this->gripper_->is_gripper_closed();
}

void DynamixelNcGripperDriver::disable_gripper()
{
    this->gripper_->disable();
}