toranger.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 
00023 //
00024 // Desc: Base class for ->ranger interface converter drivers.
00025 // Author: Geoffrey Biggs
00026 // Date: 06/05/2007
00027 //
00029 
00030 #include "toranger.h"
00031 
00033 //      Driver management
00035 
00036 // Constructor
00037 // Nothing to do
00038 ToRanger::ToRanger (ConfigFile* cf, int section)
00039         : Driver (cf, section, false, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_RANGER_CODE)
00040 {
00041         inputDevice = NULL;
00042 }
00043 
00044 // Destructor
00045 ToRanger::~ToRanger (void)
00046 {
00047         if (deviceGeom.sensor_poses != NULL)
00048                 delete deviceGeom.sensor_poses;
00049         if (deviceGeom.sensor_sizes != NULL)
00050                 delete deviceGeom.sensor_sizes;
00051 }
00052 
00053 // Setup function
00054 // Cleans up the output data for use
00055 int ToRanger::Setup (void)
00056 {
00057         // Clean output
00058         memset (&deviceGeom, 0, sizeof (deviceGeom));
00059 
00060         return 0;
00061 }
00062 
00063 // Shutdown function
00064 // Ensures all the ranger data memory is freed
00065 int ToRanger::Shutdown (void)
00066 {
00067         if (deviceGeom.sensor_poses != NULL)
00068         {
00069                 delete deviceGeom.sensor_poses;
00070                 deviceGeom.sensor_poses = NULL;
00071         }
00072         if (deviceGeom.sensor_sizes != NULL)
00073         {
00074                 delete deviceGeom.sensor_sizes;
00075                 deviceGeom.sensor_sizes = NULL;
00076         }
00077 
00078         return 0;
00079 }
00080 
00082 //      Message handling
00084 
00085 // Message processing
00086 int ToRanger::ProcessMessage (QueuePointer &respQueue, player_msghdr *hdr, void *data)
00087 {
00088         // Check for capabilities requests first
00089         HANDLE_CAPABILITY_REQUEST (device_addr, respQueue, hdr, data, PLAYER_MSGTYPE_REQ, PLAYER_CAPABILTIES_REQ);
00090 
00091         // Pass property get/set messages through to the input device
00092         if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ, PLAYER_GET_INTPROP_REQ, device_addr) ||
00093                         Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ, PLAYER_SET_INTPROP_REQ, device_addr) ||
00094                         Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ, PLAYER_GET_DBLPROP_REQ, device_addr) ||
00095                         Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ, PLAYER_SET_DBLPROP_REQ, device_addr) ||
00096                         Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ, PLAYER_GET_STRPROP_REQ, device_addr) ||
00097                         Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ, PLAYER_SET_STRPROP_REQ, device_addr))
00098         {
00099                 inputDevice->PutMsg (InQueue, hdr, data);
00100                 ret_queue = respQueue;
00101                 return 0;
00102         }
00103         // Pass responses to them back to the client
00104         else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_RESP_ACK, PLAYER_GET_INTPROP_REQ, inputDeviceAddr) ||
00105                         Message::MatchMessage (hdr, PLAYER_MSGTYPE_RESP_ACK, PLAYER_SET_INTPROP_REQ, inputDeviceAddr) ||
00106                         Message::MatchMessage (hdr, PLAYER_MSGTYPE_RESP_ACK, PLAYER_GET_DBLPROP_REQ, inputDeviceAddr) ||
00107                         Message::MatchMessage (hdr, PLAYER_MSGTYPE_RESP_ACK, PLAYER_SET_DBLPROP_REQ, inputDeviceAddr) ||
00108                         Message::MatchMessage (hdr, PLAYER_MSGTYPE_RESP_ACK, PLAYER_GET_STRPROP_REQ, inputDeviceAddr) ||
00109                         Message::MatchMessage (hdr, PLAYER_MSGTYPE_RESP_ACK, PLAYER_SET_STRPROP_REQ, inputDeviceAddr) ||
00110                         Message::MatchMessage (hdr, PLAYER_MSGTYPE_RESP_NACK, PLAYER_GET_INTPROP_REQ, inputDeviceAddr) ||
00111                         Message::MatchMessage (hdr, PLAYER_MSGTYPE_RESP_NACK, PLAYER_SET_INTPROP_REQ, inputDeviceAddr) ||
00112                         Message::MatchMessage (hdr, PLAYER_MSGTYPE_RESP_NACK, PLAYER_GET_DBLPROP_REQ, inputDeviceAddr) ||
00113                         Message::MatchMessage (hdr, PLAYER_MSGTYPE_RESP_NACK, PLAYER_SET_DBLPROP_REQ, inputDeviceAddr) ||
00114                         Message::MatchMessage (hdr, PLAYER_MSGTYPE_RESP_NACK, PLAYER_GET_STRPROP_REQ, inputDeviceAddr) ||
00115                         Message::MatchMessage (hdr, PLAYER_MSGTYPE_RESP_NACK, PLAYER_SET_STRPROP_REQ, inputDeviceAddr))
00116         {
00117                 hdr->addr = device_addr;
00118                 Publish (ret_queue, hdr, data);
00119                 return 0;
00120         }
00121 
00122         return -1;
00123 }
Last updated 12 September 2005 21:38:45