hokuyo_laser_driver.cpp

Go to the documentation of this file.

#include "hokuyo_laser_driver.h"
#include <cmath>
#include "ctime.h"

HokuyoLaserDriver::HokuyoLaserDriver()
{
  //setDriverId(driver string id);

  this->laser = NULL;
  std::string laser_id(ros::this_node::getName());
  replace(laser_id.begin(), laser_id.end(), '/', '_');
  ROS_INFO("Laser ID: %s", laser_id.c_str());

  try
  {
    this->laser = new CHokuyo(laser_id);
    setDriverId(this->laser->get_id());
  }
  catch (CException & e)
  {
    ROS_FATAL("'%s'",e.what().c_str());
  }

  this->number_of_scans = 0;

  this->config_.is_multiecho = false;

}

bool HokuyoLaserDriver::openDriver(void)
{
  //setDriverId(driver string id);
  ROS_DEBUG("openDriver");
  try
  {
    ROS_DEBUG("eth:%d ip:%s usb:%s",this->use_eth,this->ip.c_str(),this->port.c_str());
    if(this->config_.use_ethernet)
      this->laser->connect(false,this->config_.ip_address);
    else
      this->laser->connect(true,this->config_.usb_port);

    // get specs 
    this->laser->get_specifications(this->specs);
    // treatment
    this->range_min = this->specs.min_dist/1000; // [m]
    this->range_max = this->specs.max_dist/1000; // [m]
    this->angle_increment = 2*M_PI / this->specs.num_steps; // [rad]
    this->angle_min = (this->specs.first_step - this->specs.center_step)  * this->angle_increment; // [rad]
    this->angle_max = (this->specs.last_step  - this->specs.center_step)  * this->angle_increment; // [rad]
    this->scan_time = 0.025;
    this->time_increment = 0.0001;                                            //TODO especificar increment
    // config scan


    evnts.push_back(this->laser->get_new_scan_event());
    evnts.push_back(this->laser->get_end_event());
    evnts.push_back(this->laser->get_error_check_command_event());

  }catch( CHokuyoException &e )
  {
    ROS_FATAL("'%s'",e.what().c_str());
  }

  return true;
}

bool HokuyoLaserDriver::closeDriver(void)
{
  ROS_DEBUG("closeDriver");
  try
  {
    this->laser->disconnect();
  }
  catch (CException & e)
  {
    ROS_ERROR("'%s'", e.what().c_str());
    return false;
  }
  return true;
}

bool HokuyoLaserDriver::startDriver(void)
{
  ROS_DEBUG("startDriver");
  try{
    THokuyo_scan_config sc;
    sc = this->translateConfig(this->config_);
    ROS_INFO("Scan Configuration\n\tmin: %f\n\tmax: %f\n\tgroup: %d\n\tinterval: %d\n\tshort? %d\n\tintensity? %d\n\tmultiecho? %d",
             config_.angle_min, config_.angle_max, config_.cluster, config_.interval,
             config_.is_short, config_.is_intensity, config_.is_multiecho);
    this->laser->start(sc);
    ROS_INFO("Acquisition Started");
  }
  catch (CException & e)
  {
    ROS_ERROR("'%s'", e.what().c_str());
    return false;
  }


  return true;
}

bool HokuyoLaserDriver::stopDriver(void)
{
  ROS_DEBUG("stopDriver");
  try{
    this->laser->stop();
    ROS_INFO("Acquisition finished");
  }
  catch (CException & e)
  {
    ROS_ERROR("'%s'", e.what().c_str());
    return false;
  }
  return true;
}

void HokuyoLaserDriver::config_update(const Config& new_cfg, uint32_t level)
{
  ROS_DEBUG("config_update");
  this->lock();

  //update driver with new_cfg data

  switch(this->getState()){
    case HokuyoLaserDriver::OPENED:
      break;
    case HokuyoLaserDriver::RUNNING:
      this->frame_id = new_cfg.frame_id;
      break;
    case HokuyoLaserDriver::CLOSED:
      this->ip = new_cfg.ip_address;
      this->port = new_cfg.usb_port;
      this->use_eth = new_cfg.use_ethernet;
      break;
  }

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

  this->unlock();
}

HokuyoLaserDriver::~HokuyoLaserDriver()
{
  delete this->laser;
}

//                           DRIVER FUNCTIONS


void HokuyoLaserDriver::getScan(THokuyo_scan & scan)
{
  ROS_DEBUG("getting scan");
  this->laser->get_scan(scan);
  ROS_DEBUG("scan received");
}

THokuyo_scan_config HokuyoLaserDriver::translateConfig(Config & cfg)
{
  THokuyo_scan_config sc;

  sc.num = 0;

  // -- Angles --
  sc.start_step = this->specs.center_step + (int)(cfg.angle_min/this->angle_increment);
  // limits check
  if(sc.start_step < this->specs.first_step)
    sc.start_step = this->specs.first_step;
  else if(sc.start_step > this->specs.center_step)
    sc.end_step = this->specs.center_step;
  // update config
  cfg.angle_min = (sc.start_step - this->specs.center_step) * this->angle_increment; // [rad]

  sc.end_step   = (int)( cfg.angle_max/this->angle_increment + this->specs.center_step);
  // limits check
  if(sc.end_step > this->specs.last_step)
    sc.end_step = this->specs.last_step;
  else if(sc.end_step < this->specs.center_step)
    sc.end_step = this->specs.center_step;
  // update config
  cfg.angle_max = (sc.end_step - this->specs.center_step) * this->angle_increment; // [rad]

  // -- Scan --
  sc.grouping = cfg.cluster;
  sc.interval = cfg.interval;
  sc.is_short = cfg.is_short;
  sc.is_intensity = cfg.is_intensity;
  sc.is_multiecho = cfg.is_multiecho;
  //sc.high_sensivity = cfg.high_sensivity;

  ROS_DEBUG("New Configuration:\n\tmin: %d(%f)\n\tmax: %d(%f)\n\tgroup %d\n\tinterval %d\n\tshort?\n\t%d(%d)\n\tintensity?%d(%d)\n\tmultiecho?%d(%d)",
           sc.start_step,
           cfg.angle_min,
           sc.end_step,
           cfg.angle_max,
           sc.grouping,
           sc.interval,
           sc.is_short,
           cfg.is_short,
           sc.is_intensity,
           cfg.is_intensity,
           sc.is_multiecho,
           cfg.is_multiecho);

  return sc;
}