基于ROS的OMPL库RRT*算法代码学习

本文是在学习路径规划算法中的一篇学习记录,由于对c++掌握粗浅,将整个代码进行了详细的梳理,由于代码中有很多是之前梳理过的《基于ROS的A*算法代码学习》中的,对本文graph_searcher.h注释可参考上一篇的Astar_searcher.h; node.h注释可参考上一篇的node.h; graph_searcher.cpp注释可参考Astar_searcher.cpp; waypoint_generator.cpp可参考上一篇的waypoint_generator.cpp.本文在参考其他博主总结的基础上,整理出了多种代码实现的方法.

(代码注释中英夹杂,英文注释是原来作者的注释,中文注释是本人后来添加的)

过程演示

1.编译功能包.

2.Ctrl+Shift+t新建窗口打开roscore.

3.Ctrl+Shift+t新建窗口打开rviz.
记得要source devel/setup.sh,告诉这个窗口你功能包的位置,否则3d Nav Goal插件会找不到.

4.在rviz中打开demo.rviz(路径src/grid_path_searcher/launch/rviz_config).

5.Ctrl+Shift+t新建窗口加载地图.
记得source.


6.RRT*路径搜索.

下面是结果

这是终端里的信息

ROS包

主要程序文件demo_node.cpp

#include <iostream>
#include <fstream>
#include <math.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <ros/ros.h>
#include <ros/console.h>
#include <sensor_msgs/PointCloud2.h>

#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <visualization_msgs/MarkerArray.h>
#include <visualization_msgs/Marker.h>

#include <ompl/config.h>
#include <ompl/base/StateSpace.h>
#include <ompl/base/Path.h>
#include <ompl/base/spaces/RealVectorBounds.h>
#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/base/StateValidityChecker.h>
#include <ompl/base/OptimizationObjective.h>
#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
#include <ompl/geometric/planners/rrt/RRTstar.h>
#include <ompl/geometric/SimpleSetup.h>

#include "graph_searcher.h"
#include "backward.hpp"

using namespace std;
using namespace Eigen;

namespace ob = ompl::base;
namespace og = ompl::geometric;

namespace backward {
backward::SignalHandling sh;
}

// simulation param from launch file
double _resolution, _inv_resolution, _cloud_margin;
double _x_size, _y_size, _z_size;    

// useful global variables
bool _has_map   = false;

Vector3d _start_pt;
Vector3d _map_lower, _map_upper;
int _max_x_id, _max_y_id, _max_z_id;

// ros related
ros::Subscriber _map_sub, _pts_sub;
ros::Publisher  _grid_map_vis_pub, _RRTstar_path_vis_pub;

RRTstarPreparatory * _RRTstar_preparatory     = new RRTstarPreparatory();

void rcvWaypointsCallback(const nav_msgs::Path & wp);
void rcvPointCloudCallBack(const sensor_msgs::PointCloud2 & pointcloud_map);
void pathFinding(const Vector3d start_pt, const Vector3d target_pt);
void visRRTstarPath(vector<Vector3d> nodes );

void rcvWaypointsCallback(const nav_msgs::Path & wp)
{     
    if( wp.poses[0].pose.position.z < 0.0 || _has_map == false )
        return;

    Vector3d target_pt;
    target_pt << wp.poses[0].pose.position.x,
                 wp.poses[0].pose.position.y,
                 wp.poses[0].pose.position.z;

    ROS_INFO("[node] receive the planning target");
    pathFinding(_start_pt, target_pt); 
}

void rcvPointCloudCallBack(const sensor_msgs::PointCloud2 & pointcloud_map)
{   
    if(_has_map ) return;

    pcl::PointCloud<pcl::PointXYZ> cloud;
    pcl::PointCloud<pcl::PointXYZ> cloud_vis;
    sensor_msgs::PointCloud2 map_vis;

    pcl::fromROSMsg(pointcloud_map, cloud);
    
    if( (int)cloud.points.size() == 0 ) return;

    pcl::PointXYZ pt;
    for (int idx = 0; idx < (int)cloud.points.size(); idx++)
    {    
        pt = cloud.points[idx];        

        // set obstalces into grid map for path planning
        _RRTstar_preparatory->setObs(pt.x, pt.y, pt.z);

        // for visualize only
        Vector3d cor_round = _RRTstar_preparatory->coordRounding(Vector3d(pt.x, pt.y, pt.z));
        pt.x = cor_round(0);
        pt.y = cor_round(1);
        pt.z = cor_round(2);
        cloud_vis.points.push_back(pt);
    }

    cloud_vis.width    = cloud_vis.points.size();
    cloud_vis.height   = 1;
    cloud_vis.is_dense = true;

    pcl::toROSMsg(cloud_vis, map_vis);

    map_vis.header.frame_id = "/world";
    _grid_map_vis_pub.publish(map_vis);

    _has_map = true;
}

