/* *  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 sonar ring. * Author: Andrew Howard, Richard Vaughan * Date: 28 Nov 2000 * CVS info: $Id: sonardevice.cc,v 1.3 2002/11/11 08:21:40 rtv Exp $ */#include <math.h>#include "world.hh"#include "sonardevice.hh"#include "raytrace.hh"#include "world.hh"// constructorCSonarDevice::CSonarDevice( 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_sonar_data_t );  m_command_len = 0;  m_config_len  = 1;  m_reply_len  = 1;    m_player.code = PLAYER_SONAR_CODE; // from player's messages.h  this->min_range = 0.20;  this->max_range = 5.0;  this->power_on = true;  // Initialise the sonar poses to default values  this->sonar_count = SONARSAMPLES;  for (int i = 0; i < this->sonar_count; i++)  {    this->sonars[i][0] = 0;    this->sonars[i][1] = 0;    this->sonars[i][2] = i * 2 * M_PI / this->sonar_count;  }    // zero the data  memset( &this->data, 0, sizeof(this->data) );}///////////////////////////////////////////////////////////////////////////// Startup routine//bool CSonarDevice::Startup(){  if (!CPlayerEntity::Startup())    return false;  return true;}///////////////////////////////////////////////////////////////////////////// Load the entity from the world filebool CSonarDevice::Load(CWorldFile *worldfile, int section){  int i;  int scount;  char key[256];    if (!CPlayerEntity::Load(worldfile, section))    return false;  // Load sonar min and max range  this->min_range = worldfile->ReadLength(section, "min_range", this->min_range);  this->max_range = worldfile->ReadLength(section, "max_range", this->max_range);    // power on or off? (TODO - have this setting saved)  this->power_on = worldfile->ReadInt(section, "power_on", this->power_on );  // Load the geometry of the sonar ring  scount = worldfile->ReadInt(section, "scount", 0);  if (scount > 0)  {    this->sonar_count = scount;    for (i = 0; i < this->sonar_count; i++)    {      snprintf(key, sizeof(key), "spose[%d]", i);      this->sonars[i][0] = worldfile->ReadTupleLength(section, key, 0, 0);      this->sonars[i][1] = worldfile->ReadTupleLength(section, key, 1, 0);      this->sonars[i][2] = worldfile->ReadTupleAngle(section, key, 2, 0);    }  }  return true;}///////////////////////////////////////////////////////////////////////////// Update the sonar datavoid CSonarDevice::Update( double sim_time ) {  CPlayerEntity::Update( sim_time );  // dump out if noone is subscribed  if(!Subscribed())    return;  // Check to see if it is time to update  //  - if not, return right away.  if( sim_time - m_last_update < m_interval)    return;  m_last_update = sim_time;  // Process configuration requests  UpdateConfig();    // Check bounds  assert((size_t) this->sonar_count <= ARRAYSIZE(this->data.ranges));    if( this->power_on )    // Do each sonar    for (int s = 0; s < this->sonar_count; s++)      {	// Compute parameters of scan line	double ox = this->sonars[s][0];	double oy = this->sonars[s][1];	double oth = this->sonars[s][2];	LocalToGlobal(ox, oy, oth);		double range = this->max_range;		CLineIterator lit( ox, oy, oth, this->max_range, 			   m_world->ppm, m_world->matrix, PointToBearingRange );	CEntity* ent;		while( (ent = lit.GetNextEntity()) )	  {	    if( ent != this && ent != m_parent_entity && ent->sonar_return ) 	      {		range = lit.GetRange();		break;	      }	  }		uint16_t v = (uint16_t)(1000.0 * range);		// Store range in mm in network byte order	this->data.range_count = htons(this->sonar_count);	this->data.ranges[s] = htons(v);      }  PutData( &this->data, sizeof(this->data) );  return;}///////////////////////////////////////////////////////////////////////////// Process configuration requests.void CSonarDevice::UpdateConfig(){  int s, len;  void* client;  char buffer[PLAYER_MAX_REQREP_SIZE];  player_sonar_geom_t geom;  //player_sonar_power_config_t power;  while (true)  {    len = GetConfig(&client, &buffer, sizeof(buffer));    if (len <= 0)      break;    switch (buffer[0])    {      case PLAYER_SONAR_POWER_REQ:        // we got a sonar power config	// set the new power status	this->power_on = ((player_sonar_power_config_t*)buffer)->value;        PutReply(client, PLAYER_MSGTYPE_RESP_ACK);        break;      case PLAYER_SONAR_GET_GEOM_REQ:        // Return the sonar geometry        assert(this->sonar_count <= ARRAYSIZE(geom.poses));        geom.pose_count = htons(this->sonar_count);        for (s = 0; s < this->sonar_count; s++)        {          geom.poses[s][0] = htons((short) (this->sonars[s][0] * 1000));          geom.poses[s][1] = htons((short) (this->sonars[s][1] * 1000));          geom.poses[s][2] = htons((short) (this->sonars[s][2] * 180 / M_PI));        }        PutReply(client, PLAYER_MSGTYPE_RESP_ACK, NULL, &geom, sizeof(geom));        break;      default:        PRINT_WARN1("invalid sonar configuration request [%c]", buffer[0]);        PutReply(client, PLAYER_MSGTYPE_RESP_NACK);        break;    }  }}//size_t CSonarDevice::PutData( void* vdata, size_t len )//{  // export the data right away//size_t retval = CPlayerEntity::PutData( vdata, len );  //player_sonar_data_t* data = (player_sonar_data_t*)vdata;  //return( retval );//}#ifdef INCLUDE_RTK2///////////////////////////////////////////////////////////////////////////// Initialise the rtk guivoid CSonarDevice::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 CSonarDevice::RtkShutdown(){  // Clean up the figure we created  rtk_fig_destroy(this->scan_fig);  CPlayerEntity::RtkShutdown();} ///////////////////////////////////////////////////////////////////////////// Update the rtk guivoid CSonarDevice::RtkUpdate(){  CPlayerEntity::RtkUpdate();     // Get global pose  //double gx, gy, gth;  //GetGlobalPose(gx, gy, gth);  //rtk_fig_origin(this->scan_fig, gx, gy, gth );  rtk_fig_clear(this->scan_fig);  // see the comment in CLaserDevice for why this gets the data out of  // the buffer instead of storing hit points in ::Update() - RTV  player_sonar_data_t data;    if( Subscribed() > 0 && m_world->ShowDeviceData( this->lib_entry->type_num) )  {    size_t res = GetData( &data, sizeof(data));    if( res == sizeof(data) )    {      for (int s = 0; s < this->sonar_count; s++)      {        double ox = this->sonars[s][0];        double oy = this->sonars[s][1];        double oth = this->sonars[s][2];        LocalToGlobal(ox, oy, oth);        	// convert from integer mm to double m        double range = (double)ntohs(data.ranges[s]) / 1000.0;	//if( range < max_range )	  {	    double x1 = ox;	    double y1 = oy;	    double x2 = x1 + range * cos(oth); 	    double y2 = y1 + range * sin(oth);       	    	    rtk_fig_line(this->scan_fig, x1, y1, x2, y2 );	  }      }    }    //else    //PRINT_WARN2( "GET DATA RETURNED WRONG AMOUNT (%d/%d bytes)", res, sizeof(data) );  }  }#endif