astarpp.cpp

一个非常有用的客户端开发程序

#include <iostream>
#include <string>
#include <stdio.h>
#include <stdlib.h>
#include <algorithm>
#include <math.h>
#include "gridwidget.h"
#include "msg_types.h"
#include <vector>
#include <set>
#include "util/gettimeofday.h"
#include "util/timeutil.h"
#include "util/robotId.h"
#include "util/misc.h"
#include "math/src/bresenham.h"
#include "aStarPP.h"
#include "logger/src/logger.h"

static AStarPP *m_ptr = NULL;

#define AStarPP_DEBUG (1)
#define SHORT_DEBUG (0)
// debug like recorder - 1 char per message
#define DEBUG_GEOMETRY (1)
#define DEBUG_GEOMETRY_SHORT (0)
#define DEBUG_LRF (0)
#define DEBUG_ODOMETRY (0)
#define DEBUG_FLEX (0)
#define DEBUG_REACTIVE (0)
#define DEBUG_REACTIVE_SHORT (0)
#define DEBUG_GRID_COMPARE (0)
#define DEBUG_CHOICE (0)
#define ENABLE_HISTOGRAM (1)

// Use Odometry instead of Kalman poses
#define USE_ODOMETRY (1)

#define USE_LRF (1)
#define USE_FLEX (0)
#define LINE_UPDATE_EMPTY (1)

#define PUBLISH_GRID (0)
#define DEBUG_TIMING (1)

#ifdef _WINDOWS
double round(double d)
{
   return static_cast<int>((d < 0.0) ? (d - 0.5) : (d + 0.5));
}
#endif

/**********************************************
 * msgHandler Geometry
 **********************************************/
static void msgHandlerGeometry(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void *clientData)
{
   IPC_RETURN_TYPE err = IPC_OK;
   FORMATTER_PTR formatter;
   rescue_geometry_message msg;

   formatter = IPC_msgInstanceFormatter(msgRef);
   err = IPC_unmarshallData(formatter, callData, &msg,
         sizeof(rescue_geometry_message));
   IPC_freeByteArray(callData);

   if(SHORT_DEBUG) {
      rlogDebug("g");
   }

   if (DEBUG_GEOMETRY) {
      rlogDebug("Got GEOMETRY message from client %d : time: %ld,%ld\n",
            (int)clientData, msg.robot.ts.tv_sec, msg.robot.ts.tv_usec);
      for (int i=0; i<msg.num; i++) {
         rlogDebug("Sensor %d: Position (%f,%f,%f) and Orientation (%f,%f,%f)\n",
               i,msg.x_coords[i],msg.y_coords[i],msg.z_coords[i],
               msg.pitch_angles[i], msg.yaw_angles[i], msg.roll_angles[i]);
      }
      rlogDebug("minRange = %f maxRange = %f viewAngle = %f  type = %d\n",
            msg.minRange, msg.maxRange, msg.viewAngle, msg.type);
   }

   if(msg.num == 0) {
      rlogWarning("Geo Message type: %d with 0 beams received. Not accepting. \n", msg.type);
      return;
   }

   if (m_ptr)
      m_ptr->setGeometry(msg);
}


/**********************************************
 * msgHandler Flex message
 **********************************************/
static void msgHandlerFlex(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void *clientData)
{
   clientData=clientData;
   IPC_RETURN_TYPE err = IPC_OK;
   FORMATTER_PTR formatter;
   rescue_flex_message msg;

   formatter = IPC_msgInstanceFormatter(msgRef);
   err = IPC_unmarshallData(formatter, callData, &msg,
         sizeof(rescue_flex_message));
   IPC_freeByteArray(callData);

   if(SHORT_DEBUG) {
      rlogDebug("f");
   }

   if (DEBUG_FLEX) {
      rlogDebug("Got Flex message from client %d : time: %ld,%ld\n",
            (int)clientData, msg.robot.ts.tv_sec, msg.robot.ts.tv_usec);

      rlogDebug("Values: ");
      for (int i=0; i<msg.num; i++) {
         rlogDebug("%d, ",msg.value[i]);
      }
      rlogDebug("\n");
   }

   if (m_ptr)
      m_ptr->update(msg);
}


