源码地址:https://github.com/nalin1096/path_planning
具体使用案例,参考:ROS功能包http://blog.csdn.net/zhangrelay/article/details/76850690
使用ROS实现了基于RRT路径规划算法。
发行版 - indigo
算法在有一个障碍的环境找到优化的路径。算法可视化在RVIZ完成,代码是用C ++编写。
包有两个可执行文件:
1 ros_nodeRVIZ参数:
1 Frame_id =“path_planner”
2 marker_topic =“path_planner_rrt”
说明:
如果想修改环境environment,如下:
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
using namespace rrt;
void initializeMarkers(visualization_msgs::Marker &boundary,
visualization_msgs::Marker &obstacle)
{
//init headers
boundary.header.frame_id = obstacle.header.frame_id = "path_planner";
boundary.header.stamp = obstacle.header.stamp = ros::Time::now();
boundary.ns = obstacle.ns = "path_planner";
boundary.action = obstacle.action = visualization_msgs::Marker::ADD;
boundary.pose.orientation.w = obstacle.pose.orientation.w = 1.0;
//setting id for each marker
boundary.id = 110;
obstacle.id = 111;
//defining types
boundary.type = visualization_msgs::Marker::LINE_STRIP;
obstacle.type = visualization_msgs::Marker::LINE_LIST;
//setting scale
boundary.scale.x = 1;
obstacle.scale.x = 0.2;
//assigning colors
boundary.color.r = obstacle.color.r = 0.0f;
boundary.color.g = obstacle.color.g = 0.0f;
boundary.color.b = obstacle.color.b = 0.0f;
boundary.color.a = obstacle.color.a = 1.0f;
}
vector initializeBoundary()
{
vector bondArray;
geometry_msgs::Point point;
//first point
point.x = 0;
point.y = 0;
point.z = 0;
bondArray.push_back(point);
//second point
point.x = 0;
point.y = 100;
point.z = 0;
bondArray.push_back(point);
//third point
point.x = 100;
point.y = 100;
point.z = 0;
bondArray.push_back(point);
//fourth point
point.x = 100;
point.y = 0;
point.z = 0;
bondArray.push_back(point);
//first point again to complete the box
point.x = 0;
point.y = 0;
point.z = 0;
bondArray.push_back(point);
return bondArray;
}
vector initializeObstacles()
{
vector< vector > obstArray;
vector obstaclesMarker;
obstacles obst;
obstArray = obst.getObstacleArray();
for(int i=0; i("path_planner_rrt",1);
//defining markers
visualization_msgs::Marker boundary;
visualization_msgs::Marker obstacle;
initializeMarkers(boundary, obstacle);
//initializing rrtTree
RRT myRRT(2.0,2.0);
int goalX, goalY;
goalX = goalY = 95;
boundary.points = initializeBoundary();
obstacle.points = initializeObstacles();
env_publisher.publish(boundary);
env_publisher.publish(obstacle);
while(ros::ok())
{
env_publisher.publish(boundary);
env_publisher.publish(obstacle);
ros::spinOnce();
ros::Duration(1).sleep();
}
return 1;
}
#include
#include
vector< vector > obstacles::getObstacleArray()
{
vector obstaclePoint;
geometry_msgs::Point point;
//first point
point.x = 50;
point.y = 50;
point.z = 0;
obstaclePoint.push_back(point);
//second point
point.x = 50;
point.y = 70;
point.z = 0;
obstaclePoint.push_back(point);
//third point
point.x = 80;
point.y = 70;
point.z = 0;
obstaclePoint.push_back(point);
//fourth point
point.x = 80;
point.y = 50;
point.z = 0;
obstaclePoint.push_back(point);
//first point again to complete the box
point.x = 50;
point.y = 50;
point.z = 0;
obstaclePoint.push_back(point);
obstacleArray.push_back(obstaclePoint);
return obstacleArray;
}
#include
#include
#include
#include
using namespace rrt;
/**
* default constructor for RRT class
* initializes source to 0,0
* adds sorce to rrtTree
*/
RRT::RRT()
{
RRT::rrtNode newNode;
newNode.posX = 0;
newNode.posY = 0;
newNode.parentID = 0;
newNode.nodeID = 0;
rrtTree.push_back(newNode);
}
/**
* default constructor for RRT class
* initializes source to input X,Y
* adds sorce to rrtTree
*/
RRT::RRT(double input_PosX, double input_PosY)
{
RRT::rrtNode newNode;
newNode.posX = input_PosX;
newNode.posY = input_PosY;
newNode.parentID = 0;
newNode.nodeID = 0;
rrtTree.push_back(newNode);
}
/**
* Returns the current RRT tree
* @return RRT Tree
*/
vector RRT::getTree()
{
return rrtTree;
}
/**
* For setting the rrtTree to the inputTree
* @param rrtTree
*/
void RRT::setTree(vector input_rrtTree)
{
rrtTree = input_rrtTree;
}
/**
* to get the number of nodes in the rrt Tree
* @return tree size
*/
int RRT::getTreeSize()
{
return rrtTree.size();
}
/**
* adding a new node to the rrt Tree
*/
void RRT::addNewNode(RRT::rrtNode node)
{
rrtTree.push_back(node);
}
/**
* removing a node from the RRT Tree
* @return the removed tree
*/
RRT::rrtNode RRT::removeNode(int id)
{
RRT::rrtNode tempNode = rrtTree[id];
rrtTree.erase(rrtTree.begin()+id);
return tempNode;
}
/**
* getting a specific node
* @param node id for the required node
* @return node in the rrtNode structure
*/
RRT::rrtNode RRT::getNode(int id)
{
return rrtTree[id];
}
/**
* return a node from the rrt tree nearest to the given point
* @param X position in X cordinate
* @param Y position in Y cordinate
* @return nodeID of the nearest Node
*/
int RRT::getNearestNodeID(double X, double Y)
{
int i, returnID;
double distance = 9999, tempDistance;
for(i=0; igetTreeSize(); i++)
{
tempDistance = getEuclideanDistance(X,Y, getPosX(i),getPosY(i));
if (tempDistance < distance)
{
distance = tempDistance;
returnID = i;
}
}
return returnID;
}
/**
* returns X coordinate of the given node
*/
double RRT::getPosX(int nodeID)
{
return rrtTree[nodeID].posX;
}
/**
* returns Y coordinate of the given node
*/
double RRT::getPosY(int nodeID)
{
return rrtTree[nodeID].posY;
}
/**
* set X coordinate of the given node
*/
void RRT::setPosX(int nodeID, double input_PosX)
{
rrtTree[nodeID].posX = input_PosX;
}
/**
* set Y coordinate of the given node
*/
void RRT::setPosY(int nodeID, double input_PosY)
{
rrtTree[nodeID].posY = input_PosY;
}
/**
* returns parentID of the given node
*/
RRT::rrtNode RRT::getParent(int id)
{
return rrtTree[rrtTree[id].parentID];
}
/**
* set parentID of the given node
*/
void RRT::setParentID(int nodeID, int parentID)
{
rrtTree[nodeID].parentID = parentID;
}
/**
* add a new childID to the children list of the given node
*/
void RRT::addChildID(int nodeID, int childID)
{
rrtTree[nodeID].children.push_back(childID);
}
/**
* returns the children list of the given node
*/
vector RRT::getChildren(int id)
{
return rrtTree[id].children;
}
/**
* returns number of children of a given node
*/
int RRT::getChildrenSize(int nodeID)
{
return rrtTree[nodeID].children.size();
}
/**
* returns euclidean distance between two set of X,Y coordinates
*/
double RRT::getEuclideanDistance(double sourceX, double sourceY, double destinationX, double destinationY)
{
return sqrt(pow(destinationX - sourceX,2) + pow(destinationY - sourceY,2));
}
/**
* returns path from root to end node
* @param endNodeID of the end node
* @return path containing ID of member nodes in the vector form
*/
vector RRT::getRootToEndPath(int endNodeID)
{
vector path;
path.push_back(endNodeID);
while(rrtTree[path.front()].nodeID != 0)
{
//std::cout<
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#define success false
#define running true
using namespace rrt;
bool status = running;
void initializeMarkers(visualization_msgs::Marker &sourcePoint,
visualization_msgs::Marker &goalPoint,
visualization_msgs::Marker &randomPoint,
visualization_msgs::Marker &rrtTreeMarker,
visualization_msgs::Marker &finalPath)
{
//init headers
sourcePoint.header.frame_id = goalPoint.header.frame_id = randomPoint.header.frame_id = rrtTreeMarker.header.frame_id = finalPath.header.frame_id = "path_planner";
sourcePoint.header.stamp = goalPoint.header.stamp = randomPoint.header.stamp = rrtTreeMarker.header.stamp = finalPath.header.stamp = ros::Time::now();
sourcePoint.ns = goalPoint.ns = randomPoint.ns = rrtTreeMarker.ns = finalPath.ns = "path_planner";
sourcePoint.action = goalPoint.action = randomPoint.action = rrtTreeMarker.action = finalPath.action = visualization_msgs::Marker::ADD;
sourcePoint.pose.orientation.w = goalPoint.pose.orientation.w = randomPoint.pose.orientation.w = rrtTreeMarker.pose.orientation.w = finalPath.pose.orientation.w = 1.0;
//setting id for each marker
sourcePoint.id = 0;
goalPoint.id = 1;
randomPoint.id = 2;
rrtTreeMarker.id = 3;
finalPath.id = 4;
//defining types
rrtTreeMarker.type = visualization_msgs::Marker::LINE_LIST;
finalPath.type = visualization_msgs::Marker::LINE_STRIP;
sourcePoint.type = goalPoint.type = randomPoint.type = visualization_msgs::Marker::SPHERE;
//setting scale
rrtTreeMarker.scale.x = 0.2;
finalPath.scale.x = 1;
sourcePoint.scale.x = goalPoint.scale.x = randomPoint.scale.x = 2;
sourcePoint.scale.y = goalPoint.scale.y = randomPoint.scale.y = 2;
sourcePoint.scale.z = goalPoint.scale.z = randomPoint.scale.z = 1;
//assigning colors
sourcePoint.color.r = 1.0f;
goalPoint.color.g = 1.0f;
randomPoint.color.b = 1.0f;
rrtTreeMarker.color.r = 0.8f;
rrtTreeMarker.color.g = 0.4f;
finalPath.color.r = 0.2f;
finalPath.color.g = 0.2f;
finalPath.color.b = 1.0f;
sourcePoint.color.a = goalPoint.color.a = randomPoint.color.a = rrtTreeMarker.color.a = finalPath.color.a = 1.0f;
}
vector< vector > getObstacles()
{
obstacles obst;
return obst.getObstacleArray();
}
void addBranchtoRRTTree(visualization_msgs::Marker &rrtTreeMarker, RRT::rrtNode &tempNode, RRT &myRRT)
{
geometry_msgs::Point point;
point.x = tempNode.posX;
point.y = tempNode.posY;
point.z = 0;
rrtTreeMarker.points.push_back(point);
RRT::rrtNode parentNode = myRRT.getParent(tempNode.nodeID);
point.x = parentNode.posX;
point.y = parentNode.posY;
point.z = 0;
rrtTreeMarker.points.push_back(point);
}
bool checkIfInsideBoundary(RRT::rrtNode &tempNode)
{
if(tempNode.posX < 0 || tempNode.posY < 0 || tempNode.posX > 100 || tempNode.posY > 100 ) return false;
else return true;
}
bool checkIfOutsideObstacles(vector< vector > &obstArray, RRT::rrtNode &tempNode)
{
double AB, AD, AMAB, AMAD;
for(int i=0; i > &obstArray)
{
int nearestNodeID = myRRT.getNearestNodeID(tempNode.posX,tempNode.posY);
RRT::rrtNode nearestNode = myRRT.getNode(nearestNodeID);
double theta = atan2(tempNode.posY - nearestNode.posY,tempNode.posX - nearestNode.posX);
tempNode.posX = nearestNode.posX + (rrtStepSize * cos(theta));
tempNode.posY = nearestNode.posY + (rrtStepSize * sin(theta));
if(checkIfInsideBoundary(tempNode) && checkIfOutsideObstacles(obstArray,tempNode))
{
tempNode.parentID = nearestNodeID;
tempNode.nodeID = myRRT.getTreeSize();
myRRT.addNewNode(tempNode);
return true;
}
else
return false;
}
bool checkNodetoGoal(int X, int Y, RRT::rrtNode &tempNode)
{
double distance = sqrt(pow(X-tempNode.posX,2)+pow(Y-tempNode.posY,2));
if(distance < 3)
{
return true;
}
return false;
}
void setFinalPathData(vector< vector > &rrtPaths, RRT &myRRT, int i, visualization_msgs::Marker &finalpath, int goalX, int goalY)
{
RRT::rrtNode tempNode;
geometry_msgs::Point point;
for(int j=0; j("path_planner_rrt",1);
//defining markers
visualization_msgs::Marker sourcePoint;
visualization_msgs::Marker goalPoint;
visualization_msgs::Marker randomPoint;
visualization_msgs::Marker rrtTreeMarker;
visualization_msgs::Marker finalPath;
initializeMarkers(sourcePoint, goalPoint, randomPoint, rrtTreeMarker, finalPath);
//setting source and goal
sourcePoint.pose.position.x = 2;
sourcePoint.pose.position.y = 2;
goalPoint.pose.position.x = 95;
goalPoint.pose.position.y = 95;
rrt_publisher.publish(sourcePoint);
rrt_publisher.publish(goalPoint);
ros::spinOnce();
ros::Duration(0.01).sleep();
srand (time(NULL));
//initialize rrt specific variables
//initializing rrtTree
RRT myRRT(2.0,2.0);
int goalX, goalY;
goalX = goalY = 95;
int rrtStepSize = 3;
vector< vector > rrtPaths;
vector path;
int rrtPathLimit = 1;
int shortestPathLength = 9999;
int shortestPath = -1;
RRT::rrtNode tempNode;
vector< vector > obstacleList = getObstacles();
bool addNodeResult = false, nodeToGoal = false;
while(ros::ok() && status)
{
if(rrtPaths.size() < rrtPathLimit)
{
generateTempPoint(tempNode);
//std::cout<<"tempnode generated"<= rrtPathLimit)
{
status = success;
std::cout<<"Finding Optimal Path"<
obstacles:
#ifndef OBSTACLES_H
#define OBSTACLES_H
#include
#include
#include
using namespace std;
class obstacles
{
public:
/** Default constructor */
obstacles() {}
/** Default destructor */
virtual ~obstacles() {}
vector< vector > getObstacleArray();
protected:
private:
vector< vector > obstacleArray;
};
#endif // OBSTACLES_H
#ifndef rrt_h
#define rrt_h
#include
using namespace std;
namespace rrt {
class RRT{
public:
RRT();
RRT(double input_PosX, double input_PosY);
struct rrtNode{
int nodeID;
double posX;
double posY;
int parentID;
vector children;
};
vector getTree();
void setTree(vector input_rrtTree);
int getTreeSize();
void addNewNode(rrtNode node);
rrtNode removeNode(int nodeID);
rrtNode getNode(int nodeID);
double getPosX(int nodeID);
double getPosY(int nodeID);
void setPosX(int nodeID, double input_PosX);
void setPosY(int nodeID, double input_PosY);
rrtNode getParent(int nodeID);
void setParentID(int nodeID, int parentID);
void addChildID(int nodeID, int childID);
vector getChildren(int nodeID);
int getChildrenSize(int nodeID);
int getNearestNodeID(double X, double Y);
vector getRootToEndPath(int endNodeID);
private:
vector rrtTree;
double getEuclideanDistance(double sourceX, double sourceY, double destinationX, double destinationY);
};
};
#endif
CMakeLists:
cmake_minimum_required(VERSION 2.8.3)
project(path_planning)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
visualization_msgs
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependencies might have been
## pulled in transitively but can be declared for certainty nonetheless:
## * add a build_depend tag for "message_generation"
## * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs# visualization_msgs
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
LIBRARIES path_planning
CATKIN_DEPENDS roscpp std_msgs visualization_msgs
DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
# include_directories(include)
include_directories(include
${catkin_INCLUDE_DIRS}
)
## Declare a cpp library
add_library(path_planning
src/rrt.cpp
src/obstacles.cpp
)
## Declare a cpp executable
# add_executable(path_planning_node src/path_planning_node.cpp)
add_executable(rrt_node src/rrt_node.cpp)
add_dependencies(rrt_node path_planning)
target_link_libraries(rrt_node path_planning ${catkin_LIBRARIES} )
add_executable(env_node src/environment.cpp)
add_dependencies(env_node path_planning)
target_link_libraries(env_node path_planning ${catkin_LIBRARIES} )
## Add cmake target dependencies of the executable/library
## as an example, message headers may need to be generated before nodes
# add_dependencies(path_planning_node path_planning_generate_messages_cpp)
## Specify libraries to link a library or executable target against
# target_link_libraries(path_planning_node
# ${catkin_LIBRARIES}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
# install(TARGETS path_planning path_planning_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_path_planning.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
package:
path_planning
1.0.0
A path planning algorithm using RRT visualized in RVIZ
Nalin Gupta
TODO
catkin
roscpp
std_msgs
visualization_msgs
roscpp
std_msgs
visualization_msgs