/home/fwph/code/wurde/modules/sickLaser/sickLaser.cpp

Go to the documentation of this file.

#include <CommsManager.H>
#include <RangeFinder.H>
#include <Logger.H>
#include <sys/select.h>
#include <sys/time.h>
#include <time.h>
#include <unistd.h>
#include <string.h>
#include <math.h>
#include <fstream>
#include <iostream>

#include "sick.h"
#include "serial.h"

sick_laser_t laser;

using namespace WURDE;
using namespace std;

string parseLaserConfig(std::string config,std::string laserName);
bool initializeLaser(RangeFinder &myLaser,string port);

int main (int argc, char *argv[]){

       CommsManager myManager("TomPLS");

       string port,confFile;
       bool log=false,invert=false;
       ofstream outfile;
       stringstream laserdata;
       Time timer;

       laserdata.fill('0');

       myManager.setRealName("sickServer");
       myManager.setHelpString("\t-c [config] Specifies alternate config file to use. Default is\n\t\tlaser-config.xml.\n\n\
Note that the -n flag selects which config entity will be used from the laser-config file.\n\nThis module provides access to SICK PLS and LMS lasers.\n");
       myManager.parseOptions(argc,argv,"c:");
       Logger myLogger(myManager.getName());
       RangeFinder myLaser(myManager.getName());

       myManager.registerSupplier(&myLaser);
       myManager.setSleep(0.05);

       if(g_globalConfiguration.haveOption("c")){
              confFile=g_globalConfiguration.getOption("c");
              if(confFile[0]!='/'){
                     confFile=g_globalConfiguration.getConfigDirectory()+"/"+confFile;
              }
       }else{
              confFile=g_globalConfiguration.getConfigDirectory()+"/"+"laser-config.xml";
       }

       g_logdebug << "Reading configuration for " << myManager.getName() << " from " << confFile<< endl;
       port=parseLaserConfig(confFile,myManager.getName());

       if(g_globalConfiguration.getOption("logLaser")=="true"){
              log=true;
              outfile.open((g_globalConfiguration.getDataDirectory()+"/"+myManager.getName()+"LaserLog.txt").c_str());
       }

       if(g_globalConfiguration.getOption("invertLaser")=="true"){
              invert=true;
       }

       g_debug("sickLaser initializing.");


       if(myManager.getState()!=STATE_INFO){
              if(!initializeLaser(myLaser,port)){
                     myManager.cleanUp();
                     g_fatal("Failed to initialize laser! Exiting.");
                     exit(-1);
              }
       }
       
       while(myManager.runUpdate()==STATE_RUN){
              
              fd_set rfds;
              
              FD_ZERO(&rfds);
              FD_SET(laser.dev.fd, &rfds);
              
              while (!select(laser.dev.fd + 1, &rfds, NULL, NULL, NULL)) {
                     FD_SET(laser.dev.fd, &rfds);
              }
              
              sick_handle_laser(&laser);
              if (laser.new_reading) {
                     myLaser.data.ranges.clear();
                
                     if(invert){
                            for(int i=laser.numvalues-1;i>=0;--i){
                                   myLaser.data.ranges.push_back(laser.raw[i]);
                            }
                     }else{
                            for(int i=0;i<laser.numvalues;++i){
                                   myLaser.data.ranges.push_back(laser.raw[i]);
                            }
                     }
                     myLaser.publishData();

                     if(log){
                            timer.now();
                            laserdata << timer.getSeconds() << ".";
                            laserdata.width(6);
                            laserdata << timer.getUSeconds() << " ";
                            if(invert){
                                   for(int i=laser.numvalues-1;i>=0;--i){
                                          laserdata << " " << laser.raw[i];
                                   }
                            }else{
                                   for(int i=0;i<laser.numvalues;++i){
                                          laserdata << " " << laser.raw[i];
                                   }
                            }
                            
                            laserdata << endl;
                     }
              }
       
        
              //    myLaser.publishInfo();
       }
       
       if(myManager.getState()!=STATE_INFO){
              sick_stop_laser(&laser);
       }

       g_info("sickLaser shutting down cleanly.");
       outfile << laserdata.str();
       outfile.close();
       myManager.cleanUp();

       return 0;
}

