p_ranger.cc
Go to the documentation of this file.
00001 /*
00002  *  Player - One Hell of a Robot Server
00003  *  Copyright (C) 2004, 2005 Richard Vaughan
00004  *
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  * Desc: Ranger interface for Stage's player driver
00024  * Author: Richard Vaughan
00025  * Date: 25 November 2010
00026  * CVS: $Id$
00027  */
00028 
00029 // DOCUMENTATION ------------------------------------------------------------
00030 
00039 // CODE ----------------------------------------------------------------------
00040 
00041 #include "p_driver.h"
00042 using namespace Stg;
00043 
00044 InterfaceRanger::InterfaceRanger( player_devaddr_t addr,
00045                                   StgDriver* driver,
00046                                   ConfigFile* cf,
00047                                   int section )
00048   : InterfaceModel( addr, driver, cf, section, "ranger" )
00049 {
00050   this->scan_id = 0;
00051 }
00052 
00053 void InterfaceRanger::Publish( void )
00054 {
00055   ModelRanger* rgr = dynamic_cast<ModelRanger*>(this->mod);
00056         
00057   // the Player interface dictates that if multiple sensor poses are
00058   // given, then we have exactly one range reading per sensor. To give
00059   // multiple ranges from the same origin, only one sensor is allowed.
00060   
00061   // need to use the mutable version since we access the range data via a regular pointer
00062   std::vector<ModelRanger::Sensor>& sensors = rgr->GetSensorsMutable();
00063         
00064   player_ranger_data_range_t prange;
00065   memset( &prange, 0, sizeof(prange) );  
00066         
00067   player_ranger_data_intns_t pintens;
00068   memset( &pintens, 0, sizeof(pintens) );       
00069   
00070   std::vector<double> rv, iv;
00071   
00072   if( sensors.size() == 1 ) // a laser scanner type, with one beam origin and many ranges                                       
00073     {
00074       prange.ranges_count = sensors[0].ranges.size();
00075       prange.ranges = prange.ranges_count ? &sensors[0].ranges[0] : NULL;
00076 
00077       pintens.intensities_count = sensors[0].intensities.size();      
00078       pintens.intensities = pintens.intensities_count ? &sensors[0].intensities[0] : NULL;
00079     }
00080   else
00081     {  // a sonar/IR type with one range per beam origin        
00082       FOR_EACH( it, sensors )
00083         {
00084           if( it->ranges.size() ) 
00085             rv.push_back( it->ranges[0] );
00086                                         
00087           if( it->intensities.size() ) 
00088             iv.push_back( it->intensities[0] );
00089         }
00090                         
00091       prange.ranges_count = rv.size();
00092       prange.ranges = rv.size() ? &rv[0] : NULL;        
00093                         
00094       pintens.intensities_count = iv.size();
00095       pintens.intensities = iv.size() ? &iv[0] : NULL;
00096     }
00097         
00098   if( prange.ranges_count )
00099     this->driver->Publish(this->addr,
00100                           PLAYER_MSGTYPE_DATA,
00101                           PLAYER_RANGER_DATA_RANGE,
00102                           (void*)&prange, sizeof(prange), NULL);                        
00103                 
00104   if( pintens.intensities_count )
00105     this->driver->Publish(this->addr,
00106                           PLAYER_MSGTYPE_DATA,
00107                           PLAYER_RANGER_DATA_INTNS,
00108                           (void*)&pintens, sizeof(pintens), NULL);              
00109 }
00110 
00111 
00112 int InterfaceRanger::ProcessMessage(QueuePointer & resp_queue,
00113                                     player_msghdr_t* hdr,
00114                                     void* data)
00115 {
00116   ModelRanger* mod = (ModelRanger*)this->mod;
00117         
00118   // Is it a request to get the ranger's config?
00119   if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
00120                             PLAYER_RANGER_REQ_GET_CONFIG,
00121                             this->addr))
00122     {
00123       if( hdr->size == 0 )
00124         {                        
00125           // the Player ranger config is a little weaker than Stage's
00126           // natice device, so all we can do is warn about this.
00127           PRINT_WARN( "stageplugin ranger config describes only the first sensor of the ranger." );
00128                                         
00129           player_ranger_config_t prc;
00130           bzero(&prc,sizeof(prc));
00131                                         
00132           const ModelRanger::Sensor& s = mod->GetSensors()[0];
00133                                         
00134           prc.min_angle = -s.fov/2.0;
00135           prc.max_angle = +s.fov/2.0;
00136           prc.angular_res = s.fov / (double)s.sample_count;
00137           prc.max_range = s.range.max;
00138           prc.min_range = s.range.min;
00139           prc.range_res = 1.0 / mod->GetWorld()->Resolution();
00140           prc.frequency = 1.0E6 / mod->GetInterval();
00141                                         
00142           this->driver->Publish(this->addr, resp_queue,
00143                                 PLAYER_MSGTYPE_RESP_ACK,
00144                                 PLAYER_RANGER_REQ_GET_CONFIG,
00145                                 (void*)&prc, sizeof(prc), NULL);
00146           return(0);
00147         }
00148       else
00149         {
00150           PRINT_ERR2("config request len is invalid (%d != %d)", (int)hdr->size,0);
00151           return(-1);
00152         }
00153     }
00154   else // Is it a request to get the ranger's geom?
00155     if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
00156                               PLAYER_RANGER_REQ_GET_GEOM,
00157                               this->addr))
00158       {
00159         if(hdr->size == 0)
00160           {
00161             Geom geom = mod->GetGeom();
00162             Pose pose = mod->GetPose();
00163                                                 
00164             const std::vector<ModelRanger::Sensor>& sensors = mod->GetSensors();
00165                          
00166             // fill in the geometry data formatted player-like
00167             player_ranger_geom_t pgeom;
00168             bzero( &pgeom, sizeof(pgeom));
00169             pgeom.pose.px = pose.x;
00170             pgeom.pose.py = pose.y;
00171             pgeom.pose.pyaw = pose.a;
00172             pgeom.size.sl = geom.size.x;
00173             pgeom.size.sw = geom.size.y;
00174                                                  
00175             pgeom.element_poses_count = pgeom.element_sizes_count = sensors.size();
00176 
00177             player_pose3d_t poses[ sensors.size() ];
00178             player_bbox3d_t sizes[ sensors.size() ];
00179                          
00180             for( size_t s=0; s<pgeom.element_poses_count; s++ )
00181               {
00182                 poses[s].px = sensors[s].pose.x;
00183                 poses[s].py = sensors[s].pose.y;
00184                 poses[s].pz = sensors[s].pose.z;
00185                 poses[s].proll = 0.0;
00186                 poses[s].ppitch = 0.0;
00187                 poses[s].pyaw = sensors[s].pose.a;
00188 
00189                 sizes[s].sw = sensors[s].size.x;
00190                 sizes[s].sl = sensors[s].size.y;
00191                 sizes[s].sh = sensors[s].size.z;
00192               }
00193 
00194             pgeom.element_poses = poses;
00195             pgeom.element_sizes = sizes;
00196 
00197             this->driver->Publish(this->addr, resp_queue,
00198                                   PLAYER_MSGTYPE_RESP_ACK,
00199                                   PLAYER_RANGER_REQ_GET_GEOM,
00200                                   (void*)&pgeom, sizeof(pgeom), NULL);
00201             return(0);
00202           }
00203         else
00204           {
00205             PRINT_ERR2("config request len is invalid (%d != %d)", (int)hdr->size,0);
00206             return(-1);
00207           }
00208       }
00209   
00210   // Don't know how to handle this message.
00211   PRINT_WARN2( "stage ranger doesn't support message %d:%d.",
00212                hdr->type, hdr->subtype);
00213   return(-1);
00214 }
00215 
00216 


stage
Author(s): Richard Vaughan , Brian Gerkey , Reed Hedges , Andrew Howard , Toby Collett , Pooya Karimian , Jeremy Asher , Alex Couture-Beil , Geoff Biggs , Rich Mattes , Abbas Sadat
autogenerated on Thu Aug 27 2015 15:20:57