// Our collision checker. For this demo, our robot's state space
class ValidityChecker : public ob::StateValidityChecker
{
public:
    // 单冒号(:)的作用是表示后面是初始化列表,可以参考https://blog.csdn.net/lusirking/article/details/83988421
    // ob::StateValidityChecker父类构造的时候要传入一个状态空间信息(si)
    ValidityChecker(const ob::SpaceInformationPtr& si) :
        ob::StateValidityChecker(si) {}
    // Returns whether the given state's position overlaps the
    // circular obstacle
    bool isValid(const ob::State* state) const
    {   
        // We know we're working with a RealVectorStateSpace in this
        // example, so we downcast state into the specific type.
        const ob::RealVectorStateSpace::StateType* state3D =
            state->as<ob::RealVectorStateSpace::StateType>();
        /**
        *
        *
        STEP 1: Extract the robot's (x,y,z) position from its state
        *
        *
        */
        auto x = (*state3D)[0];
        auto y = (*state3D)[1];
        auto z = (*state3D)[2];

        return _RRTstar_preparatory->isObsFree(x, y, z);
    }
};

// Returns a structure representing the optimization objective to use
// for optimal motion planning. This method returns an objective which
// attempts to minimize the length in configuration space of computed
// paths.
ob::OptimizationObjectivePtr getPathLengthObjective(const ob::SpaceInformationPtr& si)
{
    return ob::OptimizationObjectivePtr(new ob::PathLengthOptimizationObjective(si));
}

ob::OptimizationObjectivePtr getThresholdPathLengthObj(const ob::SpaceInformationPtr& si)
{
    ob::OptimizationObjectivePtr obj(new ob::PathLengthOptimizationObjective(si));
    obj->setCostThreshold(ob::Cost(1.51));
    return obj;
}

