visiondevice.cc

一个机器人平台

    {
      blobleft = s;
      blobcol = m_scan_channel[s];

      // loop until we hit the end of the blob
      // there has to be a gap of >1 pixel to end a blob
      // this avoids getting lots of crappy little blobs
      while( m_scan_channel[s] == blobcol || m_scan_channel[s+1] == blobcol ) 
        s++;

      blobright = s-1;
      double robotHeight = 0.6; // meters
      int xCenterOfBlob = blobleft + ((blobright - blobleft )/2);
      double rangeToBlobCenter = m_scan_range[ xCenterOfBlob ];
      if(!rangeToBlobCenter)
      {
        // if the range to the "center" is zero, then use the range
        // to the start.  this can happen, for example, when two 1-pixel
        // blobs that are 1 pixel apart are grouped into a single blob in 
        // whose "center" there is really no blob at all
        rangeToBlobCenter = m_scan_range[ blobleft ];
      }
      double startyangle = atan2( robotHeight/2.0, rangeToBlobCenter );
      double endyangle = -startyangle;
      blobtop = cameraImageHeight/2 - (int)(startyangle/yRadsPerPixel);
      blobbottom = cameraImageHeight/2 -(int)(endyangle/yRadsPerPixel);
      int yCenterOfBlob = blobtop +  ((blobbottom - blobtop )/2);

      if (blobtop < 0)
        blobtop = 0;
      if (blobbottom > cameraImageHeight - 1)
        blobbottom = cameraImageHeight - 1;

      // useful debug - keep
      /*
        cout << "Robot " << this
        << " sees " << (int)blobcol-1
        << " start: " << blobleft
        << " end: " << blobright
        << " top: " << blobtop
        << " bottom: " << blobbottom
        << endl << endl;
        */

      // fill in an arrau entry for this blob
      //
      blobs[numBlobs].channel = blobcol-1;
      blobs[numBlobs].x = xCenterOfBlob;
      blobs[numBlobs].y = yCenterOfBlob;
      blobs[numBlobs].left = blobleft;
      blobs[numBlobs].top = blobtop;
      blobs[numBlobs].right = blobright;
      blobs[numBlobs].bottom = blobbottom;
      blobs[numBlobs].area = (blobtop - blobbottom) * (blobleft-blobright);
      blobs[numBlobs].range = (int)(rangeToBlobCenter*1000.0); 

      numBlobs++;
    }
  }

  // now we have the blobs we have to pack them into ACTS format (yawn...)

  // ACTS has blobs sorted by channel (color), and by area within channel, 
  // so we'll bubble sort the
  // blobs - this could be more efficient, so might fix later.
  if( numBlobs > 1 )
  {
    //cout << "Sorting " << numBlobs << " blobs." << endl;
    int change = true;
    ColorBlob tmp;

    while( change )
    {
      change = false;

      for( int b=0; b<numBlobs-1; b++ )
      {
        // if the channels are in the wrong order
        if( (blobs[b].channel > blobs[b+1].channel)
            // or they are the same channel but the areas are 
            // in the wrong order
            ||( (blobs[b].channel == blobs[b+1].channel) 
                && blobs[b].area < blobs[b+1].area ) )
        {		      
          //switch the blobs
          memcpy( &tmp, &(blobs[b]), sizeof( ColorBlob ) );
          memcpy( &(blobs[b]), &(blobs[b+1]), sizeof( ColorBlob ) );
          memcpy( &(blobs[b+1]), &tmp, sizeof( ColorBlob ) );

          change = true;
        }
      }
    }
  }

  // now run through the blobs, packing them into the ACTS buffer
  // counting the number of blobs in each channel and making entries
  // in the acts header

  for( int b=0; b<numBlobs; b++ )
  {
    // I'm not sure the ACTS-area is really just the area of the
    // bounding box, or if it is in fact the pixel count of the
    // actual blob. Here it's just the rectangular area.

    // useful debug - leave in
    /*
      cout << "blob "
      << " channel: " <<  (int)blobs[b].channel
      << " area: " <<  blobs[b].area
      << " left: " <<  blobs[b].left
      << " right: " <<  blobs[b].right
      << " top: " <<  blobs[b].top
      << " bottom: " <<  blobs[b].bottom
      << endl;
    */

    // RTV - blobs[b].area is already set above - just byteswap
    data->blobs[b].area = htonl(blobs[b].area);

    // look up the color for this channel
    data->blobs[b].color = htonl( this->channels[ blobs[b].channel ] );

    data->blobs[b].x = htons(blobs[b].x);
    data->blobs[b].y = htons(blobs[b].y);
    data->blobs[b].left = htons(blobs[b].left);
    data->blobs[b].right = htons(blobs[b].right);
    data->blobs[b].top = htons(blobs[b].top);
    data->blobs[b].bottom = htons(blobs[b].bottom);
    data->blobs[b].range = htons(blobs[b].range);

    // increment the count for this channel
    data->header[blobs[b].channel].num++;
  }


  // now we finish the header by setting the blob indexes and byte
  // swapping the counts.
  int pos = 0;
  for( int ch=0; ch<PLAYER_BLOBFINDER_MAX_CHANNELS; ch++ )
  {
    data->header[ch].index = htons(pos);
    pos += data->header[ch].num;

    // byte swap the blob count for each channel
    PRINT_DEBUG2( "channel: %d blobs: %d\n", ch,  data->header[ch].num ); 
    data->header[ch].num = htons(data->header[ch].num);
  }

  // and set the image width * height
  data->width = htons((uint16_t)cameraImageWidth);
  data->height = htons((uint16_t)cameraImageHeight);

  return sizeof(player_blobfinder_data_t);
}


