laserobstacleavoid.cc

机器人障碍物避免和路径规划

#include <stdio.h>
#include <string.h> /* for strcpy() */
#include <stdlib.h>  /* for atoi(3),rand(3) */
//#include <libplayerc++/playerclient.h>  //修改内容，本身有＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊

#include <libplayerc++/playerc++.h> //添加内容

#include <time.h>

#include <iostream> //添加内容
#include "args.h" //添加内容
#define RAYS 32 //添加内容

#include <sys/types.h>
#include <unistd.h>

#include <signal.h>


#include "GridMap.h"




#define LASER_CELL_DEPTH 1
#define PI 3.1415926


using namespace PlayerCc;  //添加内容


// ============================    Global variables   =====================
bool turnOnMotors = false;
char host[256] = "localhost";
int  port = PLAYER_PORTNUM;
char auth_key[PLAYER_KEYLEN];

bool knownNBV = false;  



// ====== variables used to store Laser Scan Data
double laser_scan_range[380];
double laser_scan_beamangle[380];
double laser_scan_x[380];
double laser_scan_y[380];

//====== variables used to describe the robot's world position
double RobotWorldX;
double RobotWorldY;
double RobotWorldHeading;//in radian

//====== variables used to describe the robot's speed
double speed;
double turnrate;//in radian



unsigned int XCellsNum;
unsigned int YCellsNum;

double *CellsOP;// one pointer of a dynamic memory area,used in GetNBV.h

// pointer of a dynamic memory area,used to store all of cells' value to
// generate the grid map in the function SaveExit()

DROSGridCellType *GlobalCellsValue;

extern DROSGridCellType *CellsMapValue;

//==================     Global functions    ================
//void DspRemoteLaserData(LaserProxy& lp);
//void DspLocalLaserData(LaserProxy& lp, double range[],double angle[],double x[],double y[]);

void Exit_CtrlC(int sig);
void SaveExit(int sig);
void FillCells (GridMap *map, LaserProxy &lp, Position2dProxy &pp);
void StoreCellsOP(double *CellsOP);
// ==========================================================

  double xmin = -10.0;  //-10.0;
  double ymin = -10.0;  //-10.0;
  double xmax = 30.0;//30.0; //50.0;
  double ymax = 50.0; //50.0;
  double gridcell_width = 0.05; //0.05;


  GridMap *map = new GridMap("anonymous", xmin, ymin, xmax, ymax,
    			       gridcell_width);



int main(int argc, char** argv)
{  
  map -> Fill(1.0);

  XCellsNum = map -> GetXCellsNum();
  YCellsNum = map -> GetYCellsNum();

  CellsOP = (double *) malloc ( XCellsNum * YCellsNum * sizeof(double));
  if(CellsOP == NULL)
  {
    printf("Sorry! cannot distribute the dynamic memorry!\n");
    exit(1);
  }
  printf("\nHello, the pointer of the memorry CellsOP is %p!\n", CellsOP);



//======  Connect to Player server and test if it is connected correctly  ======//
  PlayerClient robot(host,port);
/*  if(strlen(auth_key))                 //此部分是修改的，本身是程序的部分＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊
  {
    if(robot.Authenticate(auth_key))                 //在PlayerClient这个类中没有方法Authenticate（）＊＊＊＊＊＊＊＊＊＊＊＊
    {
      puts("Authentication failed.");
      exit(1);
    }
  }
*/

//======  Connect to LaserProxy and test if it is connected correctly  ======//
//Access mode can be set 'a'(all) 'r'(read) 'w'(write) 'c'(close)
//if get 'e'(error) using GetAccess()it is not connected correctly
//User can use ChangeAccess() to change the access mode for the device later

  LaserProxy lp(&robot,0);
/*  if(lp.GetAccess() == 'e') {                   //此部分是修改的，本身是程序的部分＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊
      puts("Error getting laser device access!");
      exit(1);
    }
  lp.ChangeAccess('r'); //not necessary to change the access mode
*/

//======  Connect to PositionProxy and test if it is connected correctly  ======//
   Position2dProxy pp(&robot,0);  //  以修改，原来是(&robot,0,'a')
/*  if(pp.GetAccess() == 'e') {               //此部分是修改的，本身是程序的部分＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊
    puts("Error getting position device access!");
    exit(1);
  }
*/

//======  some other relative settings  ======//

  /* maybe turn on the motors */
   turnOnMotors=true;                //添加内容＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊
 /*if(turnOnMotors && pp.SetMotorEnable(1)) //此部分是修改的，本身是程序的部分;turnOnMotors始终是faulse；
                                                    //也不存在SetMotorEnable这个方法
   exit(1);
 */
  /* set the odometry to (1.0,1.0,0.0)*/
  pp.SetOdometry(-7.0,-7.0,0.0);  //已修改，原来是(1.0,1.0,0.0);这是设置生成的"grid.map"文件中白色区域相对于黑色区域的
                                         //坐标转向和起始位置
 


  for(;;)
  {  
    /* this blocks until new data comes; 10Hz by default */
    robot.Read();



//======  引用laserobstacleavoid.cc文件中的内容完成避碰  ======//
       double newspeed = 1.0;
      double newturnrate = 0;
      double minR = 1e9;
      double minL = 1e9;

      uint count = lp.GetCount();
      for (uint j=0; j < count/2; ++j)
      {
        if (minR > lp[j])
          minR = lp[j];
      }
      for (uint j = count/2; j < count; ++j)
      {
        if (minL > lp[j])
          minL = lp[j];
      }
 
      double l = (1e5*minR)/500-100;
      double r = (1e5*minL)/500-100;

      if (l > 100)
        l = 100;
      if (r > 100)
        r = 100;

      newspeed = (r+l)/1e3;

      newturnrate = (r-l);
      newturnrate = limit(newturnrate, -40.0, 40.0);
      newturnrate = dtor(newturnrate);
  

// Getting laser scan data from LaserProxy and store them in the global variables
    for(uint i = 0; i <= lp.GetCount();i++)
    {
      laser_scan_range[i] = lp.GetRange(i);
      laser_scan_beamangle[i] = lp.GetBearing(i);  //这个角度是相对于局部坐标系下的角度，单位是弧度
      laser_scan_x[i] = lp.GetPoint(i).px;         //这里得到的坐标（x,y）是相对于局部坐标系的坐标；机器人的朝向是x方向
      laser_scan_y[i] = lp.GetPoint(i).py;
      printf("i=%d  angle=%f  r=%f  x=%f  y=%f\n",i,laser_scan_beamangle[i],laser_scan_range[i],laser_scan_x[i],
       laser_scan_y[i]);
    }

// ==============================================================================
// Geting the robot's current world position from PositionProxy and store them in
// global variable; maybe it should be gotten from Localization modle to get more
// accurate position information which will be used to generate the grid map
     RobotWorldX = pp.GetXPos();
     RobotWorldY = pp.GetYPos();
     RobotWorldHeading = pp.GetYaw(); //in radian

     pp.SetSpeed(newspeed,newturnrate);   //设置上面用到的转速newturnrate

     FillCells(map,lp,pp);
 

     signal(SIGINT, SaveExit);

  }

  return(0);
}





