single.cc

Go to the documentation of this file.

#include <libplayerc++/playerc++.h>
#include <iostream>
#include <assert.h>

// For argument parsing
#include "args.h"

#define POP 100 // population size
#define VSPEED 0.4 // meters per second
#define WGAIN 1.0 // turn speed gain
#define SAFE_DIST 0.3 // meters
#define SAFE_ANGLE 0.4 // radians

int main(int argc, char **argv)
{
  // Parse command line options
  parse_args(argc,argv);

  // We throw exceptions on creation if we fail
  try
  {
    using namespace PlayerCc;
    using namespace std;

    // Create a player client object, using the variables assigned by the
    // call to parse_args()
    PlayerClient robot (gHostname, gPort);
    robot.SetDataMode( PLAYER_DATAMODE_PULL );
    robot.SetReplaceRule( true );
    
    Position2dProxy pp( &robot, gIndex );
    SonarProxy      sp( &robot, gIndex );
    Graphics2dProxy gp( &robot, gIndex );

    sp.RequestGeom();   // query the server for sonar positions
        
    while(1)
      {
        // blocks until new data comes from Player
        robot.Read();
        
        for( int i=0; i<POP; i++ )
          {
            // compute the vector sum of the sonar ranges             
            double dx=0, dy=0;
            
            int num_ranges = sp.GetCount();
            for( int s=0; s<num_ranges; s++ )
              {
                player_pose3d_t spose = sp.GetPose(s);
                double srange = sp.GetScan(s);
                
                dx += srange * cos( spose.pyaw );
                dy += srange * sin( spose.pyaw );
              }
            
            // compute the direction of the resultant vector 
            double resultant_angle = atan2( dy, dx );
            //double resultant_magnitude = hypot( dy, dx );
            
            double forward_speed = 0.0;
            double side_speed = 0.0;       
            double turn_speed = WGAIN * resultant_angle;
                    
            // find the index of the forward-pointing sensor
            int forward = num_ranges/2 -1 ;
            
            // if the front is clear, drive forwards
            if( (sp.GetScan(forward-1) > SAFE_DIST) &&
                (sp.GetScan(forward) > SAFE_DIST) &&
                (sp.GetScan(forward+1) > SAFE_DIST) && 
                (fabs( resultant_angle ) < SAFE_ANGLE) )
              {
                forward_speed = VSPEED;
              }
        
            // send a command to the robot's position device
            pp.SetSpeed( forward_speed, side_speed, turn_speed );
            
            // draw the resultant vector on the robot to show what it
            // is thinking
            if( forward_speed > 0 )           
              gp.Color( 0,255,0,0 );
            else
              gp.Color( 0,255,255,0 );

            player_point_2d_t pts[2];
            pts[0].px = 0;
            pts[0].py = 0;
            pts[1].px = dx;
            pts[1].py = dy;     
            
            gp.Clear();
            gp.DrawPolyline( pts, 2 );
            
          }    
      }
  }
  catch (PlayerCc::PlayerError e)
    {
      std::cerr << e << std::endl;
      return -1;
    }
}