#ifdef INCLUDE_RTK2

///////////////////////////////////////////////////////////////////////////
// Initialise the rtk gui
void CVisionDevice::RtkStartup()
{
  CPlayerEntity::RtkStartup();
  
  // Create a figure representing this object
  this->vision_fig = rtk_fig_create(m_world->canvas, this->fig, 99);
  rtk_fig_origin(this->vision_fig, -0.75, +0.75, 0.0);
  rtk_fig_movemask(this->vision_fig, RTK_MOVE_TRANS);
}


///////////////////////////////////////////////////////////////////////////
// Finalise the rtk gui
void CVisionDevice::RtkShutdown()
{
  // Clean up the figure we created
  rtk_fig_destroy(this->vision_fig);

  CPlayerEntity::RtkShutdown();
} 


///////////////////////////////////////////////////////////////////////////
// Update the rtk gui
void CVisionDevice::RtkUpdate()
{
  CPlayerEntity::RtkUpdate();

  if (Subscribed() < 1)
  {
    rtk_fig_show(this->vision_fig, 0);
    return;
  }
  
  rtk_fig_clear(this->vision_fig);
  rtk_fig_show(this->vision_fig, 1);
    
  // The vision figure is attached to the entity, but we dont want
  // it to rotate.  Fix the rotation here.
  double gx, gy, gth;
  double nx, ny, nth;
  GetGlobalPose(gx, gy, gth);
  rtk_fig_get_origin(this->vision_fig, &nx, &ny, &nth);
  rtk_fig_origin(this->vision_fig, nx, ny, -gth);
  
  player_blobfinder_data_t data;

  // if a client is subscribed to this device
  if( Subscribed() > 0 && m_world->ShowDeviceData( this->lib_entry->type_num) )
  {
    // attempt to get the right size chunk of data from the mmapped buffer
    if( GetData( &data, sizeof(data) ) == sizeof(data) )
    {  
      double scale = 0.007; // shrink from pixels to meters for display
	
      short width = ntohs(data.width);
      short height = ntohs(data.height);
      double mwidth = width * scale;
      double mheight = height * scale;
	
      // the view outline rectangle
      rtk_fig_color_rgb32(this->vision_fig, 0xFFFFFF);
      rtk_fig_rectangle(this->vision_fig, 0.0, 0.0, 0.0, mwidth,  mheight, 1 ); 
      rtk_fig_color_rgb32(this->vision_fig, 0x000000);
      rtk_fig_rectangle(this->vision_fig, 0.0, 0.0, 0.0, mwidth,  mheight, 0); 

      for( int c=0; c<PLAYER_BLOBFINDER_MAX_CHANNELS;c++)
      {
        short numblobs = ntohs(data.header[c].num);
        short index = ntohs(data.header[c].index);	    
	    
        for( int b=0; b<numblobs; b++ )
	      {
          // set the color from the blob data
          rtk_fig_color_rgb32( this->vision_fig, 
                               ntohl(data.blobs[index+b].color) ); 

          //short x =  ntohs(data.blobs[index+b].x);
          //short y =  ntohs(data.blobs[index+b].y);
          short top =  ntohs(data.blobs[index+b].top);
          short bot =  ntohs(data.blobs[index+b].bottom);
          short left =  ntohs(data.blobs[index+b].left);
          short right =  ntohs(data.blobs[index+b].right);
		
          //double mx = x * scale;
          //double my = y * scale;
          double mtop = top * scale;
          double mbot = bot * scale;
          double mleft = left * scale;
          double mright = right * scale;
	    
	  // get the range in meters
	  //double range = (double)ntohs(data.blobs[index+b].range) / 1000.0; 
	  
          rtk_fig_rectangle(this->vision_fig, 
                            -mwidth/2.0 + (mleft+mright)/2.0, 
                            -mheight/2.0 +  (mtop+mbot)/2.0,
                            0.0, 
                            mright-mleft, 
                            mbot-mtop, 
                            1 );

	  //rtk_fig_line( this->vision_fig,
	  //	0,0,0, 

	      }
      }
    }
    //else
    //PRINT_WARN( "no vision data available" );
  }
}

#endif