/******************************************************************************* File: irdevice.cc* Author: Richard Vaughan* Date: 22 October 2001* Desc: Simulates a single sensor of HRL's Infrared Data and Ranging Device** 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** CVS info:* $Source: /cvsroot/playerstage/code/stage/src/models/Attic/idardevice.cc,v $* $Author: rtv $* $Revision: 1.2 $******************************************************************************/#include <math.h>#include <sys/types.h>#include <sys/socket.h>#include <arpa/inet.h>#include <unistd.h>#include <iostream>#include "world.hh"#include "idardevice.hh"#include "raytrace.hh"#define DEBUG//#undef DEBUG// control display of player index in device body//#define RENDER_INDEX//#define RENDER_SCANLINES//#define SHOW_MSGS#define IDAR_TRANSMIT_ANGLE        M_PI/4.0#define IDAR_RECEIVE_ANGLE        M_PI/4.0#define IDAR_SCANLINES        5#define IDAR_MAX_RANGE       1.0// handy circular normalization macros#define NORMALIZECIRCULAR_INT(value,max) (((value%max)+max)%max)#define NORMALIZECIRCULAR_DOUBLE(value,max) fmod( fmod(value,max)+max, max)#define NORMALIZERADIANS(value)  NORMALIZECIRCULAR_DOUBLE(value,TWOPI)#define NORMALIZEDEGREES(value)  NORMALIZECIRCULAR_DOUBLE(value,360.0)// constructor CIdarDevice::CIdarDevice(LibraryItem* libit, CWorld *world, CEntity *parent )  : CPlayerEntity(libit, world, parent ){  // we're invisible except to other IDARs  laser_return = LaserTransparent;  sonar_return = false;  obstacle_return = false;  idar_return = IDARReceive;  // set the Player IO sizes correctly for this type of Entity  m_data_len    = 0;//sizeof( player_idar_data_t );  m_command_len = 0;//sizeof( player_idar_command_t );  m_config_len  = 1;  m_reply_len  = 1;    m_player.code = PLAYER_IDAR_CODE; // from player's messages.h      m_max_range = IDAR_MAX_RANGE;      size_x = 0.03; // this is the actual physical size of the HRL device  size_y = 0.02; // but can be changed in the world file to suit  // Set the default shape  shape = ShapeRect;  m_interval = 0.001; // updates frequently,  // but only has work to do when it receives a command  // so it's not a CPU hog unless we send it lots of messages   // init the message buffer  memset( &recv, 0, sizeof(recv) );  m_num_scanlines = IDAR_SCANLINES;   m_angle_per_sensor = IDAR_TRANSMIT_ANGLE;  m_angle_per_scanline = m_angle_per_sensor / m_num_scanlines;}///////////////////////////////////////////////////////////////////////////// Startup routine//bool CIdarDevice::Startup(){  if (!CPlayerEntity::Startup())    return false;  return true;}void CIdarDevice::Sync( void ){  // sync our children  CPlayerEntity::Sync();  void *client;  player_idar_config_t cfg;    // Get config  int res = GetConfig( &client, &cfg, sizeof(cfg));    switch( res )    {     case 0:      // nothing available - nothing to do      break;          case -1: // error      PRINT_ERR( "get config failed" );      break;          case sizeof(cfg): // the size we expect - we received a config request      // what does the client want us to do?      switch( cfg.instruction )	{	case IDAR_TRANSMIT:	  //puts( "TX" );	  TransmitMessage( &(cfg.tx) );	  PutReply(client, PLAYER_MSGTYPE_RESP_ACK );	  	  break;	  	case IDAR_RECEIVE:	  //puts( "RX" );	  // send back the currently stored message	  PutReply(client, PLAYER_MSGTYPE_RESP_ACK, NULL, &recv, sizeof(recv));	  ClearMessage(); // wipe the buffer	  break;	  	case IDAR_RECEIVE_NOFLUSH:	  //puts( "RX_NF" );	  // send back the currently stored message	  PutReply(client, PLAYER_MSGTYPE_RESP_ACK, NULL, &recv, sizeof(recv));	  // don't clear the current mesg	  break;	  	  // two in one for efficiency!	case IDAR_TRANSMIT_RECEIVE:	  TransmitMessage( &(cfg.tx) );	  PutReply(client, PLAYER_MSGTYPE_RESP_ACK, NULL, &recv, sizeof(recv));	  ClearMessage(); // wipe the buffer	  break;	default:	  printf( "STAGE warning: unknown idar config instruction %d\n",		  cfg.instruction );	}      break;          default: // a bad value      PRINT_ERR1( "wierd: idar config returned %d ", res );      break;    }}// NOTE unlike most devices, live data is not published here - that's// done in ReceiveMessage()void CIdarDevice::Update( double sim_time ) {  CPlayerEntity::Update( sim_time ); // inherit some debug output    // UPDATE OUR RENDERING  double x, y, th;  GetGlobalPose( x,y,th );    ReMap( x, y, th );    // dump out if noone is subscribed  if(!Subscribed())    {      return;     }    m_last_update = sim_time;}void CIdarDevice::ClearMessage( void ){  // wipe the message (zero the buffer)  memset( &recv, 0, sizeof(recv) );#ifdef INCLUDE_RTK2  // unrender the message   rtk_fig_clear(this->data_fig);#endif}void CIdarDevice::TransmitMessage( idartx_t* transmit ){  // really should be a valid message here  assert( transmit && (transmit->len > 0) );    //#ifdef DEBUG  //printf( "\nTRANSMITTING %d bytes at %p - ",  //transmit->len, transmit->mesg );    //for( int f=0; f<transmit->len; f++ )  //printf( "%d ", transmit->mesg[f] );  //puts ( "" );  //#endif    // now perform the transmission  double ox, oy, oth;  GetGlobalPose( ox, oy, oth ); // start position for the ray trace#ifdef INCLUDE_RTK2  rtk_fig_clear(this->rays_fig);#endif    double scan_start_angle =     (oth - m_angle_per_sensor/2.0) + m_angle_per_scanline/2.0;        for( int scanline=0; scanline < m_num_scanlines; scanline++ )      {		// Compute parameters of scan line	double scanline_bearing = 	scan_start_angle + scanline * m_angle_per_scanline;	      // normalize to make things easier to read      scanline_bearing = fmod( scanline_bearing, TWOPI );	            CLineIterator lit( ox, oy, scanline_bearing, m_max_range,			 m_world->ppm, m_world->matrix,			 PointToBearingRange );	            //printf( "direction %d scanline %d "      //      "from %.2f,%.2f %.2fm at %.2f radians\n",       //      s, scanline, ox, oy, m_max_range, scanline_bearing );	            CEntity* ent;      double range = m_max_range;	            // get the next thing in our beam, until the beam is blocked      while( (ent = lit.GetNextEntity() ) ) 	{	  //printf( "HIT %p type %s idar_return %d (i am %p,%d,%d)\n",	  //ent, ent->token, ent->idar_return,	  //(CEntity*)this, token, idar_return );			  //printf( "IDARReceive == %d\n", IDARReceive );	  // if it's not me or my parent or a sibling and it's not	  // transparent to idar, i'll try sending a message to it	  if( (ent != (CEntity*)this) && 	      (ent != this->m_parent_entity) &&	      (ent->m_parent_entity != this->m_parent_entity) &&	      ( ent->idar_return != IDARTransparent) )	    {		    	      range = lit.GetRange();	      assert( range >= 0 );		   	      //printf( "s:%d.%d range: %.2f\n",	      //    s, scanline, range );	      //printf( "hit entity %p (%d:%d:%d)\n",	      //     ent, ent->m_player.port,  ent->m_player.code,	      //     ent->m_player.index );	      uint8_t intensity;		    	      switch( ent->idar_return )		{		case IDARReceive: // it's a receiver		  // attempt to poke this message into his receiver		  //printf( "TRANSMIT to %p type %s idar_return %d\n",				//ent, ent->token, ent->idar_return );								  //PRINT_DEBUG1( "POKING A MESSAGE INTO %p", ent );		  if( (intensity = LookupIntensity(0,range,false)) > 0 )		    ((CIdarDevice*)ent)->		      ReceiveMessage( (CEntity*)this, 				      transmit->mesg,				      transmit->len,				      intensity,				      false );		  		  // continue to next case, since IR receivers are