机器人路径规划算法keyword
A* 已讲解了
然后是rrt 快速搜索随机树(Rapid-exploration Random Tree)
中文论文
然后有一份在ros上写的代码,这个是拷贝点击打开链接这个博客的代码
这份rrt代码也很简单,容易理解,代码变量名很清楚,顺便还学了ros的节点和rviz之类的写法,还是很不错的
这里这贴一个主节点代码,就是画边框和障碍物的,可以参考一下写法
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <path_planning/rrt.h>
#include <path_planning/obstacles.h>
#include <geometry_msgs/Point.h>
#include <iostream>
#include <cmath>
#include <math.h>
#include <stdlib.h>
#include <unistd.h>
#include <vector>
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<geometry_msgs::Point> initializeBoundary()
{
vector<geometry_msgs::Point> 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<geometry_msgs::Point> initializeObstacles()
{
vector< vector<geometry_msgs::Point> > obstArray;
vector<geometry_msgs::Point> obstaclesMarker;
obstacles obst;
obstArray = obst.getObstacleArray();
for(int i=0; i<obstArray.size(); i++)
{
for(int j=1; j<5; j++)
{
obstaclesMarker.push_back(obstArray[i][j-1]);
obstaclesMarker.push_back(obstArray[i][j]);
}
}
return obstaclesMarker;
}
int main(int argc,char** argv)
{
//initializing ROS
ros::init(argc,argv,"env_node");
ros::NodeHandle n;
//defining Publisher
ros::Publisher env_publisher = n.advertise<visualization_msgs::Marker>("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;
}