路径规划算法 快速搜索随机树(Rapid-exploration Random Tree)

机器人路径规划算法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;
}


  • 1
    点赞
  • 17
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值