void pathFinding(const Vector3d start_pt, const Vector3d target_pt)
{
    // ompl使用一般步骤:
    //     1.实例化一个状态空间;
    //     2.实例化一个控制空间(可选);
    //     3.实例化一个空间信息类;
    //     4.实例化一个问题描述;
    //     5.实例化一个规划器;
    //     6.路径简化器.
    // 不熟悉ompl库的使用流程可以参考下面的网站简单学习一下:
    //     https://blog.csdn.net/wjydym/article/details/104593543
    // Construct the robot state space in which we're planning. 
    // 构造一个三维的状态空间
    ob::StateSpacePtr space(new ob::RealVectorStateSpace(3));

    // Set the bounds of space to be in [0,1].
    // 边界条件设置
    ob::RealVectorBounds bounds(3);
    bounds.setLow(0, - _x_size * 0.5);
    bounds.setLow(1, - _y_size * 0.5);
    bounds.setLow(2, 0.0);

    bounds.setHigh(0, + _x_size * 0.5);
    bounds.setHigh(1, + _y_size * 0.5);
    bounds.setHigh(2, _z_size);

    // 将设置的边界赋予状态空间space，其中as的作用是类型转换
    space->as<ob::RealVectorStateSpace>()->setBounds(bounds);
    // Construct a space information instance for this state space
    // 构造状态空间信息SpaceInformation,后面的一些操作需要用到状态空间信息
    ob::SpaceInformationPtr si(new ob::SpaceInformation(space));
    // Set the object used to check which states in the space are valid
    // 为状态空间设置状态检查函数StateValidChecker,ValidityChecker肯定要知道状态的信息，也就是si
    si->setStateValidityChecker(ob::StateValidityCheckerPtr(new ValidityChecker(si)));
    // 载入设置
    si->setup();

    // Set our robot's starting state
    // 设定起点
    ob::ScopedState<> start(space);
    /**
    *
    *
    STEP 2: Finish the initialization of start state
    *
    *
    */ //  todo can  I simply it?
    // 方法1:
    // start[0] = (&start_pt)->operator[](0)  ;
    // start[1] = (&start_pt)->operator[](1)  ;
    // start[2] = (&start_pt)->operator[](2)  ;
    // 方法2:
    // start[0] = start_pt(0);
    // start[1] = start_pt(1);
    // start[2] = start_pt(2);
    // 方法3:
    // start->as<ob::RealVectorStateSpace::StateType>()->values[0] = start_pt(0);
    // start->as<ob::RealVectorStateSpace::StateType>()->values[1] = start_pt(1);
    // start->as<ob::RealVectorStateSpace::StateType>()->values[2] = start_pt(2);
    // 方法4:
    start->as<ob::RealVectorStateSpace::StateType>()->operator[](0) = start_pt(0);
    start->as<ob::RealVectorStateSpace::StateType>()->operator[](1) = start_pt(1);
    start->as<ob::RealVectorStateSpace::StateType>()->operator[](2) = start_pt(2);

    // Set our robot's goal state
    // 设定终点
    ob::ScopedState<> goal(space);
    /**
    *
    *
    STEP 3: Finish the initialization of goal state
    *
    *
    */
    // 方法1:
    // goal[0] = (&target_pt)->operator[](0)  ;
    // goal[1] = (&target_pt)->operator[](1)  ;
    // goal[2] = (&target_pt)->operator[](2)  ;
    // 方法2:
    // goal[0] = target_pt(0);
    // goal[1] = target_pt(1);
    // goal[2] = target_pt(2);
    // 方法3:
    // goal->as<ob::RealVectorStateSpace::StateType>()->values[0] = target_pt(0);
    // goal->as<ob::RealVectorStateSpace::StateType>()->values[1] = target_pt(1);
    // goal->as<ob::RealVectorStateSpace::StateType>()->values[2] = target_pt(2);
    // 方法4:
    goal->as<ob::RealVectorStateSpace::StateType>()->operator[](0) = target_pt(0);
    goal->as<ob::RealVectorStateSpace::StateType>()->operator[](1) = target_pt(1);
    goal->as<ob::RealVectorStateSpace::StateType>()->operator[](2) = target_pt(2);
    // Create a problem instance

    /**
    *
    *
    STEP 4: Create a problem instance, 
    please define variable as pdef
    *
    *
    */
    // 构造问题实例,传入之前构造的状态空间信息SpaceInformation
    auto pdef(std::make_shared<ob::ProblemDefinition>(si));

    // Set the start and goal states
    // 为问题实例设置起点和终点
    pdef->setStartAndGoalStates(start, goal);

    // Set the optimization objective
    /**
    *
    *
    STEP 5: Set the optimization objective, the options you can choose are defined earlier:
    getPathLengthObjective() and getThresholdPathLengthObj()
    *
    *
    */
    // 该语句为本问题设置了一个长度优化器，来自于ompl库自带的路径长度优化类
    // 方法1:
    // pdef->setOptimizationObjective(getPathLengthObjective(si));
    // 方法2:
    pdef->setOptimizationObjective(std::make_shared<ob::PathLengthOptimizationObjective>(si));

    // Construct our optimizing planner using the RRTstar algorithm.
    /**
    *
    *
    STEP 6: Construct our optimizing planner using the RRTstar algorithm, 
    please define varible as optimizingPlanner
    *
    *
    */
    // 构造一个使用RRTstar算法对状态空间进行规划的规划器
    ob::PlannerPtr optimizingPlanner(new og::RRTstar(si));

    // Set the problem instance for our planner to solve
    // 传入问题实例给规划器
    optimizingPlanner->setProblemDefinition(pdef);
    // 调用setup()将设置载入。
    optimizingPlanner->setup();

    // attempt to solve the planning problem within one second of
    // planning time
    ob::PlannerStatus solved = optimizingPlanner->solve(1.0);

    // 如果有解,就将得到的路径压入堆栈,将路径在rviz中显示出来
    if (solved)
    {
        // get the goal representation from the problem definition (not the same as the goal state)
        // and inquire about the found path
        og::PathGeometric* path = pdef->getSolutionPath()->as<og::PathGeometric>();
        
        vector<Vector3d> path_points;

        for (size_t path_idx = 0; path_idx < path->getStateCount (); path_idx++)
        {
            const ob::RealVectorStateSpace::StateType *state = path->getState(path_idx)->as<ob::RealVectorStateSpace::StateType>();
            /**
            *
            *
            STEP 7: Trandform the found path from path to path_points for rviz display
            *
            *
            */
            // 方法1:
            // auto x = state->operator[](0);
            // auto y = state->operator[](1);
            // auto z = state->operator[](2);
            // 方法2:
            // auto x = (*state)[0];
            // auto y = (*state)[1];
            // auto z = (*state)[2];
            // 方法3:
            auto x = state->values[0];
            auto y = state->values[1];
            auto z = state->values[2];
            Vector3d temp_mat(x,y,z);
            path_points.push_back(temp_mat);
        }
        visRRTstarPath(path_points);       
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "demo_node");
    ros::NodeHandle nh("~");

    _map_sub  = nh.subscribe( "map",       1, rcvPointCloudCallBack );
    _pts_sub  = nh.subscribe( "waypoints", 1, rcvWaypointsCallback );

    _grid_map_vis_pub             = nh.advertise<sensor_msgs::PointCloud2>("grid_map_vis", 1);
    _RRTstar_path_vis_pub         = nh.advertise<visualization_msgs::Marker>("RRTstar_path_vis",1);


    nh.param("map/cloud_margin",  _cloud_margin, 0.0);
    nh.param("map/resolution",    _resolution,   0.2);
    
    nh.param("map/x_size",        _x_size, 50.0);
    nh.param("map/y_size",        _y_size, 50.0);
    nh.param("map/z_size",        _z_size, 5.0 );
    
    nh.param("planning/start_x",  _start_pt(0),  0.0);
    nh.param("planning/start_y",  _start_pt(1),  0.0);
    nh.param("planning/start_z",  _start_pt(2),  0.0);

    _map_lower << - _x_size/2.0, - _y_size/2.0,     0.0;
    _map_upper << + _x_size/2.0, + _y_size/2.0, _z_size;
    
    _inv_resolution = 1.0 / _resolution;
    
    _max_x_id = (int)(_x_size * _inv_resolution);
    _max_y_id = (int)(_y_size * _inv_resolution);
    _max_z_id = (int)(_z_size * _inv_resolution);

    _RRTstar_preparatory  = new RRTstarPreparatory();
    _RRTstar_preparatory  -> initGridMap(_resolution, _map_lower, _map_upper, _max_x_id, _max_y_id, _max_z_id);
    
    ros::Rate rate(100);
    bool status = ros::ok();
    while(status) 
    {
        ros::spinOnce();      
        status = ros::ok();
        rate.sleep();
    }

    delete _RRTstar_preparatory;
    return 0;
}

