lasertransform.cc

00001 /*
00002  *  Player - One Hell of a Robot Server
00003  *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
00004  *                      gerkey@usc.edu    kaspers@robotics.usc.edu
00005  *
00006  *  This program is free software; you can redistribute it and/or modify
00007  *  it under the terms of the GNU General Public License as published by
00008  *  the Free Software Foundation; either version 2 of the License, or
00009  *  (at your option) any later version.
00010  *
00011  *  This program is distributed in the hope that it will be useful,
00012  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014  *  GNU General Public License for more details.
00015  *
00016  *  You should have received a copy of the GNU General Public License
00017  *  along with this program; if not, write to the Free Software
00018  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019  *
00020  */
00021 
00022 
00023 #include "lasertransform.h"
00024 
00026 // Constructor
00027 LaserTransform::LaserTransform( ConfigFile* cf, int section)
00028   : Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_LASER_CODE)
00029 {
00030   // Must have an input laser
00031   if (cf->ReadDeviceAddr(&this->laser_addr, section, "requires",
00032                          PLAYER_LASER_CODE, -1, NULL) != 0)
00033   {
00034     this->SetError(-1);    
00035     return;
00036   }
00037   this->laser_device = NULL;
00038   this->laser_timestamp.tv_sec = 0;
00039   this->laser_timestamp.tv_usec = 0;
00040 
00041   // Outgoing data
00042   this->time.tv_sec = 0;
00043   this->time.tv_usec = 0;
00044   memset(&this->data, 0, sizeof(this->data));
00045 
00046   return;
00047 }
00048 
00049 
00051 // Set up the device (called by server thread).
00052 int LaserTransform::Setup()
00053 {
00054   // Subscribe to the laser.
00055   if(Device::MatchDeviceAddress(this->laser_addr, this->device_addr))
00056   {
00057     PLAYER_ERROR("attempt to subscribe to self");
00058     return(-1);
00059   }
00060   if(!(this->laser_device = deviceTable->GetDevice(this->laser_addr)))
00061   {
00062     PLAYER_ERROR("unable to locate suitable laser device");
00063     return(-1);
00064   }
00065   if(this->laser_device->Subscribe(this->InQueue) != 0)
00066   {
00067     PLAYER_ERROR("unable to subscribe to laser device");
00068     return(-1);
00069   }
00070   return 0;
00071 }
00072 
00073 
00075 // Shutdown the device (called by server thread).
00076 int LaserTransform::Shutdown()
00077 {
00078   // Unsubscribe from devices.
00079   this->laser_device->Unsubscribe(this->InQueue);
00080   
00081   return 0;
00082 }
00083 
00085 // Process an incoming message
00086 int LaserTransform::ProcessMessage(QueuePointer & resp_queue, 
00087                                 player_msghdr * hdr, 
00088                                 void * data)
00089 {
00090   // Handle new data from the laser
00091   if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA, PLAYER_LASER_DATA_SCAN, 
00092                            this->laser_addr))
00093   {
00094     assert(hdr->size != 0);
00095     player_laser_data_t * l_data = reinterpret_cast<player_laser_data_t * > (data);
00096     this->UpdateLaser(l_data);
00097     return(0);
00098   }
00099   // Forward any request to the laser
00100   else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, -1, this->device_addr))
00101   {
00102     // Forward the message
00103     laser_device->PutMsg(this->InQueue, hdr, data);
00104     // Store the return address for later use
00105     this->ret_queue = resp_queue;
00106     // Set the message filter to look for the response
00107     this->InQueue->SetFilter(this->laser_addr.host,
00108                              this->laser_addr.robot,
00109                              this->laser_addr.interf,
00110                              this->laser_addr.index,
00111                              -1,
00112                              hdr->subtype);
00113     // No response now; it will come later after we hear back from the
00114     // laser
00115     return(0);
00116   }
00117   // Forward response (success or failure) from the laser
00118   else if((Message::MatchMessage(hdr, PLAYER_MSGTYPE_RESP_ACK, 
00119                             -1, this->laser_addr)) ||
00120      (Message::MatchMessage(hdr, PLAYER_MSGTYPE_RESP_NACK,
00121                             -1, this->laser_addr)))
00122   {
00123     // Copy in our address and forward the response
00124     hdr->addr = this->device_addr;
00125     this->Publish(this->ret_queue, hdr, data);
00126     // Clear the filter
00127     this->InQueue->ClearFilter();
00128 
00129     return(0);
00130   }
00131 
00132   return(-1);
00133 }
Last updated 12 September 2005 21:38:45