Astar算法机器人路径规划

Astar算法机器人路径规划 Ubuntu,在playerstage仿真。

////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// STL A* Search implementation
// （C）2001 Justin Heyes-Jones
//
// This uses my A* code to solve the 8-puzzle

////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#include 
#include 
#include 

#include 

using namespace std;

// Configuration

#define NUM_TIMES_TO_RUN_SEARCH 1
#define DISPLAY_SOLUTION_FORWARDS 1
#define DISPLAY_SOLUTION_BACKWARDS 0
#define DISPLAY_SOLUTION_INFO 1
#define DEBUG_LISTS 0

// AStar search class
#include “stlastar.h“ // See header for copyright and usage information

// Global data

#define BOARD_WIDTH   （3）
#define BOARD_HEIGHT  （3）

#define GM_TILE     （-1）
#define GM_SPACE	 （0）
#define GM_OFF_BOARD （1）

// Definitions

// To use the search class you must define the following calls...

// Data
//		Your own state space information
// Functions
//		（Optional） Constructor.
//		Nodes are created by the user so whether you use a
//      constructor with parameters as below or just set the object up after the 
//      constructor is up to you.
//
//		（Optional） Destructor. 
//		The destructor will be called if you create one. You 
//		can rely on the default constructor unless you dynamically allocate something in
//		your data
//
//		float GoalDistanceEstimate（ PuzzleState &nodeGoal ）;
//		Return the estimated cost to goal from this node （pass reference to goal node）
//
//		bool IsGoal（ PuzzleState &nodeGoal ）;
//		Return true if this node is the goal.
//
//		bool GetSuccessors（ AStarSearch *astarsearch ）;
//		For each successor to this state call the AStarSearch‘s AddSuccessor call to 
//		add each one to the current search - return false if you are out of memory and the search
//		will fail
//
//		float GetCost（ PuzzleState *successor ）;
//		Return the cost moving from this state to the state of successor
//
//		bool IsSameState（ PuzzleState &rhs ）;
//		Return true if the provided state is the same as this state

// Here the example is the 8-puzzle state ...
class PuzzleState
{

public:

	// defs

	typedef enum
	{
		TL_SPACE
		TL_1
		TL_2
		TL_3
		TL_4
		TL_5
		TL_6
		TL_7
		TL_8

	} TILE;

	// data

	static TILE g_goal[ BOARD_WIDTH*BOARD_HEIGHT];
	static TILE g_start[ BOARD_WIDTH*BOARD_HEIGHT];

	// the tile data for the 8-puzzle
	TILE tiles[ BOARD_WIDTH*BOARD_HEIGHT ];

	// member functions

	PuzzleState（） {  
						memcpy（ tiles g_goal sizeof（ TILE ） * BOARD_WIDTH * BOARD_HEIGHT ）;			
					}

	PuzzleState（ TILE *param_tiles ） 
					{
						memcpy（ tiles param_tiles sizeof（ TILE ） * BOARD_WIDTH * BOARD_HEIGHT ）;			
					}

	float GoalDistanceEstimate（ PuzzleState &nodeGoal ）;
	bool IsGoal（ PuzzleState &nodeGoal ）;
	bool GetSuccessors（ AStarSearch *astarsearch

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

 -rw-r--r--       342  2011-03-10 02:01  8 puzzle Astar\astar.cfg

 -rwxr-xr-x       277  2011-03-14 00:40  8 puzzle Astar\astar.world

 -rw-r--r--     15412  2009-02-16 12:09  8 puzzle Astar\8puzzle.cpp

 -rw-r--r--      8467  2011-03-13 22:20  8 puzzle Astar\astar.cpp

 -rw-r--r--     16411  2011-03-13 23:28  8 puzzle Astar\stlastar.h

 -rw-r--r--     15171  2011-02-15 23:31  8 puzzle Astar\bitmaps\SRI-AIC-kwing.png

 -rw-r--r--      3138  2011-02-15 23:31  8 puzzle Astar\bitmaps\cave.png

 -rw-r--r--      1454  2011-02-15 23:31  8 puzzle Astar\bitmaps\table.png

 -rw-r--r--      1676  2011-03-03 01:03  8 puzzle Astar\bitmaps\circle3.png

 -rw-r--r--    236278  2011-03-03 00:37  8 puzzle Astar\bitmaps\circle2.bmp

 -rw-r--r--       290  2011-02-15 23:31  8 puzzle Astar\bitmaps\Makefile.am

 -rw-r--r--      5598  2011-02-15 23:31  8 puzzle Astar\bitmaps\rink.png

 -rw-r--r--      6754  2011-02-15 23:31  8 puzzle Astar\bitmaps\hospital_section.png

 -rw-r--r--     39605  2011-02-15 23:31  8 puzzle Astar\bitmaps\frieburg.png

 -rw-r--r--    236278  2011-03-02 18:15  8 puzzle Astar\bitmaps\circle.bmp

 -rw-r--r--       745  2011-02-15 23:31  8 puzzle Astar\bitmaps\human_outline.png

 -rw-r--r--      1880  2011-03-02 18:11  8 puzzle Astar\bitmaps\circle.PNG

 -rw-r--r--      3638  2011-02-15 23:31  8 puzzle Astar\bitmaps\ghost.png

 -rw-r--r--       651  2011-02-15 23:31  8 puzzle Astar\bitmaps\cave_compact.png

 -rw-r--r--     44083  2011-02-15 23:31  8 puzzle Astar\bitmaps\hospital.png

 -rw-r--r--    224435  2011-02-15 23:31  8 puzzle Astar\bitmaps\SFU_1200x615.png

 -rw-r--r--      6355  2011-02-15 23:31  8 puzzle Astar\bitmaps\889_05.png

 -rw-r--r--      4267  2011-02-15 23:31  8 puzzle Astar\bitmaps\uoa_robotics_lab.png

 -rw-r--r--      1257  2011-02-15 23:31  8 puzzle Astar\bitmaps\submarine_small.png

 -rw-r--r--      1741  2011-03-03 00:45  8 puzzle Astar\bitmaps\circle2.png

 -rw-r--r--       574  2011-02-15 23:31  8 puzzle Astar\bitmaps\autolab.png

 -rw-r--r--       242  2011-02-15 23:31  8 puzzle Astar\bitmaps\space_invader.png

 -rw-r--r--      1783  2011-02-15 23:31  8 puzzle Astar\bitmaps\sal2.png

 -rw-r--r--      9265  2011-02-15 23:31  8 puzzle Astar\bitmaps\mbicp.png

 -rw-r--r--       736  2011-02-15 23:31  8 puzzle Astar\bitmaps\submarine.png

............此处省略16个文件信息