Urgd.cpp

From HotDec

Pseudo-Code Description

sighandler() {

  Handle SIGINT signal
  Close connection to hokuyo laser rangefinder
  Close and Unlink Posix.4 Priority Message Queue

}

initURG() {

  Initialize modem parameters for the hokuyo laser rangefinder

}

recvFunction() {

  Call hokuyo.get_data()
  Call hokuyo.rotate_data()
  Call hokuyo.get_track_points()
     All from urg.cpp

  Copy distance data from rangefinder into laser packet
  Send laser data packet into LASER_DATA_QUEUE

}

main() {

  Set up Posix.4 Priority Message Queue
  Create 100ms timer
  Call initURG()
  
  while(1) {
     sigwait -- Block on SIGALRM signal {
        recvFunction()
     }
  }

}