2021-04-06

**ROS与Navigation-编写自定义A*算法全局路径规划教程**

向ROS中添加一个新的全局路径规划器，新的路径规划器必须遵守nav_core包中定义的nav_core::BaseGlobalPlanner的C++接口。一旦编写了全局路径规划器，它必须作为插件添加到ROS中，以便它可以被move_base包使用。本例程以turtlebot作为机器人平台，部署A*路径规划算法。

1.创建新的package

cd catkin_ws/src

catkin_create_pkg relaxed_astar nav_core roscpp rospy std_msgs

2.进入catkin_ws/src/relaxed_astar/src目录下，创建RAstar_ros.h文件以及RAstar_ros.cpp文件（代码源文件地址为https://github.com/coins-lab/relaxed_astar 下面我也会给出）

该目录下打开终端，输入编辑命令

gedit RAstar_ros.h

将以下代码复制到文件中

/* iPath: A C++ Library of Intelligent Global Path Planners for Mobile Robots with ROS Integration. * Website: http://www.iroboapp.org/index.php?title=IPath* Contact: ** Copyright (c) 2014* Owners: Al-Imam University/King AbdulAziz Center for Science and Technology (KACST)/Prince Sultan University* All rights reserved.** License Type: GNU GPL**   This program is free software: you can redistribute it and/or modify*   it under the terms of the GNU General Public License as published by*   the Free Software Foundation, either version 3 of the License, or*   (at your option) any later version.*   This program is distributed in the hope that it will be useful,*   but WITHOUT ANY WARRANTY; without even the implied warranty of*   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the*   GNU General Public License for more details.**   You should have received a copy of the GNU General Public License*   along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h>
#include <string>/** include ros libraries**********************/
#include <ros/ros.h>#include <actionlib/client/simple_action_client.h>
#include <move_base_msgs/MoveBaseAction.h>#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <move_base_msgs/MoveBaseGoal.h>
#include <move_base_msgs/MoveBaseActionGoal.h>#include "sensor_msgs/LaserScan.h"
#include "sensor_msgs/PointCloud2.h"#include <nav_msgs/Odometry.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/GetPlan.h>#include <tf/tf.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
/** ********************************************/ #include <boost/foreach.hpp>
//#define forEach BOOST_FOREACH/** for global path planner interface */
#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_2d/costmap_2d.h>
#include <nav_core/base_global_planner.h>#include <geometry_msgs/PoseStamped.h>
#include <angles/angles.h>//#include <pcl_conversions/pcl_conversions.h>
#include <base_local_planner/world_model.h>
#include <base_local_planner/costmap_model.h>#include <set>
using namespace std;
using std::string;#ifndef RASTAR_ROS_CPP
#define RASTAR_ROS_CPP/*** @struct cells* @brief A struct that represents a cell and its fCost.*/
struct cells {int currentCell;float fCost;};      namespace RAstar_planner {class RAstarPlannerROS : public nav_core::BaseGlobalPlanner {
public:RAstarPlannerROS (ros::NodeHandle &); //this constructor is may be not neededRAstarPlannerROS ();RAstarPlannerROS(std::string name, costmap_2d::Costmap2DROS* costmap_ros);ros::NodeHandle ROSNodeHandle;/** overriden classes from interface nav_core::BaseGlobalPlanner **/void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);bool makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan);void getCorrdinate (float& x, float& y);int convertToCellIndex (float x, float y);void convertToCoordinate(int index, float& x, float& y);bool isCellInsideMap(float x, float y);void mapToWorld(double mx, double my, double& wx, double& wy);vector<int> RAstarPlanner(int startCell, int goalCell);vector<int> findPath(int startCell, int goalCell, float g_score[]);vector<int> constructPath(int startCell, int goalCell, float g_score[]);float calculateHCost(int cellID, int goalCell){int x1=getCellRowID(goalCell);int y1=getCellColID(goalCell);int x2=getCellRowID(cellID);int y2=getCellColID(cellID);return abs(x1-x2)+abs(y1-y2);//return min(abs(x1-x2),abs(y1-y2))*sqrt(2) + max(abs(x1-x2),abs(y1-y2))-min(abs(x1-x2),abs(y1-y2));}void addNeighborCellToOpenList(multiset<cells> & OPL, int neighborCell, int goalCell, float g_score[]);vector <int> findFreeNeighborCell (int CellID);bool isStartAndGoalCellsValid(int startCell,int goalCell); float getMoveCost(int CellID1, int CellID2);float getMoveCost(int i1, int j1, int i2, int j2);bool isFree(int CellID); //returns true if the cell is Freebool isFree(int i, int j); int getCellIndex(int i,int j) //get the index of the cell to be used in Path{return (i*width)+j;  }int getCellRowID(int index)//get the row ID from cell index{return index/width;}int getCellColID(int index)//get colunm ID from cell index{return index%width;}float originX;float originY;float resolution;costmap_2d::Costmap2DROS* costmap_ros_;double step_size_, min_dist_from_robot_;costmap_2d::Costmap2D* costmap_;//base_local_planner::WorldModel* world_model_;bool initialized_;int width;int height;
};};
#endif

