leuze_laser_driver.cpp

Go to the documentation of this file.

#include "leuze_laser_driver.h"
#include "leuze_laser_exceptions.h"

LeuzeLaserDriver::LeuzeLaserDriver() :
  leuze_("leuze")
{
  setDriverId("leuze");
}

bool LeuzeLaserDriver::openDriver(void)
{
  try
  {
    this->lock();
    std::string port(config_.serial_port);
    this->unlock();  

    leuze_.open(port);
  }
  catch (CException & e)
  {
    leuze_.close();
    ROS_ERROR("'%s'", e.what().c_str());
    return false;
  }
  return true;
}

bool LeuzeLaserDriver::closeDriver(void)
{
  try
  {
    leuze_.close();
  }
  catch (CException & e)
  {
    ROS_ERROR("'%s'", e.what().c_str());
    return false;
  }
  return true;
}

bool LeuzeLaserDriver::startDriver(void)
{
  try
  {
    leuze_.start();
  }
  catch (CException & e)
  {
    ROS_ERROR("'%s'", e.what().c_str());
    return false;
  }
  return true;
}

bool LeuzeLaserDriver::stopDriver(void)
{
  try
  {
    leuze_.stop();
  }
  catch (CException & e)
  {
    ROS_ERROR("'%s'", e.what().c_str());
    return false;
  }
  return true;
}

void LeuzeLaserDriver::config_update(const Config& new_cfg, uint32_t level)
{
  this->lock();
  
  // depending on current state
  // update driver with new_cfg data
  switch(this->getState())
  {
    case LeuzeLaserDriver::CLOSED:
       config_.serial_port = new_cfg.serial_port;
      break;

    case LeuzeLaserDriver::OPENED:
      break;

    case LeuzeLaserDriver::RUNNING:
      break;
  }

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

  this->unlock();
}

std::string LeuzeLaserDriver::getNewScanEventId(void) const
{
  return leuze_.get_new_scan_event_id();
}

std::string LeuzeLaserDriver::getNoLaserEventId(void) const
{
  return leuze_.get_laser_not_present_event_id();
}

sensor_msgs::LaserScan LeuzeLaserDriver::getScan(void)
{
  TLeuzeScan scan;
  leuze_.get_scan(&scan);
  return leuzeScantoLaserScan(scan);
}

iri_common_drivers_msgs::leuze_status LeuzeLaserDriver::getStatus(void)
{
  iri_common_drivers_msgs::leuze_status ros_status;
  TLeuzeStatus status;
  
  leuze_.get_status(&status);
  ros_status.header.stamp    = ros::Time::now();
  ros_status.header.frame_id = config_.frame_id;
  ros_status.alarm1=status.alarm1;
  ros_status.alarm2=status.alarm2;
  ros_status.ossd1=status.ossd1;
  ros_status.ossd2=status.ossd2;
  ros_status.current_pair=status.current_pair;

  return ros_status;
}

sensor_msgs::LaserScan LeuzeLaserDriver::leuzeScantoLaserScan(const TLeuzeScan & leuze_scan)
{
  sensor_msgs::LaserScan scan;
  static ros::Time start=ros::Time::now();
  ros::Time end=ros::Time::now();

  scan.header.stamp    = ros::Time::now();
  scan.header.frame_id = config_.frame_id;

  unsigned int num_ranges = ((leuze_scan.stop-leuze_scan.start)/leuze_scan.res)+1;

  scan.angle_max       = (0.006268668*(leuze_scan.start-1))-(95.04*M_PI/180.0); //start angle of the scan [rad]
  scan.angle_min       = (0.006268668*(leuze_scan.stop-1))-(95.04*M_PI/180.0);//end angle of the scan [rad]
  scan.angle_increment = leuze_scan.res*(-0.006268668);//-(scan.angle_min - scan.angle_max) / (double)(num_ranges-1);
  scan.time_increment  = 0.f;//time between measurements [seconds]
  scan.scan_time       = (end-start).toSec();//time between scans [seconds]
  scan.range_min       = 0.f;//minimum range value [m]
  scan.range_max       = 50.f;//maximum range value [m]

  for(unsigned int ii=0; ii<num_ranges; ii++)
    scan.ranges.push_back((float)leuze_scan.data[ii]/1000.f);

  start=end;

  return scan;
}

bool LeuzeLaserDriver::changePair(unsigned char new_pair)
{
  if(new_pair>LEUZE_FP4)
    return false;
  else
  {
    try{
      leuze_.change_pair((pairs_t)new_pair);
      return true;
    }catch(CLeuzeLaserException &e){
      return false;
    }
  }
}
  
LeuzeLaserDriver::~LeuzeLaserDriver()
{
}