/* *  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 set of binary contact switches *        useful as bumpers or whiskers. * Author: Richard Vaughan * Date: 28 Nov 2000 * CVS info: $Id: bumperdevice.cc,v 1.3 2002/11/11 08:21:39 rtv Exp $ */#include <math.h>#include "world.hh"#include "bumperdevice.hh"#include "raytrace.hh"//#include "player.h"#include "world.hh"// constructorCBumperDevice::CBumperDevice( 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_bumper_data_t );  m_command_len = 0;  m_config_len  = 1; // can request geometry of bumper set  m_reply_len  = 1;    m_player.code = PLAYER_BUMPER_CODE; // from player.h  m_interval = 0.1; // update interval in seconds   // zero the config data  memset( this->bumpers, 0, 	  sizeof(this->bumpers[0])*PLAYER_BUMPER_MAX_SAMPLES );  // a sensible default is 1 straight 10cm sensor facing forwards  this->bumper_count = 1;  this->bumpers[0].x_offset = 0;   this->bumpers[0].y_offset = 0;   this->bumpers[0].th_offset = 0;   this->bumpers[0].length = 0.1;   this->bumpers[0].radius = 0; }///////////////////////////////////////////////////////////////////////////// Startup routine//bool CBumperDevice::Startup(){  if (!CPlayerEntity::Startup())    return false;  return true;}///////////////////////////////////////////////////////////////////////////// Load the entity from the world filebool CBumperDevice::Load(CWorldFile *worldfile, int section){  int i;  int scount;  char key[256];    if (!CPlayerEntity::Load(worldfile, section))    return false;  // Load the geometry of the bumper ring  scount = worldfile->ReadInt(section, "bcount", 0);  if (scount > 0)  {    this->bumper_count = scount;    for (i = 0; i < this->bumper_count; i++)    {      snprintf(key, sizeof(key), "bpose[%d]", i);      this->bumpers[i].x_offset =         worldfile->ReadTupleLength(section, key, 0,                                     this->bumpers[i].x_offset );      this->bumpers[i].y_offset =         worldfile->ReadTupleLength(section, key, 1,                                    this->bumpers[i].y_offset );      this->bumpers[i].th_offset =         worldfile->ReadTupleAngle(section, key, 2,                                   this->bumpers[i].th_offset );      this->bumpers[i].length =         worldfile->ReadTupleLength(section, key, 3,                                    this->bumpers[i].length );            this->bumpers[i].radius =         worldfile->ReadTupleLength(section, key, 4,                                   this->bumpers[i].radius );    }  }    //printf( "bumper count %d\n", this->bumper_count );  // convert those specs into internal scanline repn.  for (i = 0; i < this->bumper_count; i++)  {    if( this->bumpers[i].radius == 0 )    {       //printf( "bumper #%d is a straight line\n", i );      // straight line scan      // shift from center to end of arc            double cx = this->bumpers[i].x_offset ;      double cy = this->bumpers[i].y_offset;      // rotate the angle 90 deg      double oth = this->bumpers[i].th_offset + M_PI/2.0;      double len = this->bumpers[i].length;	        double dx = len/2.0 * cos(oth);      double dy = len/2.0 * sin(oth);	        this->scanlines[i].x_offset = cx - dx;      this->scanlines[i].y_offset = cy - dy;      this->scanlines[i].th_offset = oth;      this->scanlines[i].length = len;      this->scanlines[i].radius = this->bumpers[i].radius;    }    else // arc scan    {      //printf( "bumper #%d is an arc\n", i );      // no need for processing for arcs - just copy the setup in      memcpy( &this->scanlines[i],               &this->bumpers[i],               sizeof(this->bumpers[i]) );    }  }  return true;}///////////////////////////////////////////////////////////////////////////// Update the bumper datavoid CBumperDevice::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();    // the data to export to player  player_bumper_data_t data;  // Check bounds  assert((size_t) this->bumper_count <= ARRAYSIZE(data.bumpers));  // Do each bumper  for (int s = 0; s < this->bumper_count; s++)  {    double ox = this->scanlines[s].x_offset;    double oy = this->scanlines[s].y_offset;    double oth = this->scanlines[s].th_offset;    double len = this->scanlines[s].length;    double radius = this->scanlines[s].radius;          LocalToGlobal(ox, oy, oth);          if(  radius == 0.0 )    {      // straight line scan      CLineIterator lit( ox, oy, oth, len,                          m_world->ppm, m_world->matrix, PointToBearingRange );      data.bumpers[s] = (uint8_t)0; // no hit so far	        CEntity* ent;      while( (ent = lit.GetNextEntity()) )	    {	      if( ent!=this && ent!=m_parent_entity && ent->obstacle_return )         {          data.bumpers[s] = (uint8_t)1; // we're touching something!          break;        }	    }    }    else // arc scan    {	        double scan_th = len/radius;      //printf( "request scan (%.2f %.2f %.2f) radius %.2f scan_th %.2f\n",      //  ox, oy, oth, radius, scan_th );      CArcIterator ait( ox, oy, oth, radius, scan_th,                         m_world->ppm, m_world->matrix );	        data.bumpers[s] = (uint8_t)0; // no hit so far	        CEntity* ent;      while( (ent = ait.GetNextEntity()) )	    {	      if( ent!=this && ent!=m_parent_entity && ent->obstacle_return            && ent != m_world->root )         {          data.bumpers[s] = (uint8_t)1; // we're touching something!          break;        }	    }    }  }      data.bumper_count = (uint8_t)this->bumper_count;  PutData( &data, sizeof(data) );    return;}///////////////////////////////////////////////////////////////////////////// Process configuration requests.void CBumperDevice::UpdateConfig(){  int len;  void* client;  char buffer[PLAYER_MAX_REQREP_SIZE];  player_bumper_geom_t geom;  while (true)  {    len = GetConfig(&client, &buffer, sizeof(buffer));    if (len <= 0)      break;    switch (buffer[0] )    {      case PLAYER_BUMPER_GET_GEOM_REQ:        // Return the bumper geometry        assert(this->bumper_count <= ARRAYSIZE(geom.bumper_def));        geom.bumper_count = htons(this->bumper_count);        for (int s = 0; s < this->bumper_count; s++)        {          geom.bumper_def[s].x_offset =             htons((short) (this->bumpers[s].x_offset * 1000));          geom.bumper_def[s].y_offset =             htons((short) (this->bumpers[s].y_offset * 1000));          geom.bumper_def[s].th_offset =             htons((short) (RTOD(this->bumpers[s].th_offset) ));          geom.bumper_def[s].length =             htons((short) (this->bumpers[s].length * 1000));          geom.bumper_def[s].radius =             htons((short) (this->bumpers[s].radius * 1000));        }        PutReply(client, PLAYER_MSGTYPE_RESP_ACK, NULL, &geom, sizeof(geom));        break;      default:        PRINT_WARN1("invalid bumper configuration request [%c]", buffer[0]);        PutReply(client, PLAYER_MSGTYPE_RESP_NACK);        break;    }  }}#ifdef INCLUDE_RTK2///////////////////////////////////////////////////////////////////////////// Initialise the rtk guivoid CBumperDevice::RtkStartup(){  CPlayerEntity::RtkStartup();    // draw the bumpers into the body fig  // Set the color  rtk_fig_color_rgb32(this->fig, this->color);    for( int s=0; s<this->bumper_count; s++ )    this->RtkRenderBumper( this->fig, s );    // Create a figure representing the data  this->scan_fig = rtk_fig_create(m_world->canvas, NULL, 55);  rtk_fig_linewidth( this->scan_fig, 2 ); // thick line  rtk_fig_color_rgb32(this->scan_fig, RGB(255,0,0) ); // RED!}///////////////////////////////////////////////////////////////////////////// Finalise the rtk guivoid CBumperDevice::RtkShutdown(){  // Clean up the figure we created  rtk_fig_destroy(this->scan_fig);  CPlayerEntity::RtkShutdown();} ///////////////////////////////////////////////////////////////////////////// Update the rtk guivoid CBumperDevice::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_bumper_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->bumper_count; s++)	    {	              // set the color dependent on the bumper state         // Set the color        if( data.bumpers[s] )          this->RtkRenderBumper( this->scan_fig, s );	    }    }    //else    //PRINT_WARN2( "GET DATA RETURNED WRONG AMOUNT (%d/%d bytes)", res, sizeof(data) );  }  }void CBumperDevice::RtkRenderBumper( rtk_fig_t* fig, int bumper ){  double ox = this->scanlines[bumper].x_offset;  double oy = this->scanlines[bumper].y_offset;  double oth = this->scanlines[bumper].th_offset;  double len = this->scanlines[bumper].length;    if( this->scanlines[bumper].radius == 0 ) // straight line  {    double x2 = ox + len * cos( oth );    double y2 = oy + len * sin( oth );    rtk_fig_line( fig, ox, oy, x2, y2 );  }  else // arc  {    double xy_size = 2.0 * this->scanlines[bumper].radius;	      double arc_theta =       this->scanlines[bumper].length / this->scanlines[bumper].radius;          double start_angle = -arc_theta/2.0;    double end_angle = +arc_theta/2.0;          rtk_fig_ellipse_arc( fig, ox, oy, oth,                          xy_size, xy_size,                          start_angle, end_angle );   }}#endif