void visRRTstarPath(vector<Vector3d> nodes )
{
    visualization_msgs::Marker Points, Line; 
    Points.header.frame_id = Line.header.frame_id = "world";
    Points.header.stamp    = Line.header.stamp    = ros::Time::now();
    Points.ns              = Line.ns              = "demo_node/RRTstarPath";
    Points.action          = Line.action          = visualization_msgs::Marker::ADD;
    Points.pose.orientation.w = Line.pose.orientation.w = 1.0;
    Points.id = 0;
    Line.id   = 1;
    Points.type = visualization_msgs::Marker::POINTS;
    Line.type   = visualization_msgs::Marker::LINE_STRIP;

    Points.scale.x = _resolution/2; 
    Points.scale.y = _resolution/2;
    Line.scale.x   = _resolution/2;

    //points are green and Line Strip is blue
    Points.color.g = 1.0f;
    Points.color.a = 1.0;
    Line.color.b   = 1.0;
    Line.color.a   = 1.0;

    geometry_msgs::Point pt;
    for(int i = 0; i < int(nodes.size()); i++)
    {
        Vector3d coord = nodes[i];
        pt.x = coord(0);
        pt.y = coord(1);
        pt.z = coord(2);

        Points.points.push_back(pt);
        Line.points.push_back(pt);
    }
    _RRTstar_path_vis_pub.publish(Points);
    _RRTstar_path_vis_pub.publish(Line); 
}

参考引用

demo_node.cpp注释参考了https://blog.csdn.net/weixin_43795921/article/details/102674696.
代码来源:https://github.com/KailinTong/Motion-Planning-for-Mobile-Robots/tree/master/hw_3.
相关知识来源:深蓝学院<<移动机器人运动规划>>