1、 Download and compile Feature Pack

cd ~/catkin_ ws/srcsudo apt-get install  https://github.com/ros-planning/navigaTIoncd  ..catkin_ make

2、 Documents covered in the function package


Function pack function

Acml location algorithm

move_ The main framework in base navigation

base_ local_ Planner local path planner

dwa_ local_ Implementation of planner DWA algorithm for local path planning

global_ Planner global path planning

Navfn global path planning, old version, bug

carrot_ Planner is a simple global path planner

clear_ costmap_ Recovery clears the recovery behavior of the cost map

costmap_ 2D implementation of 2D cost map

fake_ API interface of localization acml

map_ Server provides map data, yaml or image

move_ slow_ and_ The slow moving repair mechanism of clear will limit the speed of the robot

nav_ Core path planning interface class

rotate_ Recovery rotation recovery

voxel_ Grid 3D cost map

There are three global Planners:

(1)carrot_ planner

carrot_ The planner checks whether the target to be reached is an obstacle. If it is an obstacle, it will replace the target point with a nearby accessible point. Therefore, this module does not do any global planning. This module is not practical in a complex indoor environment.

(2)navfn

Navfn uses the Dijkstra algorithm to find the shortest path.

(3)global planner

Global planner is an upgraded version of navfn.

Compared with navfn, it adds more options: support a * algorithm; Can switch quadratic approximation; Switch mesh path;

3、 Global planner global path planning

Contents under this file: 10 header files and 8 source files

Look at the file of a * algorithm

First, make an example and understand it in combination with the example

The following files will contain other files. You need to look at them as a whole. Here we will sort out three files first, and others will take your time

1、astar.h

#ifndef _ ASTAR_ H#define _ ASTAR_ H#include #include #include #include namespace global_ Planner {class index {public: index (int a, float b) {I = a; cost = B;} int i; float cost;}; / / points and costs corresponding to index (I, cost) struct greater1 {boot operator() (const Index & A, const Index & B) const {return a.cost > b.cost;}}; class astarexpansion: public expander {    public:        AStarExpansion(PotenTIalCalculator* p_calc, int nx, int ny);        bool calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y, int cycles, float* potential);    private:        void add(unsigned char* costs, float* potential, float prev_potential, int next_i, int end_x, int end_y) ;        std::vector queue_;};}

2、astar.cpp