/**********************************************
 * msgHandler Target and Choice angles
 **********************************************/
static void msgHandlerChoiceAngle(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void *clientData)
{
   clientData=clientData;
   IPC_RETURN_TYPE err = IPC_OK;
   FORMATTER_PTR formatter;
   rescue_angle_pref_message msg;

   formatter = IPC_msgInstanceFormatter(msgRef);
   err = IPC_unmarshallData(formatter, callData, &msg,
         sizeof(rescue_angle_pref_message));
   IPC_freeByteArray(callData);

   if(SHORT_DEBUG) {
      rlogDebug("tc");
   }

   if (DEBUG_CHOICE) {
      rlogDebug("Got Target and Choice angles message from client %d : time: %ld,%ld\n",
            (int)clientData, msg.robot.ts.tv_sec, msg.robot.ts.tv_usec);

      rlogDebug("Values: ");
      rlogDebug("target  %d, ",msg.target);
      rlogDebug("choice %d, ",msg.choice);
      
      rlogDebug("\n");
   }

    if (m_ptr)
       m_ptr->update(msg);
}

/**********************************************
 * msgHandler LRF message
 **********************************************/
static void msgHandlerLRF(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void *clientData)
{
   clientData=clientData;
   IPC_RETURN_TYPE err = IPC_OK;
   FORMATTER_PTR formatter;
   rescue_rangescan_ranges_message msg;

   formatter = IPC_msgInstanceFormatter(msgRef);
   err = IPC_unmarshallData(formatter, callData, &msg,
         sizeof(rescue_rangescan_ranges_message));
   IPC_freeByteArray(callData);

   if(SHORT_DEBUG) {
      rlogDebug("L");
   }

   if (DEBUG_LRF) {
      rlogDebug("Got LRF message from client %d : time: %ld,%ld\n",
            (int)clientData, msg.robot.ts.tv_sec, msg.robot.ts.tv_usec);

      rlogDebug("Ranges: ");
      for (int i=0; i<msg.nranges; i++) {
         rlogDebug("%f, ",msg.ranges[i]);
      }
      rlogDebug("\n");
   }

   if(msg.nranges > 700) {     // 3D scan.
      if(AStarPP_DEBUG) {
         rlogDebug("Received 3D scan -> ignoring \n");
      }
      delete [] msg.ranges;
      delete [] msg.angles;
      return;
   }

   if (m_ptr)
      m_ptr->update(msg);
}


/*************************************************************************
 * MessageHandler Odometry
 *************************************************************************/
static void msgHandlerOdometry(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void *clientData)
{
   clientData = clientData;
   IPC_RETURN_TYPE err = IPC_OK;
   FORMATTER_PTR formatter;
   rescue_odometry_message msg;

   formatter = IPC_msgInstanceFormatter(msgRef);
   err = IPC_unmarshallData(formatter, callData, &msg, 
         sizeof(rescue_odometry_message));
   IPC_freeByteArray(callData);

   if(SHORT_DEBUG) {
      rlogDebug("o");
   }

   if (DEBUG_ODOMETRY) {
      rlogDebug("Got ODOMETRY message from client %d : time: %ld,%ld\n",
            (int)clientData, msg.robot.ts.tv_sec, msg.robot.ts.tv_usec);
      rlogDebug("Odometry:");
      rlogDebug("%ld,%ld,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",
            msg.robot.ts.tv_sec, 
            msg.robot.ts.tv_usec,
            msg.isMoving,
            msg.posX,
            msg.posZ,
            msg.posTh,
            0.0,       //Here a "dummy" gets read in the logfilereder.
            0.0,       //I dont know what ists good for, so I set 0.0
            msg.transVel,
            msg.rotVel,
            msg.battery);
   }

   if (m_ptr)
      m_ptr->update(msg);
}


