idardevice.cc

一个机器人平台

		  // also reflectors.
		  //break;
		  
		case IDARReflect: // it's a reflector (ie. an obstacle)
		  //printf( "REFLECT from %p type %s idar_return %d\n",
		  //	ent, ent->token, ent->idar_return );
			
		  // try poking this message into my own receiver
		  if( (intensity = LookupIntensity(0,range,true)) > 0 )
		    ReceiveMessage( (CEntity*)this, 
				    transmit->mesg,
				    transmit->len,
				    intensity, 
				    true );
		  break;
			
		default:
		  printf( "STAGE WARNING: UNKNOWN IDAR VALUE %d\n", 
			  ent->idar_return );
		}
		  
	      break; // out of the while loop because we hit something
	    }
	}
#ifdef INCLUDE_RTK2
#ifdef RENDER_SCANLINES
      rtk_fig_color_rgb32(this->rays_fig, RGB(220,220,220) );	
      rtk_fig_arrow(this->rays_fig, 0,0, scanline_bearing-oth, range, 0.03);
#endif
#endif    
    }
}


bool CIdarDevice::ReceiveMessage( CEntity* sender,
				  unsigned char* mesg, int len, 
				  uint8_t intensity,
				  bool reflection )
{
  //PRINT_DEBUG( "RECEIVE MESSAGE" );

  //printf( "mesg recv - sensor: %d  len: %d  intensity: %d  refl: %d\n",
  //  sensor, len, intensity, reflection );

  // TEST INTENSITY
  // we only accept this message if it's the most intense thing we've seen
  if( intensity < recv.intensity )
    return false;

  
  // TEST REFLECTION AND  ANGLE OF INCIDENCE
  // we accept reflections without checking angle
  if( !reflection ) 
    { // it's not a reflection, so we check the angle of incidence 
      double sx, sy, sa; // the sender's pose
      double lx, ly, la; // my pose
      
      sender->GetGlobalPose( sx, sy, sa );
      this->GetGlobalPose( lx, ly, la );
      
      double incidence = atan2( sy-ly, sx-lx ) - la;
      NORMALIZE(incidence);
      
      //printf( "%p G: %.2f,%.2f,%.2f   L: %.2f,%.2f,%.2f inc: %.2f\n", 
      //      this, sx, sy, sa, lx, ly, la, incidence );
      
      // if it's out of our receptive angle, we reject it and bail.
      if( fabs(incidence) > IDAR_RECEIVE_ANGLE/2.0 )
	return false;
    }
  
  
  // looks good  - accept the message
  // copy the message into the data buffer
  memcpy( &recv.mesg, mesg, len );
  
  recv.len = len;
  recv.intensity = intensity;
  recv.reflection = (uint8_t)reflection;
  
  // record the time we received this message
  recv.timestamp_sec = m_world->m_sim_timeval.tv_sec;
  recv.timestamp_usec = m_world->m_sim_timeval.tv_usec;
  
#ifdef INCLUDE_RTK2
      
  // reflections are green, remote messages are red
  rtk_fig_color_rgb32(this->data_fig, 
		      reflection ? RGB(0,200,0) : RGB(200,0,0) );
  
  rtk_fig_arrow(this->data_fig, 0,0,0, 1.5*size_x, size_y );
  
#ifdef SHOW_MSGS
  
  // room for the message in hex text
  char message[ 3 * IDARBUFLEN + 6];
  
  // print the message in hex, with a space between each char
  for( int c=0; c<recv.len; c++ )
    sprintf( message + 3*c, "%2X ", recv.mesg[c] );
  message[ 3 * recv.len ] = 0; // terminate
  
  // add the intensity
  sprintf( message, "%s (%d)", message, recv.intensity );
  
  
  rtk_fig_text(this->data_fig, 0,0,0, message);
#endif
#endif

  return true; // we accepted the message
}



// HERE ARE A BUNCH OF RANGE/INTENSITY CONVERSION TABLES DETERMINED
// EMPIRICALLY BY EXPERIMENTS WITH THE PHERBOTS. WE USE ONE OF THESE
// FOR ROBOT-TO-ROBOT TRANSMISSIONS AND ONE FOR REFLECTIONS

// ranges are in INCHES

// (BASED ON 3g32)
int reflection_intensity[] = { 150,129,117,115,112,104,103,102,101,100};
int reflection_range[]     = {   0,  1,  7, 10, 14, 26, 28, 31, 35, 39, 99};
int reflection_len = 10;

// (BASED ON HERE_I_AM)
int direct_intensity[] = { 130,126,125,123,122,121,120,119,118,117,115,114,
			   112,112,111,109,108,107,106,105,104,103,101, 97,
			   96, 95, 94, 93, 90 };
int direct_range[]     = {   6,  9, 12, 15, 18, 21, 24, 28, 32, 36, 40, 44, 
			     48, 52, 56, 60, 64, 68, 72, 76, 80, 84, 88, 92, 
			     96,100,104,108,120,1200};
