tibi_dabo_laser_3d_driver.cpp

Go to the documentation of this file.

#include "tibi_dabo_laser_3d_driver.h"
#include "exceptions.h"
#include "ros/ros.h"

TibiDaboLaser3dDriver::TibiDaboLaser3dDriver(void)
{
  //setDriverId(driver string id);
  this->servo=NULL;
  this->serial_number="";
  this->baudrate=1000000;
  this->servo_id=(unsigned char)-1;
  this->servo_config_file="";
  this->scan_enabled=false;
  this->fixed_angle=0.0;
  ros::param::get("~scan_velocity", this->new_velocity);
  ros::param::get("~min_angle", this->new_min_angle);
  ros::param::get("~max_angle", this->new_max_angle);
  ros::param::get("~scan_velocity", this->current_velocity);
  ros::param::get("~min_angle", this->current_min_angle);
  ros::param::get("~max_angle", this->current_max_angle);
  this->dir=right_left_scan;
}

bool TibiDaboLaser3dDriver::openDriver(void)
{
  double current_angle;
  std::string servo_name="laser_3d_servo";

  //setDriverId(driver string id);
  CDynamixelServer *dyn_server=CDynamixelServer::instance();
  try{
    if(dyn_server->get_num_buses()>0)
    {
      this->servo=new CDynamixelMotor(servo_name,
                                      this->serial_number,
                                      this->baudrate,
                                      this->servo_id);
      this->servo->enable();
      this->servo->load_config(this->servo_config_file);
      current_angle=this->servo->get_current_angle();
      if(this->scan_enabled)
      {
        this->servo->move_absolute_angle(this->current_min_angle,this->current_velocity);
        usleep(fabs((this->current_min_angle-current_angle)/this->current_velocity)*1000000);
      }
      {
        this->servo->move_absolute_angle(this->fixed_angle,this->current_velocity);
        usleep(fabs((this->fixed_angle-current_angle)/this->current_velocity)*1000000);
      }
      return true;
    }
    else
      return false;
  }catch(CException &e){
    if(this->servo!=NULL)
    {
      delete this->servo;
      this->servo=NULL;
    }
    ROS_WARN("%s",e.what().c_str());
    return false;
  }

  return true;
}

bool TibiDaboLaser3dDriver::closeDriver(void)
{
  if(this->servo!=NULL)
  {
    delete this->servo;
    this->servo=NULL;
  }
  return true;
}

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

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

void TibiDaboLaser3dDriver::config_update(Config& new_cfg, uint32_t level)
{
  this->lock();
  
  // depending on current state
  // update driver with new_cfg data
  switch(this->getState())
  {
    case TibiDaboLaser3dDriver::CLOSED:
      this->serial_number=new_cfg.bus_serial;
      this->baudrate=new_cfg.bus_baudrate;
      this->servo_id=new_cfg.servo_id;
      this->servo_config_file=new_cfg.config_file;
      this->scan_enabled=new_cfg.scan;
      this->fixed_angle=new_cfg.fixed_angle;
      break;

    case TibiDaboLaser3dDriver::OPENED:
      this->new_velocity=new_cfg.scan_velocity;
      this->new_min_angle=new_cfg.min_angle;
      this->new_max_angle=new_cfg.max_angle;
      this->scan_enabled=new_cfg.scan;
      this->fixed_angle=new_cfg.fixed_angle;
      break;

    case TibiDaboLaser3dDriver::RUNNING:
      break;
  }

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

  this->unlock();
}

void TibiDaboLaser3dDriver::start_scan(void)
{
  this->current_velocity=this->new_velocity;
  this->current_min_angle=this->new_min_angle;
  this->current_max_angle=this->new_max_angle;
  try{
    if(this->dir==left_right_scan)
      this->servo->move_absolute_angle(this->current_max_angle,this->current_velocity);
    else
      this->servo->move_absolute_angle(this->current_min_angle,this->current_velocity);
  }catch(CException &e){
  }
}

bool TibiDaboLaser3dDriver::is_scan_done(void)
{
  static ros::Time start_time = ros::Time::now();
  ros::Time current_time=ros::Time::now();
  ros::Duration scan_duration;
  double current_angle;

  try{
    scan_duration=ros::Duration((this->current_max_angle-this->current_min_angle)/this->current_velocity);
    if(this->dir==left_right_scan)
    {
      current_angle=this->servo->get_current_angle();
      if(fabs((this->current_max_angle-current_angle)<1.0) || ((current_time-start_time)>scan_duration))
      {
        this->dir=right_left_scan;
        start_time=ros::Time::now();
        return true;
      }
      else
        return false;
    }
    else
    {
      current_angle=this->servo->get_current_angle();
      if((fabs(this->current_min_angle-current_angle)<1.0) || ((current_time-start_time)>scan_duration))
      {
        this->dir=left_right_scan;
        start_time=ros::Time::now();
        return true;
      }
      else
        return false;
    }
  }catch(CException &e){
    return false;
  }
}

double TibiDaboLaser3dDriver::get_current_angle(void)
{
  double angle=0.0;

  try{
    angle=this->servo->get_current_angle();
  }catch(CException &e){
  
  }

  return angle;
}

double TibiDaboLaser3dDriver::get_current_velocity(void)
{
  double speed=0.0;
  
  try{
    speed=this->servo->get_current_speed();
  }catch(CException &e){
  }

  return speed;
}

bool TibiDaboLaser3dDriver::is_scan_enabled(void)
{
  return this->scan_enabled;
}

double TibiDaboLaser3dDriver::get_fixed_angle(void)
{
 return this->fixed_angle;
}

void TibiDaboLaser3dDriver::move_absolute_angle(double angle)
{
  try{
    this->servo->move_absolute_angle(angle,100);
  }catch(CException &e){
  }
}

TibiDaboLaser3dDriver::~TibiDaboLaser3dDriver(void)
{
  if(this->servo!=NULL)
  {
    delete this->servo;
    this->servo=NULL;
  }
}