AStarPP::AStarPP(GridWidget *gw, int sizeX, int sizeY, double resolution):astar(sizeX,resolution) {
   init(gw, sizeX, sizeY, resolution);
}


AStarPP::AStarPP(int sizeX, int sizeY, double resolution):astar(sizeX,resolution) {
   init(NULL, sizeX, sizeY, resolution);
}

void AStarPP::init(GridWidget *gw, int sizeX, int sizeY, double resolution)
{
   _geometryInfoLRFReceived = false;
   _geometryInfoLRFInitialized = false;
   _flex_hist_initialized = false;
   _scan_hist_initialized = false;

   m_ptr = this;
   _geometryLRF.num=0;
   _lastPosX=_lastPosZ=_lastPosTh=0;
   _firstOdoMsg=true;
   _gw = gw;
   rangeConfidence = 0.95;
   numConsecutiveScanRequests = 0;

   struct timeval current_time;
   gettimeofday(&current_time, NULL);
   _lastFlexFreeTimeLeft.tv_sec = current_time.tv_sec;
   _lastFlexFreeTimeLeft.tv_usec = current_time.tv_usec;
   _lastFlexFreeTimeRight.tv_sec = current_time.tv_sec;
   _lastFlexFreeTimeRight.tv_usec = current_time.tv_usec;
   _maxFlexBlockedTime = 30.0;   // nach 30s flex nicht mehr als blocked werten

   _sizeX = sizeX;
   _sizeY = sizeY;
   _values = new double*[_sizeX];
   for (int i=0; i<_sizeX; i++) {
      _values[i] = new double[_sizeY];
      for (int j=0; j<_sizeY; j++) {
            _values[i][j] = -1;
        
      }
   }
   astar.setOccupancy(_values);

   // Find maxVal
   int maxVal = -INT_MAX;
   int x, y;
   for (x=0; x<_sizeX; x++)
      for (y=0; y<_sizeY; y++)
         if (_values[x][y] > maxVal) 
            maxVal =  (int) _values[x][y];

   _resolution = resolution;
   theta_offset = 0;
   prior_occ = 0.1;
   occ_evidence = 0.8;
   emp_evidence = 0.03;
   max_prob = 0.95;
   min_prob = 0.0001;
   wall_thickness = 100;
   wall_thickness_ir = 200;

   // Prepare client for reading
   int cur_context=0;

   buildTestGrid();		
  PP_Point goal(-49,-49);
 //PP_Point goal(+49,+49);
   
   findPlan(goal); //FIXME
   _gw->setClosedList(astar.getClosedList());
// Update the window
   updateWindow();
}


AStarPP::~AStarPP()
{
   for (int i=0; i<_sizeY; i++)
      delete _values[i];
   delete [] _values;
   
}


/*************************************************************************
 * Set Geometry
 *************************************************************************/
void AStarPP::setGeometry(rescue_geometry_message &msg) 
{
   if(DEBUG_GEOMETRY_SHORT) {
      rlogDebug("GOT Geometry. Type: %d\n", msg.type);
      rlogDebug("    Num: %d\n", msg.num);
   }

   switch (msg.type) {
      case RESCUE_LRF_TYPE:
         _geometryLRF = msg;
         _geometryInfoLRFReceived = true;
         _geometryInfoLRFInitialized = true;
         break;
      case RESCUE_FLEX_TYPE:
         _geometryFlex = msg;
         _geometryInfoFlexReceived = true;
         break;
   }
}

/*************************************************************************
 * Update Choice
 *************************************************************************/

void AStarPP::update(rescue_angle_pref_message &msg)
{
   _gw->setGoalAngles(msg.choice, msg.target);
}

/*************************************************************************
 * Update ODOMETRY POSE
 *************************************************************************/