#include#includenamespace global_ Planner {astarexpansion:: astarexpansion (potentialcalculator * p_calc, int XS, int YS): expander (p_calc, XS, YS) {} / / namespace name:: identifier name? / * costs: Map pointer cycles: number of cycles * / bool astarexpansion:: calculatepotentials (unsigned char * costs, double start_x, double start_y, double end_x, double end_y, int cycles, float * potential) / / the parameter {/ / queue is the vector queue searched by the heuristic:queue_. clear();  // Empty function: empty the queue / / put the starting point into the queue int start_ i = toIndex(start_x, start_y);    // (1,2)      //push_ Back: adds the element queue_ to the data structure push_ back(Index(start_i, 0));      // First, add the start point and its cost, that is, (13,0) / / STD:: fill function: assign the elements of an interval to the specified value, that is, fill the specified value in the range of (first, last) / / set the potential of all points to a maximum value, and potential is the estimated value g, f = G + H STD:: fill (potential, potential + ns_, pot_high) ; / /? / / set the potential of the starting point to 0 potential [start_i] = 0; / / the sequence number corresponding to the ending point int goal_i = toindex (end_x, end_y); / / (4,4) Int cycle = 0; / / enter the cycle. The judgment condition for continuing the cycle is that as long as the queue size is greater than 0 and the number of cycles is less than cycles. / / cycles = 2 * NX * NY in the code, that is, twice the number of all cells / /? / / PURPOSE: get the index of the minimum cost and delete it. If the index points to goal (destination), exit the algorithm and return true while (queue_. Size()) >0 & & cycle < cycles) = "" {= "" index = "" top = "queue_ [0];" the first element is placed at the end, and other elements are arranged from small to large according to the cost value = "" pop_heap() = "is to exchange the position of the top element of the heap with the last element, and then use pop_back to delete the last element =" "greater1() is to stack by the small top =" "STD:: pop_heap (queue_. Begin(), =" "queue_. End(), =" "greater1()); =" " Queue_. Pop_back(); = "if you reach the target point, it's over =" "int =" "I =" top. I; "if =" "(I =" = "goal_i) =" "return =" "true; =" execute the add function for the front, back, left and right points, add the points around the least cost point I to the search team and update the generation value = "" add (costs, = "potential, =" potential [i], = "I =" "+ =" 1, = "end_x, =" end_y); = "add (costs, =" potential, = "" Potential [i], = "" I = "" - = "" 1, = "" end_x, = "" end_y); = "" add (costs, = "" potential, = "" potential [i], = "" I = "" + = "" nx_, = "" end_x, = "" end_y); = "" add (costs, = "" potential, = "" potential [i], = "" I = "" - = "" nx_, = "" end_x, = "" end_y); = "" cycle + +; = ""} = "" return = "" false; = ""} = "" add function: add points and update cost function; = "" If it is a point that has been added, it will be ignored. According to the value of costmap, it will also be ignored if it is an obstacle. = "potential [next_i] is the cost from the starting point to the current point, i.e. g (n), =" distance = "" * = "neutral_cost_" is the cost from the current point to the destination point, i.e. H (n). = "f (n) = g (n)" + = "H (n) =" "* / =" potential = "" stores g (n) of all points, i.e. the actual cost from the initial point to node n = Prev_potential = "" F = "" void = "" astarexpansion:: add (unsigned = "" char * = "" costs, = "" float * = "" potential, = "" float = "" prev_potential, = "" int = "" next_i, = "" int = "" end_x, int = "" end_y) = "" {= "" next_i = "" the current point is not in the grid, ignore = "" if = "" (next_i = "" > < 0 = "" _i = "" next_i = "" > = ns_) //Ns_? Return; / / the cost of the point not searched is pot_high. If it is less than this value, it is the searched point and skipped; / / potential [next_i] is the cost from the starting point to the current point, that is, G (n) if (potential [next_i] < pot_high) = "" pot_high? = "return; =" obstacle point, ignore? "=" if (costs [next_i] = "> = lethal_cost_ & &! (unknown & & costs [next_i] = = costmap_2d:: no_information) )Return; / / potential [next_i] is the cost from the starting point to the current point, i.e. g (n) / / potential stores the G (n) of all points, i.e. the actual cost from the initial point to node n. / / prev_potential F of the current point / / ======== see the potential_calculate.cpp file below = ======= potential [next_i] = p_calc_ - > calculatepotential (potential, costs [next_i] +Neutral_cost_, next_i, prev_potential); / / get the (x, y) int x = next_i% nx_, y = next_i / nx_in the grid map; / / x = 14% 5 = 4 y = 14 / 5 = 2? / / heuristic function: that is, H (n) the estimated cost of the best path from node n to the target point. Here, Manhattan distance = ABS (end_x - x) + ABS (end_y - y) is selected ; / / if the I value of a is 13, then the next_i in add is 12, 14, 7 and 19 respectively. / / taking 14 as an example, then x = 2, y = 2. And B is (4,4). Therefore, distance = 2 + 2 = 4 / / because this is only the number of grids and multiplied by the real distance or resolution of each grid, the last H = distance * neutral_cost_ / / so the last f = potential [next_i] +Distance * neutral_cost_ (F = G + H) / / add the current point next_i and the corresponding cost to the queue_.push_back (index (next_i, potential [next_i] + distance * neutral_cost_)); / / potential [next_i]: it is the cost from the starting point to the current point, i.e. g (n) / / distance * neutral_cost_: it is the cost from the current point to the destination point, i.e. H (n). / / F (n) = g (n) + H (n) : after calculating the two costs, add them up to f (n) and store them in the queue. / / sort the added ones in the heap and put the lowest cost point in the front queue 0] / / data structure? STD:: push_heap (queue_. Begin(), queue_. End(), greater1());}}

3、potential_ calculate.h

neutral_ cost_ Set a default value of 50

Calculatepotential() calculates use_ There are two choices for the value of rectangular:

If true, conic calculation is used

If false, use a simple method to calculate, return prev_ potential + cost。 Namely: costs [next_i] + neutral_ cost_+ prev_ potential

Map cost + single grid distance cost (initialized to 50) + previous path cost is g

 

#ifndef _ POTENTIAL_ CALCULATOR_ H#define _ POTENTIAL_ CALCULATOR_ H#include namespace global_ planner {class PotentialCalculator {    public:        PotentialCalculator(int nx, int ny)         {            setSize(nx, ny);        }        virtual float calculatePotential(float* potential, unsigned char cost, int n, float prev_potential=-1)        {            if(prev_potential < 0)="" {="" float="" min_h="std::min(" potential[n="" -="" 1] , = "potential [n =" "+ =" 1] = ""), = "min_v =" STD:: min ("potential [n =" "- =" nx_], = "potential [n =" "+ =" nx_]); = "prev_potential =" STD:: min (min_h, "min_v); =" "} =" "return =" "prev_potential =" "+ =" cost; = ""} = "" when f at the current point of prev_potential is not less than 0, prev_potential = "" + = "cost =" " Take start_cost as an example. The initial value assigned to it is 0, so the return is cost, which can be understood as the distance to the next grid or the resolution. = "" after calculating the potential = "g value, start to calculate the H value, that is, distance, using how many grids to move, which can only move up, down, left and right." virtual = "" void = "" SetSize (int = "" NX, = "int =" "NY) =" "{=" "nx_ =" NX; "ny_ =" NY; "  ns_="nx" *="" ny;="" }="" protected:="" inline="" int="" toindex(int="" x,="" int="" y)="" {="" return="" x="" +="" nx_="" *="" y;="" }="" int="" nx_,="" ny_,="" ns_;="" };="" }="">

Leave a Reply

Your email address will not be published. Required fields are marked *