astarpp.h

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

#ifndef _ASTARPP__
#define _ASTARPP__
#include <sys/time.h>
#include <math.h>
#include<map>
#include<list>
#include "msg_types.h"
#include "ipc/ipc.h"
#include "util/misc.h"
#include "pp_point.h"
#include "astar.h"

using namespace std;

class GridWidget;



class AStarPP {
   public:
      
   private:
      rescue_geometry_message _geometryLRF;
      rescue_geometry_message _geometryFlex;
      bool _geometryInfoLRFReceived;
      bool _geometryInfoLRFInitialized;
      bool _geometryInfoFlexReceived;

      GridWidget *_gw;
      double _lastPosX;
      double _lastPosZ;
      double _lastPosTh;
      bool _firstOdoMsg;
    
     
      // Grid
      double **_values;
      int _sizeX, _sizeY;

      // Probability Grid
      double _sonarMaxRangeValue;   // sonars liefern bei maxRange sehr kleine Zahlen. ab <= dieser Zahl wird sonar als maxRange interpretiert
      double _resolution;
      double theta_offset;
      double prior_occ;
      double occ_evidence, emp_evidence, max_prob, min_prob;
      double occ_evidence_sonar, emp_evidence_sonar;
      double rangeConfidence;
      double wall_thickness, wall_thickness_ir;
      rescue_rangescan_ranges_message _scan_history;

      bool _flex_hist_initialized, _scan_hist_initialized;
      rescue_flex_message _flex_history;
      
     
   public:
      AStarPP(int sizeX, int sizeY, double resolution);
      AStarPP(GridWidget *gw, int sizeX, int sizeY, double resolution);
      ~AStarPP();
      void update(rescue_odometry_message &msg); 
      void update(rescue_rangescan_ranges_message &msg);
      void update(rescue_flex_message & msg);
      void update(rescue_angle_pref_message &msg);
      void setGeometry(rescue_geometry_message &msg);
      double** getDataPtr();

   private:
      int numConsecutiveScanRequests;
      struct timeval _lastFlexFreeTimeLeft;
      struct timeval _lastFlexFreeTimeRight;
      double _maxFlexBlockedTime;
   public:
      double getCellProbability(RESCUE_SENSOR_TYPE type, double range, double dist, double maxRange);
      double getLaserCellProbability(double range, double dist, double maxRange);
      double gauss(double x, double mu, double sigma);
      void rangeUpdate(RESCUE_SENSOR_TYPE type, int num, double ranges[]);  
      void lineUpdate(RESCUE_SENSOR_TYPE type, int x1, int y1, int x2, int y2, double range, double maxRange);
      void updateWindow();
      void updateWallBalance();
      int FlexRightClear();
      int FlexLeftClear();
      void publishGrid();
      void randomizeGrid(); 
      void buildTestGrid();
      void init(GridWidget *gw, int sizeX, int sizeY, double resolution);
      float invSqrt(float x);
      //PathPlanner Stuff
      void findPlan(PP_Point);
      //Plan
      list<PP_Point> _plan;
      AStar astar;      
      
      


};
#endif

/*********************************************************************
 * (C) Copyright 2006 Albert Ludwigs University Freiburg
 *     Institute of Computer Science
 *
 * All rights reserved. Use of this software is permitted for
 * non-commercial research purposes, and it may be copied only
 * for that use. All copies must include this copyright message.
 * This software is made available AS IS, and neither the authors
 * nor the Albert Ludwigs University Freiburg make any warranty
 * about the software or its performance.
 *********************************************************************/

:q
uuu