hokuyo_laser3d_driver.cpp

Go to the documentation of this file.

#include "hokuyo_laser3d_driver.h"
#include <sstream>
#include <stdlib.h>

HokuyoLaser3dDriver::HokuyoLaser3dDriver()
{
  //setDriverId(driver string id);
  std::string h3d_id(ros::this_node::getName());
  replace(h3d_id.begin(), h3d_id.end(), '/', '_');
  ROS_INFO("Laser ID: %s", h3d_id.c_str());
  this->id = h3d_id;

  try
  {
    this->h3d = new CH3D();
    setDriverId(h3d_id);
  }
  catch (CException & e)
  {
    ROS_FATAL("'%s'",e.what().c_str());
  }
}

bool HokuyoLaser3dDriver::openDriver(void)
{
  //setDriverId(driver string id);
  try{
    this->h3d->setLaserPort(this->config_.laser_port);
    this->h3d->setVerbose(true);
    this->h3d->init();
  }catch (CException & e)
  {
    ROS_FATAL("'%s'",e.what().c_str());
  }
  return true;
}

bool HokuyoLaser3dDriver::closeDriver(void)
{
  try{
    this->h3d->close();
  }catch (CException & e)
  {
    ROS_FATAL("'%s'",e.what().c_str());
  }
  return true;
}

bool HokuyoLaser3dDriver::startDriver(void)
{
  try{
    this->h3d->setLaserMode(this->config_.laser_mode);
    this->h3d->setResolution(this->config_.resolution);
    if(this->config_.continuous)
      this->h3d->startAcquisition();
  }catch (CException & e)
  {
    ROS_FATAL("'%s'",e.what().c_str());
  }
  return true;
}

bool HokuyoLaser3dDriver::stopDriver(void)
{
  try{
    if(this->config_.continuous)
      this->h3d->stopAcquisition();
  }catch (CException & e)
  {
    ROS_FATAL("'%s'",e.what().c_str());
  }
  return true;
}

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

  //update driver with new_cfg data

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

  this->unlock();
}

HokuyoLaser3dDriver::~HokuyoLaser3dDriver()
{
  delete this->h3d;
}

// -----------------------------------------------------------------------------


std::list<std::string> HokuyoLaser3dDriver::getENDList()
{
  return this->h3d->END_list;
}

void HokuyoLaser3dDriver::getSlice(PointCloudTimeStamped &slc)
{
  return this->h3d->getSlice(slc);
}

void HokuyoLaser3dDriver::getCloud(PointCloudTimeStamped &cld)
{
  return this->h3d->getCloud(cld);
}