bool initializeLaser(RangeFinder &myLaser,string port){

  // FIXME: configuration variables
  strncpy(laser.settings.device_name, port.c_str(), MAX_NAME_LENGTH);

  if(g_globalConfiguration.getOption("laserType")=="PLS"){
         laser.settings.type=PLS;//Ok
         strncpy((char *)laser.settings.password, PLS_PASSWORD, 8);//?
         laser.settings.parity=E;//Ok
  }else if(g_globalConfiguration.getOption("laserType")=="LMS"){
         laser.settings.type=LMS;
         strncpy((char *)laser.settings.password, LMS_PASSWORD, 8);//?
         laser.settings.parity=N;//Ok
  }else{
         g_logwarn << "Don't recognize laser type " << g_globalConfiguration.getOption("laserType") << ", assuming PLS." << endl;
         laser.settings.type=PLS;
         laser.settings.parity=E;//Ok
         strncpy((char *)laser.settings.password, PLS_PASSWORD, 8);//?
  }
  
  laser.settings.range_res=CM; //?
  laser.settings.range_dist=RANGE80M;//?
  laser.settings.laser_num=0; //?
  laser.settings.detect_baudrate=1;//Ok
  laser.settings.use_highspeed=0;//?
  laser.settings.start_baudrate=9600;//Ok
  laser.settings.set_baudrate=38400;//Ok
  laser.settings.databits=8;//Ok
  laser.settings.stopbits=1;//Ok
  laser.settings.swf=0;
  laser.settings.hwf=0;
  laser.settings.angle_range=180;//Moot
  laser.settings.angle_resolution=RES_1_00_DEGREE;
  laser.settings.num_values=180;
  
  laser.raw=0;  

  laser.raw = new float[180];
  //  test_alloc(laser.raw);
  if (sick_start_laser(&laser) < 0) {
    //destroy_interface(this);
    return false;
  }

  /* populate framework info structures */
  myLaser.info.angle_min.setValueAndLock(-M_PI/2.0);

  myLaser.info.max_range.setValue(laser.settings.range_dist);
  myLaser.info.max_range.setLock();
  
  myLaser.info.uniform.setValue(true);
  myLaser.info.uniform.setLock();
  
  myLaser.info.theta_offset.setValue(-M_PI/2.0);
  myLaser.info.theta_offset.setLock();
  
  myLaser.info.x_offset.setValue(0);
  myLaser.info.x_offset.setLock();
  
  myLaser.info.y_offset.setValue(0);
  myLaser.info.y_offset.setLock();
  
  myLaser.info.half_angle.setValue(0); // laser is a ray
  myLaser.info.half_angle.setLock();
  
  myLaser.info.theta_separation.setValue(M_PI/181.0);
  myLaser.info.theta_separation.setLock();
  
  //pretend the following are true, for now
  myLaser.info.z_offset.setValue(0);
  myLaser.info.z_offset.setLock();
  
  myLaser.info.phi_offset.setValue(0);
  myLaser.info.phi_offset.setLock();
  
  myLaser.info.psi_offset.setValue(0);
  myLaser.info.psi_offset.setLock();
  
  myLaser.info.x_separation.setValue(0);
  myLaser.info.x_separation.setLock();
  
  myLaser.info.y_separation.setValue(0);
  myLaser.info.y_separation.setLock();
  
  myLaser.info.z_separation.setValue(0);
  myLaser.info.z_separation.setLock();
  
  myLaser.info.phi_separation.setValue(0);
  myLaser.info.phi_separation.setLock();
  
  myLaser.info.psi_separation.setValue(0);
  myLaser.info.psi_separation.setLock();
  
  myLaser.publishInfo();

  return true;
}

---