/* *  Stage : a multi-robot simulator. *  Copyright (C) 2001, 2002 Richard Vaughan, Andrew Howard and Brian Gerkey. * *  This program is free software; you can redistribute it and/or modify *  it under the terms of the GNU General Public License as published by *  the Free Software Foundation; either version 2 of the License, or *  (at your option) any later version. * *  This program is distributed in the hope that it will be useful, *  but WITHOUT ANY WARRANTY; without even the implied warranty of *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the *  GNU General Public License for more details. * *  You should have received a copy of the GNU General Public License *  along with this program; if not, write to the Free Software *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA * *//* * Desc: Simulates a scanning laser range finder (SICK LMS200) * Author: Andrew Howard, Richard Vaughan * Date: 28 Nov 2000 * CVS info: $Id: laserdevice.cc,v 1.6.4.5.2.1 2003/12/05 02:08:28 gerkey Exp $ */#define DEBUG#undef VERBOSE#include <iostream>#include <stage1p3.h>#include <math.h>#include "world.hh"#include "laserdevice.hh"#include "raytrace.hh"#define DEFAULT_RES 0.5#define DEFAULT_MIN -90#define DEFAULT_MAX +90///////////////////////////////////////////////////////////////////////////// Default constructorCLaserDevice::CLaserDevice(LibraryItem* libit, CWorld *world, CEntity *parent )    : CPlayerEntity(libit, world, parent ){  // set the Player IO sizes correctly for this type of Entity  m_data_len    = sizeof( player_laser_data_t );  m_command_len = 0;  m_config_len  = 1;  m_reply_len  = 1;    m_player.code = PLAYER_LASER_CODE; // from player's messages.h    // Default visibility settings  this->laser_return = LaserVisible;  this->sonar_return = 0;  this->obstacle_return = 0;    // Default laser simulation settings  this->scan_rate = 360 / 0.200; // 5Hz  this->min_res =  DTOR(0.25);  this->max_range = 8.0;  // Current laser data configuration  this->scan_res = DTOR(DEFAULT_RES);  this->scan_min = DTOR(DEFAULT_MIN);  this->scan_max = DTOR(DEFAULT_MAX);  this->scan_count = 361;  this->intensity = false;  m_last_update = 0;    // Dimensions of laser  this->shape = ShapeRect;  this->size_x = 0.155;  this->size_y = 0.155;  #ifdef INCLUDE_RTK2  this->scan_fig = NULL;#endif}///////////////////////////////////////////////////////////////////////////// Startup routine//bool CLaserDevice::Startup(){  if (!CPlayerEntity::Startup())    return false;  return true;}///////////////////////////////////////////////////////////////////////////// Load the entity from the world filebool CLaserDevice::Load(CWorldFile *worldfile, int section){  if (!CPlayerEntity::Load(worldfile, section))    return false;  // Read laser settings  this->min_res = worldfile->ReadAngle(section, "min_res", this->min_res);  this->max_range = worldfile->ReadLength(section, "max_range", this->max_range);  this->scan_rate = worldfile->ReadFloat(section, "scan_rate", this->scan_rate);    return true;}///////////////////////////////////////////////////////////////////////////// Update the laser datavoid CLaserDevice::Update( double sim_time ){  CPlayerEntity::Update( sim_time );  ASSERT(m_world != NULL);      // UPDATE OUR RENDERING  double x, y, th;  GetGlobalPose( x,y,th );      // if we've moved  ReMap(x, y, th);    // UPDATE OUR SENSOR DATA  // Check to see if it's time to update the laser scan  double interval = this->scan_count / this->scan_rate;  if( sim_time - m_last_update > interval )  {    m_last_update = sim_time;	    if( Subscribed() > 0 )    {      // Check to see if the configuration has changed      CheckConfig();            // Generate new scan data and copy to data buffer      player_laser_data_t scan_data;      GenerateScanData( &scan_data );      PutData( &scan_data, sizeof( scan_data) );	      // we may need to tell clients about this data      SetDirty( PropData, 1 );    }    else    {      // reset configuration to default.      this->scan_res = DTOR(DEFAULT_RES);      this->scan_min = DTOR(DEFAULT_MIN);      this->scan_max = DTOR(DEFAULT_MAX);      this->scan_count = 361;      this->intensity = false;	      // and indicate that the data is no longer available      //Lock();      //m_info_io->data_avail = 0;      //Unlock();	      // empty the laser beacon list      this->visible_beacons.clear();    }  }}///////////////////////////////////////////////////////////////////////////// Check to see if the configuration has changed// This currently emulates the behaviour of SICK laser range finder.bool CLaserDevice::CheckConfig(){  int len;  void* client;  char buffer[PLAYER_MAX_REQREP_SIZE];  player_laser_config_t config;  player_laser_geom_t geom;  while (true)  {    len = GetConfig(&client, &buffer, sizeof(buffer));    if (len <= 0)      break;    switch (buffer[0])    {      case PLAYER_LASER_SET_CONFIG:        if (len < (int)sizeof(config))        {          PRINT_WARN2("request has unexpected len (%d < %d)", len, sizeof(config));          break;        }                memcpy(&config, buffer, sizeof(config));        config.resolution = ntohs(config.resolution);        config.min_angle = ntohs(config.min_angle);        config.max_angle = ntohs(config.max_angle);        if (config.resolution == 25)        {          if (abs(config.min_angle) > 5000 || abs(config.max_angle) > 5000)          {            PRINT_MSG("warning: invalid laser configuration request");            PutReply(client, PLAYER_MSGTYPE_RESP_NACK);          }          else          {            this->scan_res = DTOR((double) config.resolution / 100.0);            this->scan_min = DTOR((double) config.min_angle / 100.0);            this->scan_max = DTOR((double) config.max_angle / 100.0);            this->scan_count = (int) ((this->scan_max - this->scan_min) / this->scan_res) + 1;            this->intensity = config.intensity;            PutReply(client, PLAYER_MSGTYPE_RESP_ACK);          }        }        else if (config.resolution == 50)        {          if (abs(config.min_angle) > 9000 || abs(config.max_angle) > 9000)          {            PRINT_MSG("warning: invalid laser configuration request");            PutReply(client, PLAYER_MSGTYPE_RESP_NACK);          }          else          {            this->scan_res = DTOR((double) config.resolution / 100.0);            this->scan_min = DTOR((double) config.min_angle / 100.0);            this->scan_max = DTOR((double) config.max_angle / 100.0);            this->scan_count = (int) ((this->scan_max - this->scan_min) / this->scan_res) + 1;            this->intensity = config.intensity;            PutReply(client, PLAYER_MSGTYPE_RESP_ACK);          }        }        else if (config.resolution == 100)        {          if (abs(config.min_angle) > 18000 || abs(config.max_angle) > 18000)          {            PRINT_MSG("warning: invalid laser configuration request");            PutReply(client, PLAYER_MSGTYPE_RESP_NACK);          }          else          {            this->scan_res = DTOR((double) config.resolution / 100.0);            this->scan_min = DTOR((double) config.min_angle / 100.0);            this->scan_max = DTOR((double) config.max_angle / 100.0);            this->scan_count = (int) ((this->scan_max - this->scan_min) / this->scan_res) + 1;            this->intensity = config.intensity;            PutReply(client, PLAYER_MSGTYPE_RESP_ACK);          }        }        else        {          PRINT_MSG("warning: invalid laser configuration request");          PutReply(client, PLAYER_MSGTYPE_RESP_NACK);        }        break;      case PLAYER_LASER_GET_CONFIG:        // Return the laser configuration        config.resolution = htons((int) (RTOD(this->scan_res) * 100));        config.min_angle = htons((unsigned int) (int) (RTOD(this->scan_min) * 100));        config.max_angle = htons((unsigned int) (int) (RTOD(this->scan_max) * 100));        config.intensity = this->intensity;        PutReply(client, PLAYER_MSGTYPE_RESP_ACK, NULL, &config, sizeof(config));        break;      case PLAYER_LASER_GET_GEOM:        // Return the laser geometry        geom.pose[0] = htons((short) (this->origin_x * 1000));        geom.pose[1] = htons((short) (this->origin_y * 1000));        geom.pose[2] = 0;        geom.size[0] = htons((short) (this->size_x * 1000));        geom.size[1] = htons((short) (this->size_y * 1000));        PutReply(client, PLAYER_MSGTYPE_RESP_ACK, NULL, &geom, sizeof(geom));        break;      default:        PRINT_WARN1("invalid laser configuration request [%c]", buffer[0]);        PutReply(client, PLAYER_MSGTYPE_RESP_NACK);        break;    }  }  return true;}///////////////////////////////////////////////////////////////////////////// Generate scan databool CLaserDevice::GenerateScanData( player_laser_data_t *data ){        // Get the pose of the laser in the global cs  //  double x, y, th;  GetGlobalPose(x, y, th);    // See how many scan readings to interpolate.  // To save time generating laser scans, we can  // generate a scan with lower resolution and interpolate  // the intermediate values.  // We will interpolate <skip> out of <skip+1> readings.  int skip = (int) (this->min_res / this->scan_res - 0.5);  // initialise our beacon detecting array  this->visible_beacons.clear();   // Set the header part of the data packet  data->range_count = htons(this->scan_count);  data->resolution = htons((int) (100 * RTOD(this->scan_res)));  data->min_angle = htons((int) (100 * RTOD(this->scan_min)));  data->max_angle = htons((int) (100 * RTOD(this->scan_max)));  data->range_res = htons(1);  // Make sure the data buffer is big enough  ASSERT(this->scan_count <= ARRAYSIZE(data->ranges));    // Do each scan  for (int s = 0; s < this->scan_count;)  {    double ox = x;    double oy = y;    double oth = th;    // Compute parameters of scan line    double bearing = s * this->scan_res + this->scan_min;    double pth = oth + bearing;    CLineIterator lit( ox, oy, pth, this->max_range,                        m_world->ppm, m_world->matrix, PointToBearingRange );	    CEntity* ent;    double range = this->max_range;	    while( (ent = lit.GetNextEntity()) )     {      // Ignore ourself, things which are attached to us,      // and things that we are attached to (except root)      // The latter is useful if you want to stack beacons      // on the laser or the laser on somethine else.      if (ent == this || this->IsDescendent(ent) || 	  (ent != m_world->root && ent->IsDescendent(this)))        continue;      // Construct a list of entities that have a fiducial value      if( ent->fiducial_return != FiducialNone )	{	  this->visible_beacons.push_front( (int)ent );	  	  // remove duplicates	  this->visible_beacons.sort();	  this->visible_beacons.unique();	}      // Stop looking when we see something      if(ent->laser_return != LaserTransparent)       {        range = lit.GetRange();        break;      }	    }	    // Construct the return value for the laser    uint16_t v = (uint16_t) (1000.0 * range);    data->ranges[s] = htons(v);    if (ent && ent->laser_return >= LaserBright)      data->intensity[s] = 1;    else      data->intensity[s] = 0;    s++;    // Skip some values to save time    for (int i = 0; i < skip && s < this->scan_count; i++)    {      data->ranges[s] = data->ranges[s - 1];      data->intensity[s] = data->intensity[s - 1];      s++;    }  }    // remove all duplicates from the beacon list  this->visible_beacons.unique();   return true;}#ifdef INCLUDE_RTK2///////////////////////////////////////////////////////////////////////////// Initialise the rtk guivoid CLaserDevice::RtkStartup(){  CPlayerEntity::RtkStartup();    // Create a figure representing this object  this->scan_fig = rtk_fig_create(m_world->canvas, NULL, 49);  // Set the color  rtk_fig_color_rgb32(this->scan_fig, this->color);}///////////////////////////////////////////////////////////////////////////// Finalise the rtk guivoid CLaserDevice::RtkShutdown(){  // Clean up the figure we created  rtk_fig_destroy(this->scan_fig);  CPlayerEntity::RtkShutdown();} ///////////////////////////////////////////////////////////////////////////// Update the rtk guivoid CLaserDevice::RtkUpdate(){  CPlayerEntity::RtkUpdate();   rtk_fig_clear(this->scan_fig);     // draw a figure from the data in the data buffer  // we might have put it there ourselves, or another stage  // might have generated it    // this is a good way to see the _real_ output of Stage, rather than  // some intermediate values. also it allows us to view data that was  // generated elsewhere, without this Stage being subscribed to a  // device.  so even though it does use a few more flops, i think  // it's the Right Thing to do - RTV.    //gth -= M_PI / 2.0;    // if a client is subscribed to this device  if( Subscribed() > 0 && m_world->ShowDeviceData( this->lib_entry->type_num) )  {    player_laser_data_t data;        // attempt to get the right size chunk of data from the mmapped buffer    if( GetData( &data, sizeof(data) ) == sizeof(data) )    {       // Get global pose      double gx, gy, gth;      GetGlobalPose(gx, gy, gth);      rtk_fig_origin( this->scan_fig, gx, gy, gth);            // we got it, so parse out the data and display it      short min_ang_deg = ntohs(data.min_angle);      short max_ang_deg = ntohs(data.max_angle);      unsigned short samples = ntohs(data.range_count); 	      double min_ang_rad = DTOR( (double)min_ang_deg ) / 100.0;      double max_ang_rad = DTOR( (double)max_ang_deg ) / 100.0;      double incr = (double)(max_ang_rad - min_ang_rad) / (double)samples;	      double lx = 0.0;      double ly = 0.0;      double bearing = min_ang_rad;      for( int i=0; i < (int)samples; i++ )      {        // get range, converting from mm to m        //unsigned short range_mm = ntohs(data.ranges[i]) & 0x1FFF;        unsigned short range_mm = ntohs(data.ranges[i]);        double range_m = (double)range_mm / 1000.0;	          //if( range_m == this->max_range ) range_m = 0;        bearing += incr;        //if( range_m < this->max_range )        //{        double px = range_m * cos(bearing);        double py = range_m * sin(bearing);	          //rtk_fig_line(this->scan_fig, 0.0, 0.0, px, py);	            rtk_fig_line( this->scan_fig, lx, ly, px, py );        lx = px;        ly = py;	            // add little boxes at high intensities (like in playerv)        if(data.intensity[i] > 0 )	      {          rtk_fig_rectangle(this->scan_fig, px, py, 0, 0.05, 0.05, 1);          //rtk_fig_line( this->scan_fig, 0,0,px, py );	      }	        }      // RTV - son demo      rtk_fig_line( this->scan_fig, 0.0, 0.0, lx, ly );	    }    //else    //  puts( "subscribed but no data avail!!!!!!!!!!!!!!" );  }}#endif