int direct_len = 30;


// RTV - reverse lookup distance to intensity
uint8_t CIdarDevice::LookupIntensity( uint8_t transmit_intensity,
				      double trans_range, 
				      bool reflection )
{
  // transmit_intensity isn't used for now

  // subtract our radius from the range reading

  trans_range -= size_x / 2.0;

  // convert range from m to inches (yuk!)
  int irange = (int)(trans_range * 39.37); // m to inches
   
  uint8_t result = 255; // make sure we got a result

  int* dist_table = 0;
  int* sigi_table = 0;
  int table_len = 0;

  // choose the tables to work from
  if( reflection )
    {
      dist_table = reflection_range;
      sigi_table = reflection_intensity;
      table_len = reflection_len;
    }
  else
    {
      dist_table = direct_range;
      sigi_table = direct_intensity;
      table_len = direct_len;
    }      

  // now do the lookup

  // if we're below the minimum distance, return the maximum intensity
  if( irange < dist_table[0] )
    {
      //printf( "vrange %d below minimum %d\n", vrange, dist_table[0] );
      result = sigi_table[0];
    }
  // if we're above the max distance, return zero intensity
  else if( irange > dist_table[ table_len-1 ] )
    {
      //printf( "irange %d is above maximum %d %s\n", 
      //      irange, dist_table[ table_len-1],
      //      reflection ? "reflect" : "direct" );
      
      result = 0;//sigi_table[ table_len-1 ];
    }
  // otherwise, interpolate a result from the table
  else
    for (int j = 1; j < table_len; j++)	
      {
	if( irange == dist_table[j] )
	  {
	    result = sigi_table[j];
	    break;
	  }
	else if (irange < dist_table[j])	
	  {
	    //printf( "irange %d is between %d and %d %s\n",
	    //    irange, dist_table[j-1], dist_table[j],
	    //    reflection ? "reflect" : "direct" );
	    
	    //# we're in the right range; calc ans;
	    result =  (uint8_t)((irange - dist_table[j-1])
				* (sigi_table[j] - sigi_table[j-1])
				/ (dist_table[j] - dist_table[j-1])
				+ sigi_table[j-1] );
	    break;
	  }
      }
  
  
  assert( result != 255 );
  
  if( result > 200 )
    printf( "WARNING: intensity suspicious!" );
  
  //printf( "RANGE: %umm %u' %uv  INT: %u\n", 
  //  trans_range, irange, vrange, result );
  
  return result;
} 



#ifdef INCLUDE_RTK2

///////////////////////////////////////////////////////////////////////////
// Initialise the rtk gui
void CIdarDevice::RtkStartup()
{
  CPlayerEntity::RtkStartup();
  
  // Create a figure representing this object
  this->data_fig = rtk_fig_create(m_world->canvas, this->fig, 49);
  this->rays_fig = rtk_fig_create(m_world->canvas, this->fig, 48);
  
  rtk_fig_origin(this->data_fig, 0,0,0 );
  rtk_fig_origin(this->rays_fig, 0,0,0 );
 
  // show our orientation in the body figure
  rtk_fig_line( this->fig, 0,0, size_x/2.0, 0);

  // Set the color
  rtk_fig_color_rgb32(this->data_fig, this->color);
  rtk_fig_color_rgb32(this->rays_fig, RGB(200,200,200) );
  
#ifdef RENDER_INDEX
  // render our index number
  char buf[16];
  sprintf( buf,"%d", m_player.index );
  rtk_fig_text(this->fig, 2.0 * size_x, 0, 0, buf );
#endif

}


///////////////////////////////////////////////////////////////////////////
// Finalise the rtk gui
void CIdarDevice::RtkShutdown()
{
  // Clean up the figure we created
  if(this->data_fig) rtk_fig_destroy(this->data_fig);
  if(this->rays_fig) rtk_fig_destroy(this->rays_fig);
  
  CPlayerEntity::RtkShutdown();
} 


///////////////////////////////////////////////////////////////////////////
// Update the rtk gui
void CIdarDevice::RtkUpdate()
{
  CPlayerEntity::RtkUpdate();
   
  // Get global pose
  //double gx, gy, gth;
  //GetGlobalPose(gx, gy, gth);
  //rtk_fig_origin(this->data_fig, gx, gy, gth );

  //  if( m_world->ShowDeviceData( this->type_num) )
  //{
  //  rtk_fig_show( this->data_fig, true );
  //  rtk_fig_show( this->rays_fig, true );
  // }
  //else
  //{
  //  rtk_fig_show( this->data_fig, false );
  //  rtk_fig_show( this->rays_fig, false );
  // }
  
  //if( Subscribed() < 1 )
  //{
  //rtk_fig_clear( this->rays_fig );
  //rtk_fig_clear( this->data_fig );
  // }
}

#endif