重新输入编辑命令：

gedit RAstar_ros.cpp

复制如下代码到该文件中：

/* iPath: A C++ Library of Intelligent Global Path Planners for Mobile Robots with ROS Integration. * Website: http://www.iroboapp.org/index.php?title=IPath* Contact: ** Copyright (c) 2014* Owners: Al-Imam University/King AbdulAziz Center for Science and Technology (KACST)/Prince Sultan University* All rights reserved.** License Type: GNU GPL**   This program is free software: you can redistribute it and/or modify*   it under the terms of the GNU General Public License as published by*   the Free Software Foundation, either version 3 of the License, or*   (at your option) any later version.*   This program is distributed in the hope that it will be useful,*   but WITHOUT ANY WARRANTY; without even the implied warranty of*   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the*   GNU General Public License for more details.**   You should have received a copy of the GNU General Public License*   along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h>
#include <fstream>
#include <iostream>
#include <iomanip>
#include <string>#include "RAstar_ros.h"#include <pluginlib/class_list_macros.h>
//register this planner as a BaseGlobalPlanner plugin
PLUGINLIB_EXPORT_CLASS(RAstar_planner::RAstarPlannerROS, nav_core::BaseGlobalPlanner)int value;
int mapSize;
bool* OGM;
static const float INFINIT_COST = INT_MAX; //!< cost of non connected nodes
float infinity = std::numeric_limits< float >::infinity();
float tBreak;  // coefficient for breaking ties
ofstream MyExcelFile ("RA_result.xlsx", ios::trunc);int clock_gettime(clockid_t clk_id, struct timespect *tp);timespec diff(timespec start, timespec end)
{timespec temp;if ((end.tv_nsec-start.tv_nsec)<0) {temp.tv_sec = end.tv_sec-start.tv_sec-1;temp.tv_nsec = 1000000000+end.tv_nsec-start.tv_nsec;} else {temp.tv_sec = end.tv_sec-start.tv_sec;temp.tv_nsec = end.tv_nsec-start.tv_nsec;}return temp;
}inline vector <int> findFreeNeighborCell (int CellID);namespace RAstar_planner
{//Default Constructor
RAstarPlannerROS::RAstarPlannerROS()
{}
RAstarPlannerROS::RAstarPlannerROS(ros::NodeHandle &nh)
{ROSNodeHandle = nh;}RAstarPlannerROS::RAstarPlannerROS(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
{initialize(name, costmap_ros);
}void RAstarPlannerROS::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
{if (!initialized_){costmap_ros_ = costmap_ros;costmap_ = costmap_ros_->getCostmap();ros::NodeHandle private_nh("~/" + name);originX = costmap_->getOriginX();originY = costmap_->getOriginY();width = costmap_->getSizeInCellsX();height = costmap_->getSizeInCellsY();resolution = costmap_->getResolution();mapSize = width*height;tBreak = 1+1/(mapSize); value =0;OGM = new bool [mapSize]; for (unsigned int iy = 0; iy < costmap_->getSizeInCellsY(); iy++){for (unsigned int ix = 0; ix < costmap_->getSizeInCellsX(); ix++){unsigned int cost = static_cast<int>(costmap_->getCost(ix, iy));//cout<<cost;if (cost == 0)OGM[iy*width+ix]=true;elseOGM[iy*width+ix]=false;}}MyExcelFile << "StartID\tStartX\tStartY\tGoalID\tGoalX\tGoalY\tPlannertime(ms)\tpathLength\tnumberOfCells\t" << endl;ROS_INFO("RAstar planner initialized successfully");initialized_ = true;}elseROS_WARN("This planner has already been initialized... doing nothing");
}bool RAstarPlannerROS::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,std::vector<geometry_msgs::PoseStamped>& plan)
{if (!initialized_){ROS_ERROR("The planner has not been initialized, please call initialize() to use the planner");return false;}ROS_DEBUG("Got a start: %.2f, %.2f, and a goal: %.2f, %.2f", start.pose.position.x, start.pose.position.y,goal.pose.position.x, goal.pose.position.y);plan.clear();if (goal.header.frame_id != costmap_ros_->getGlobalFrameID()){ROS_ERROR("This planner as configured will only accept goals in the %s frame, but a goal was sent in the %s frame.",costmap_ros_->getGlobalFrameID().c_str(), goal.header.frame_id.c_str());return false;}tf::Stamped < tf::Pose > goal_tf;tf::Stamped < tf::Pose > start_tf;poseStampedMsgToTF(goal, goal_tf);poseStampedMsgToTF(start, start_tf);// convert the start and goal positionsfloat startX = start.pose.position.x;float startY = start.pose.position.y;float goalX = goal.pose.position.x;float goalY = goal.pose.position.y;getCorrdinate(startX, startY);getCorrdinate(goalX, goalY);int startCell;int goalCell;if (isCellInsideMap(startX, startY) && isCellInsideMap(goalX, goalY)){startCell = convertToCellIndex(startX, startY);goalCell = convertToCellIndex(goalX, goalY);MyExcelFile << startCell <<"\t"<< start.pose.position.x <<"\t"<< start.pose.position.y <<"\t"<< goalCell <<"\t"<< goal.pose.position.x <<"\t"<< goal.pose.position.y;}else{ROS_WARN("the start or goal is out of the map");return false;}/// call global plannerif (isStartAndGoalCellsValid(startCell, goalCell)){vector<int> bestPath;bestPath.clear();bestPath = RAstarPlanner(startCell, goalCell);//if the global planner find a pathif ( bestPath.size()>0){// convert the pathfor (int i = 0; i < bestPath.size(); i++){float x = 0.0;float y = 0.0;int index = bestPath[i];convertToCoordinate(index, x, y);geometry_msgs::PoseStamped pose = goal;pose.pose.position.x = x;pose.pose.position.y = y;pose.pose.position.z = 0.0;pose.pose.orientation.x = 0.0;pose.pose.orientation.y = 0.0;pose.pose.orientation.z = 0.0;pose.pose.orientation.w = 1.0;plan.push_back(pose);}float path_length = 0.0;std::vector<geometry_msgs::PoseStamped>::iterator it = plan.begin();geometry_msgs::PoseStamped last_pose;last_pose = *it;it++;for (; it!=plan.end(); ++it) {path_length += hypot(  (*it).pose.position.x - last_pose.pose.position.x, (*it).pose.position.y - last_pose.pose.position.y );last_pose = *it;}cout <<"The global path length: "<< path_length<< " meters"<<endl;MyExcelFile << "\t" <<path_length <<"\t"<< plan.size() <<endl;//publish the planreturn true;}else{ROS_WARN("The planner failed to find a path, choose other goal position");return false;}}else{ROS_WARN("Not valid start or goal");return false;}}
void RAstarPlannerROS::getCorrdinate(float& x, float& y)
{x = x - originX;y = y - originY;}int RAstarPlannerROS::convertToCellIndex(float x, float y)
{int cellIndex;float newX = x / resolution;float newY = y / resolution;cellIndex = getCellIndex(newY, newX);return cellIndex;
}void RAstarPlannerROS::convertToCoordinate(int index, float& x, float& y)
{x = getCellColID(index) * resolution;y = getCellRowID(index) * resolution;x = x + originX;y = y + originY;}bool RAstarPlannerROS::isCellInsideMap(float x, float y)
{bool valid = true;if (x > (width * resolution) || y > (height * resolution))valid = false;return valid;
}void RAstarPlannerROS::mapToWorld(double mx, double my, double& wx, double& wy){costmap_2d::Costmap2D* costmap = costmap_ros_->getCostmap();wx = costmap->getOriginX() + mx * resolution;wy = costmap->getOriginY() + my * resolution;
}vector<int> RAstarPlannerROS::RAstarPlanner(int startCell, int goalCell){vector<int> bestPath;//float g_score [mapSize][2];
float g_score [mapSize];for (uint i=0; i<mapSize; i++)g_score[i]=infinity;timespec time1, time2;/* take current time here */clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time1);bestPath=findPath(startCell, goalCell,  g_score);clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time2);cout<<"time to generate best global path by Relaxed A* = " << (diff(time1,time2).tv_sec)*1e3 + (diff(time1,time2).tv_nsec)*1e-6 << " microseconds" << endl;MyExcelFile <<"\t"<< (diff(time1,time2).tv_sec)*1e3 + (diff(time1,time2).tv_nsec)*1e-6 ;return bestPath;}/*******************************************************************************/
//Function Name: findPath
//Inputs: the map layout, the start and the goal Cells and a boolean to indicate if we will use break ties or not
//Output: the best path
//Description: it is used to generate the robot free path
/*********************************************************************************/
vector<int> RAstarPlannerROS::findPath(int startCell, int goalCell, float g_score[])
{value++;vector<int> bestPath;vector<int> emptyPath;cells CP;multiset<cells> OPL;int currentCell;//calculate g_score and f_score of the start positiong_score[startCell]=0;CP.currentCell=startCell;CP.fCost=g_score[startCell]+calculateHCost(startCell,goalCell);//add the start cell to the open listOPL.insert(CP);currentCell=startCell;//while the open list is not empty continuie the search or g_score(goalCell) is equal to infinitywhile (!OPL.empty()&& g_score[goalCell]==infinity) {//choose the cell that has the lowest cost fCost in the open set which is the begin of the multisetcurrentCell = OPL.begin()->currentCell;//remove the currentCell from the openListOPL.erase(OPL.begin());//search the neighbors of the current Cellvector <int> neighborCells; neighborCells=findFreeNeighborCell(currentCell);for(uint i=0; i<neighborCells.size(); i++) //for each neighbor v of current cell{// if the g_score of the neighbor is equal to INF: unvisited cellif(g_score[neighborCells[i]]==infinity){g_score[neighborCells[i]]=g_score[currentCell]+getMoveCost(currentCell,neighborCells[i]);addNeighborCellToOpenList(OPL, neighborCells[i], goalCell, g_score); }//end if}//end for}//end whileif(g_score[goalCell]!=infinity)  // if g_score(goalcell)==INF : construct path {bestPath=constructPath(startCell, goalCell, g_score);return   bestPath; }else{cout << "Failure to find a path !" << endl;return emptyPath;}
}/*******************************************************************************/
//Function Name: constructPath
//Inputs: the start and the goal Cells
//Output: the best path
//Description: it is used to construct the robot path
/*********************************************************************************/
vector<int> RAstarPlannerROS::constructPath(int startCell, int goalCell,float g_score[])
{vector<int> bestPath;vector<int> path;path.insert(path.begin()+bestPath.size(), goalCell);int currentCell=goalCell;while(currentCell!=startCell){ vector <int> neighborCells;neighborCells=findFreeNeighborCell(currentCell);vector <float> gScoresNeighbors;for(uint i=0; i<neighborCells.size(); i++)gScoresNeighbors.push_back(g_score[neighborCells[i]]);int posMinGScore=distance(gScoresNeighbors.begin(), min_element(gScoresNeighbors.begin(), gScoresNeighbors.end()));currentCell=neighborCells[posMinGScore];//insert the neighbor in the pathpath.insert(path.begin()+path.size(), currentCell);}for(uint i=0; i<path.size(); i++)bestPath.insert(bestPath.begin()+bestPath.size(), path[path.size()-(i+1)]);return bestPath;
}/*******************************************************************************/
//Function Name: calculateHCost
//Inputs:the cellID and the goalCell
//Output: the distance between the current cell and the goal cell
//Description: it is used to calculate the hCost
/*********************************************************************************/
/*
float RAstarPlannerROS::calculateHCost(int cellID, int goalCell)
{    int x1=getCellRowID(goalCell);int y1=getCellColID(goalCell);int x2=getCellRowID(cellID);int y2=getCellColID(cellID);//if(getNeighborNumber()==4) //The diagonal shortcut distance between two grid points (x1,y1) and (x2,y2) is://  return min(abs(x1-x2),abs(y1-y2))*sqrt(2) + max(abs(x1-x2),abs(y1-y2))-min(abs(x1-x2),abs(y1-y2));//else//manhatten distance for 8 neighborreturn abs(x1-x2)+abs(y1-y2);
}
*/
/*******************************************************************************/
//Function Name: addNeighborCellToOpenList
//Inputs: the open list, the neighbors Cell, the g_score matrix, the goal cell
//Output:
//Description: it is used to add a neighbor Cell to the open list
/*********************************************************************************/
void RAstarPlannerROS::addNeighborCellToOpenList(multiset<cells> & OPL, int neighborCell, int goalCell, float g_score[])
{cells CP;CP.currentCell=neighborCell; //insert the neighbor cellCP.fCost=g_score[neighborCell]+calculateHCost(neighborCell,goalCell);OPL.insert(CP);//multiset<cells>::iterator it = OPL.lower_bound(CP);//multiset<cells>::iterator it = OPL.upper_bound(CP);//OPL.insert( it, CP  );
}/******************************************************************************** Function Name: findFreeNeighborCell* Inputs: the row and columun of the current Cell* Output: a vector of free neighbor cells of the current cell* Description:it is used to find the free neighbors Cells of a the current Cell in the grid* Check Status: Checked by Anis, Imen and Sahar
*********************************************************************************/vector <int> RAstarPlannerROS::findFreeNeighborCell (int CellID){int rowID=getCellRowID(CellID);int colID=getCellColID(CellID);int neighborIndex;vector <int>  freeNeighborCells;for (int i=-1;i<=1;i++)for (int j=-1; j<=1;j++){//check whether the index is validif ((rowID+i>=0)&&(rowID+i<height)&&(colID+j>=0)&&(colID+j<width)&& (!(i==0 && j==0))){neighborIndex = getCellIndex(rowID+i,colID+j);if(isFree(neighborIndex) )freeNeighborCells.push_back(neighborIndex);}}return  freeNeighborCells;}/*******************************************************************************/
//Function Name: isStartAndGoalCellsValid
//Inputs: the start and Goal cells
//Output: true if the start and the goal cells are valid
//Description: check if the start and goal cells are valid
/*********************************************************************************/
bool RAstarPlannerROS::isStartAndGoalCellsValid(int startCell,int goalCell)
{ bool isvalid=true;bool isFreeStartCell=isFree(startCell);bool isFreeGoalCell=isFree(goalCell);if (startCell==goalCell){//cout << "The Start and the Goal cells are the same..." << endl; isvalid = false;}else{if (!isFreeStartCell && !isFreeGoalCell){//cout << "The start and the goal cells are obstacle positions..." << endl;isvalid = false;}else{if (!isFreeStartCell){//cout << "The start is an obstacle..." << endl;isvalid = false;}else{if(!isFreeGoalCell){//cout << "The goal cell is an obstacle..." << endl;isvalid = false;}else{if (findFreeNeighborCell(goalCell).size()==0){//cout << "The goal cell is encountred by obstacles... "<< endl;isvalid = false;}else{if(findFreeNeighborCell(startCell).size()==0){//cout << "The start cell is encountred by obstacles... "<< endl;isvalid = false;}}}}}}return isvalid;
}float  RAstarPlannerROS::getMoveCost(int i1, int j1, int i2, int j2){float moveCost=INFINIT_COST;//start cost with maximum value. Change it to real cost of cells are connected//if cell2(i2,j2) exists in the diagonal of cell1(i1,j1)if((j2==j1+1 && i2==i1+1)||(i2==i1-1 && j2==j1+1) ||(i2==i1-1 && j2==j1-1)||(j2==j1-1 && i2==i1+1)){//moveCost = DIAGONAL_MOVE_COST;moveCost = 1.4;}//if cell 2(i2,j2) exists in the horizontal or vertical line with cell1(i1,j1)else{if ((j2==j1 && i2==i1-1)||(i2==i1 && j2==j1-1)||(i2==i1+1 && j2==j1) ||(i1==i2 && j2==j1+1)){//moveCost = MOVE_COST;moveCost = 1;}}return moveCost;} float  RAstarPlannerROS::getMoveCost(int CellID1, int CellID2){int i1=0,i2=0,j1=0,j2=0;i1=getCellRowID(CellID1);j1=getCellColID(CellID1);i2=getCellRowID(CellID2);j2=getCellColID(CellID2);return getMoveCost(i1, j1, i2, j2);} //verify if the cell(i,j) is freebool  RAstarPlannerROS::isFree(int i, int j){int CellID = getCellIndex(i, j);return OGM[CellID];} //verify if the cell(i,j) is freebool  RAstarPlannerROS::isFree(int CellID){return OGM[CellID];}
}
;bool operator<(cells const &c1, cells const &c2) { return c1.fCost < c2.fCost; }

3.在该目录下的CMakeList.txt中添加如下代码

add_library(relaxed_astar_lib src/RAstar_ros.cpp)

位置如下所示：

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include${catkin_INCLUDE_DIRS}
)## Declare a C++ library
# add_library(${PROJECT_NAME}
#   src/${PROJECT_NAME}/relaxed_astar.cpp
# )add_library(relaxed_astar_lib src/RAstar_ros.cpp)
## Add cmake target dependencies of the library

记得保存再退出。

4.编译该文件

进入catkin_ws目录下，打开新终端输入如下命令进行编译

catkin_make

这时会在~/catkin_ws/devel/lib目录下生成一个librelaxed_astar_lib.so文件

5.在/catkin_ws/src/relaxed_astar目录下创建一个plugin description file，名为relaxed_astar_planner_plugin.xml

gedit relaxed_astar_planner_plugin.xml

复制如下内容到该文件中：

<library path="lib/librelaxed_astar_lib"><class name="RAstar_planner/RAstarPlannerROS" type="RAstar_planner::RAstarPlannerROS" base_class_type="nav_core::BaseGlobalPlanner"><description>This is RA* global planner plugin by iroboapp project.</description></class>
</library>

保存后退出

6.打开目录/catkin_ws/src/relaxed_astar下的package.xml

gedit package.xml

插入如下代码

<nav_core plugin="${prefix}/relaxed_astar_planner_plugin.xml" />

位置如下所示：

<exec_depend>nav_core</exec_depend><exec_depend>roscpp</exec_depend><exec_depend>rospy</exec_depend><exec_depend>std_msgs</exec_depend><!-- The export tag contains other, unspecified, tags --><export><!-- Other tools can request additional information be placed here --><nav_core plugin="${prefix}/relaxed_astar_planner_plugin.xml" /></export>
</package>

7.复制整个relaxed_astar文件夹到turtlebot的工作空间下，我的路径为~/turtlebot_ws/src

8.进入turtlebot目录下再编译一次

cd turtlebot_ws/
catkin_make

9.编辑move_base.launch.xml文件

打开一个新的终端，输入如下命令：

roscd turtlebot_navigation/
cd launch/includes/
sudo gedit move_base.launch.xml

添加如下代码：

<param name="base_global_planner" value="RAstar_planner/RAstarPlannerROS" />

位置如下所示:

<!-- external params file that could be loaded into the move_base namespace --><rosparam file="$(arg custom_param_file)" command="load" /><!-- reset frame_id parameters using user input data --><param name="base_global_planner" value="RAstar_planner/RAstarPlannerROS" /><param name="global_costmap/global_frame" value="$(arg global_frame_id)"/><param name="global_costmap/robot_base_frame" value="$(arg base_frame_id)"/><param name="local_costmap/global_frame" value="$(arg odom_frame_id)"/><param name="local_costmap/robot_base_frame" value="$(arg base_frame_id)"/><param name="DWAPlannerROS/global_frame_id" value="$(arg odom_frame_id)"/>

保存后退出

10.在turtlebot_ws/src/relaxed_astar目录下创建两个文件夹，一个是maps一个是launch文件夹。

maps文件夹放入已经建立好的地图文件，我放入的是lab.yaml以及lab.pgm

在launch文件夹中打开新终端，输入如下代码

touch test.launch
gedit test.launch

原教程中是将如下代码粘贴到里面：

<launch><include file="$(find turtlebot_bringup)/launch/minimal.launch"></include><include file="$(find turtlebot_bringup)/launch/3dsensor.launch"<arg name="rgb_processing" value="false" /><arg name="depth_registration" value="false" /><arg name="depth_processing" value="false" /><arg name="scan_topic" value="/scan" /></include><arg name="map_file" default="$(find relaxed_astar)/maps/lab.yaml"><node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" /><arg name="initial_pose_x" default="0.0" /><arg name="initial_pose_y" default="0.0" /><arg name="initial_pose_a" default="0.0" /><include file="$(find turtlebot_navigation)/launch/includes/amcl.launch.xml"><arg name="initial_pose_x" value="$(arg initial_pose_x)"/><arg name="initial_pose_y" value="$(arg initial_pose_y)"/><arg name="initial_pose_a" value="$(arg initial_pose_a)"/></include><include file="$(find turtlebot_navigation)/launch/includes/move_base.launch.xml"/></include>

我放置的代码为：

<launch><include file="$(find turtlebot_bringup)/launch/minimal.launch"></include><!-- Define laser type--><arg name="laser_type" default="hokuyo" /><!-- laser driver --><include file="$(find turtlebot_navigation)/laser/driver/$(arg laser_type)_laser.launch"/><arg name="map_file" default="$(find relaxed_astar)/maps/lab.yaml" /><node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" /><!-- AMCL --><arg name="custom_amcl_launch_file" default="$(find turtlebot_navigation)/launch/includes/amcl/$(arg laser_type)_amcl.launch.xml"/><arg name="initial_pose_x" default="0.0"/> <!-- Use 17.0 for willow's map in simulation --><arg name="initial_pose_y" default="0.0"/> <!-- Use 17.0 for willow's map in simulation --><arg name="initial_pose_a" default="0.0"/><include file="$(arg custom_amcl_launch_file)"><arg name="initial_pose_x" value="$(arg initial_pose_x)"/><arg name="initial_pose_y" value="$(arg initial_pose_y)"/><arg name="initial_pose_a" value="$(arg initial_pose_a)"/></include><include file="$(find turtlebot_navigation)/launch/includes/move_base.launch.xml"/></launch>

具体的还需根据自己情况进行修改

11.下面可以运行一下launch文件

在一个新终端输入：

roslaunch relaxed_astar test.launch

如果看到odom received!则运行成功

在另一个新终端中输入

roslaunch turtlebot_rviz_launchers view_navigation.launch --screen

可以对turtlebot实时查看以及导航。

2021-04-06相关推荐

林阳斌集美大学计算机学院,集美大学计算机工程学院导师教师师资介绍简介-林阳斌副教授（2021.04.06）...
姓名林阳斌性别男出生年月 1987.4 所在单位计算机工程学院学历博士职称副教授毕业院校(最高学历):厦门大学承担的主要课程:数学建模,数据可视化,运筹学研究领域及方向:三维视 ...
2021.04.06
基本概念总结: 梯度更新方式参考1 参考2 参考3 BGD 交叉熵损失函数批量梯度下降法(Batch Gradient Descent) 思路是在更新每一参数时都使用所有的样本来进行更新. 优点:全 ...
Go webrtc项目pion创始人专访 | Gopher Daily (2021.04.07) ʕ◔ϖ◔ʔ
每日一谚:Go makes error handling as important as any other code Go技术生态 go webrtc项目pion的创始人专访 - https://w ...
2021年第12届蓝桥杯 Java B组省赛真题详解及小结【第1场省赛 2021.04.18】
蓝桥杯 Java B组省赛决赛真题详解及小结汇总[题目下载.2013年(第4届)~2020年(第11届)] CSDN 蓝桥杯专栏 2013年第04届蓝桥杯 Java B组省赛真题详解及小结 ...
投票服务器维护时间,2021年06月21日维护预览，合服投票结果公布！
原标题:2021年06月21日维护预览,合服投票结果公布! 亲爱的玩家,大家好!!又到了每周的维护预览时间! 以下是本周维护内容: 1.本周维护之后, 新资料片<天粹英华>将在局部服 ...
【跃迁之路】【425天】刻意练习系列184—SQL（2018.04.06）
@(跃迁之路)专栏叨叨两句技术的精进不能只是简单的刷题,而应该是不断的"刻意"练习该系列改版后正式纳入[跃迁之路]专栏,持续更新刻意练习--MySQL 2018.04.02 ...
【跃迁之路】【425天】程序员高效学习方法论探索系列（实验阶段182-2018.04.06）...
@(跃迁之路)专栏实验说明从2017.10.6起,开启这个系列,目标只有一个:探索新的学习方法,实现跃迁式成长实验期2年(2017.10.06 - 2019.10.06) 我将以自己为实验对象. ...
面试题 04.06. 后继者
面试题 04.06. 后继者思路:中序遍历,第一个大于p->val的就是答案.实在不会,用vector存中序遍历的TreeNode,一个个找. class Solution { public: ...
Interesting Finds: 2008.04.06
Other Windows Mobile 6.1 Key Features Playing with Gravatar images Top 5 Most Important Sessions fro ...
Russ Cox：这不是Go项目的标准布局 | Gopher Daily (2021.04.28) ʕ◔ϖ◔ʔ
每日一谚:Profile before you decide something is performance critical. Go技术生态 Go web开发的当前状态 - https://tno ...

2021-04-06

**ROS与Navigation-编写自定义A*算法全局路径规划教程**

2021-04-06相关推荐

最新文章

热门文章