//==============  when press Ctrl+C to save the data to grid.map  =============

void SaveExit(int sig)
{
      int i;
      int j;

      FILE *f;

      StoreCellsOP(CellsOP);


      //MoveLocaltoGlobal(); // add the information gotten since the last grid map
                           // generated to the dynamic memorry *GlobalCellsValue

      printf("\nYou pressed CTRL+C, the global map---grid.map is being generated...\n");
     



      f = fopen("grid.map", "w");

      if (f == NULL)
      {
        printf( "Failed to open the file!\n");
      }

      fprintf(f, "P2\n%i %i\n%i\n",XCellsNum, YCellsNum, (int) DROSGridCellMax);//原来是"P2\n%i %i\n%i\n"

      for (j = YCellsNum-1; j >= 0; j--)
      {
        for (i = 0; (unsigned)i < XCellsNum; i++) {
          fprintf(f, "%i ", (int)(DROSGridCellMax-CellsOP [j * XCellsNum + i]));         //已修改原来是
                                                                    //(int) (DROSGridCellMax -\CellsOP [j * XCellsNum + i])
        }
        fprintf(f, "\n");
      }

      fclose(f);



      free(CellsOP);
      exit(EXIT_SUCCESS);
}
//====================================================================

double LaserFunc(double radiusfrac, double anglefrac, double *rad) {
  if (radiusfrac < *rad)
    return -0.45;//-0.1;
  else
    return 0.45;//0.2;
}

//=======================================================================

double EmptyFunc(double radiusfrac, double anglefrac, double *rad) {
  if (radiusfrac < *rad)
    return -0.5;//-0.05;
  else
    return 0.0;
}
// ========================================================================


void FillCells (GridMap *mymap, LaserProxy &lp, Position2dProxy &pp)
{
  double wx, wy, wa;
  double vx, vy, va;
  unsigned int i;
  double a;
  double r;
  double dx, dy;
  double cx, cy, ca;
  double frac;
  double affected;
  double g;

        g = mymap->GetGridCellWidth();

        wx = RobotWorldX;
	wy = RobotWorldY;
	wa = RobotWorldHeading;


	for (i = 0; i < lp.GetCount(); i++)
	{
	  dx = laser_scan_x[i];
	  dy = laser_scan_y[i];

	  a = atan2(dy, dx);
	  r = hypot(dx, dy);

	  cx = wx;
	  cy = wy;
	  ca = wa+a;

	  affected = r + LASER_CELL_DEPTH * g;
	  frac = r / affected;

	  if (laser_scan_range[i]< 5.6)
	  {
	    mymap->AddReading(cx, cy, ca, affected, deg2rad(0.5),
			    (ProbFunc) LaserFunc, &frac);
	  } else
	    {
	      mymap->AddReading(cx, cy, ca, affected, deg2rad(0.5),
			    (ProbFunc) EmptyFunc, &frac);
	    }
	}




//==========  test GridMap class' functions   =============================
/*
    printf("\n\nGridCellWidth = %f\n", mymap -> GetGridCellWidth());
    printf("xMin_ = %f\n", mymap -> GetXMin());
    printf("xMax_ = %f\n", mymap -> GetXMax());
    printf("yMin_ = %f\n", mymap -> GetYMin());
    printf("yMax_ = %f\n", mymap -> GetYMax());

    printf("XCellsNum_ = %d\n", mymap -> GetXCellsNum());
    printf("YCellsNum_ = %d\n", mymap -> GetYCellsNum());
*/
    //mymap -> CircleFill(10.0,10.0,2.0,0.0);

//=======================================================================

}


//====================================================================
void StoreCellsOP(double *CellsOP)
{
   unsigned i,j;

  // DROSGridMap *map;
   //map = (DROSGridMap *) loop->FindComponent("DROSGridMap", NULL);
   for(j = 0; j < YCellsNum; j++)
        for(i = 0; i < XCellsNum; i++)
           CellsOP[j* XCellsNum + i] = (map -> Get(i,j)* DROSGridCellMax);
}