multiple.cc

Go to the documentation of this file.

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

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

#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** ppp = new Position2dProxy*[gPop];
    SonarProxy**      spp = new SonarProxy*[gPop];
    Graphics2dProxy** gpp = new Graphics2dProxy*[gPop];

    for( uint i=0; i<gPop; i++ )
      {
        ppp[i] = new Position2dProxy( &robot , i );
        assert( ppp[i] );

        spp[i] = new SonarProxy( &robot, i );
        assert( spp[i] );

        gpp[i] = new Graphics2dProxy( &robot, i );
        assert( gpp[i] );
        
        spp[i]->RequestGeom();  // query the server for sonar
                                // positions
      }
    
    while(1)
      {
        // blocks until new data comes from Player
        robot.Read();

        for( uint i=0; i<gPop; i++ )
          if( spp[i]->IsFresh() ) // only update if this proxy has some new data
            {
              spp[i]->NotFresh(); // mark the data as old
              
              // compute the vector sum of the sonar ranges           
              double dx=0, dy=0;
              
              int num_ranges = spp[i]->GetCount();
              for( int s=0; s<num_ranges; s++ )
                {
                  player_pose3d_t spose = spp[i]->GetPose(s);
                  double srange = spp[i]->GetScan(s);
                  
                  dx += srange * cos( spose.pyaw );
                  dy += srange * sin( spose.pyaw );
                }

              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;
              
              int forward = num_ranges/2 -1 ;
              // if the front is clear, drive forwards
              if( (spp[i]->GetScan(forward-1) > SAFE_DIST) &&
                  (spp[i]->GetScan(forward) > SAFE_DIST) &&
                  (spp[i]->GetScan(forward+1) > SAFE_DIST) && 
                  (fabs( resultant_angle ) < SAFE_ANGLE) )
                {
                  forward_speed = VSPEED;
                }

              // send a command to the robot
              ppp[i]->SetSpeed( forward_speed, side_speed, turn_speed );
              
              // draw the resultant vector on the robot to show what it
              // is thinking          
              if( forward_speed > 0 )
                gpp[i]->Color( 0,255,0,0 );
              else
                gpp[i]->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;   
              
              gpp[i]->Clear();
              gpp[i]->DrawPolyline( pts, 2 );
                      
            }    
      }
  }
  catch (PlayerCc::PlayerError e)
    {
      std::cerr << e << std::endl;
      return -1;
    }
}