void AStarPP::update(rescue_odometry_message &msg)
{
   if(!USE_ODOMETRY)
      return;

   double dX = msg.posX - _lastPosX;
   double dZ = msg.posZ - _lastPosZ;
   double dTh = msg.posTh - _lastPosTh;

   _lastPosX = msg.posX;
   _lastPosZ = msg.posZ;
   _lastPosTh = msg.posTh;

   if (_firstOdoMsg) {
      _firstOdoMsg=false;
      return;
   }


   // Make a new Grid
   double **grid = new double*[_sizeX];
   for (int x=0; x<_sizeX; x++) {
      grid[x] = new double[_sizeY];
      for (int y=0; y<_sizeY; y++) {
            grid[x][y] = -1;
      }
   }

   for (int x=0; x<_sizeX; x++) {
      for (int y=0; y<_sizeY; y++) {
         int offsX = - (int) ((double)_sizeX/2.0); 
         int offsY = - (int) ((double)_sizeY/2.0); 

         // real Pos
         double r_posX = _resolution*(x + offsX);
         double r_posY = _resolution*(y + offsY);

         // rotation dTh
         double costh = cos(DEG2RAD(dTh));
         double sinth = sin(DEG2RAD(dTh));
         double r_posX2 = r_posX * costh - r_posY * sinth;
         double r_posY2 = r_posY * costh + r_posX * sinth;

         // rotate dx, dy
         costh = cos(DEG2RAD(msg.posTh));
         sinth = sin(DEG2RAD(msg.posTh));
         double dX2 = dX * costh - dZ * sinth;
         double dY2 = dZ * costh + dX * sinth;

         // translation dX,dY
         double r_posX3 = r_posX2 - dX2;
         double r_posY3 = r_posY2 - dY2;

         // new grid Pos
         int g_posX = (int) round(r_posX3/_resolution) - offsX;
         int g_posY = (int) round(r_posY3/_resolution) - offsY;

         if (g_posX>=0 && g_posX<_sizeX && g_posY>=0 && g_posY<_sizeY)
            grid[g_posX][g_posY] = _values[x][y];
      }
   }

   for(int x=0; x<_sizeX;x++)
      delete [] _values[x];
   delete [] _values;
   _values = grid;

   // Find maxVal
   int maxVal = -INT_MAX;
   for (int x=0; x<_sizeX; x++)
      for (int y=0; y<_sizeY; y++)
         if (_values[x][y] > maxVal) 
            maxVal =  (int) _values[x][y];

//    updateHistogram(); 
//    updateFreeSpace();
   updateWindow();
   //publishGrid();
   //publishReactiveSensorInformation();
   //compareGridWithScan();
}


void AStarPP::update(rescue_flex_message & msg) 
{
   if(AStarPP_DEBUG)
      rlogDebug("Updating Flex \n");

   if (!_geometryInfoFlexReceived) {
      rlogError("Geometry not set!");

      // Trigger geometry message
      rescue_geometry_request_message msg;
      msg.type =  RESCUE_FLEX_TYPE;
      msg.robot.id = getRobotId();
      msg.robot.ts = getCurrentTime();
      IPC_publishData(RESCUE_GEOMETRY_REQUEST_NAME, &msg);
      rlogInfo("GEO Msg triggered!\n");
      return;
   }

   int flex_threshold = 500;

   if(USE_FLEX) {
      //Grab data 
      double *flex_angle = new double[msg.num];
      double *flex_posX = new double[msg.num];
      double *flex_posY = new double[msg.num];
      for (int i = 0; i < msg.num; i++) {
         flex_angle[i] = _geometryFlex.yaw_angles[i];
         flex_posX[i] = _geometryFlex.x_coords[i];
         flex_posY[i] = _geometryFlex.z_coords[i];
      }

      // Calculate Ending Point
      //   theta = DEG2RAD(270 - laser_angle[i]);

      for(int i = 0; i < msg.num; i++) {
         double flex_prob = 0.0;
         if(msg.value[i] > flex_threshold)
            flex_prob = 1.0;
         else
            flex_prob = 0.0;

         int flexXInt = ((int)(_sizeX / 2.0 + (flex_posX[i] / _resolution )));
         int flexYInt = ((int)(_sizeY / 2.0 + (flex_posY[i] / _resolution )));

         int current_x, current_y;
         double new_prob;

         int low_y = -1;
         if(flex_prob < 0.5)
            low_y = 0;