基于ROS的Local Lattice Planner算法代码学习
本文是在学习路径规划算法中的一篇学习记录,由于对c++掌握粗浅,将整个代码进行了详细的梳理,由于代码中有很多是 《基于ROS的A*算法代码学习》梳理过的,对本文hw_tool.h注释可参考的Astar_searcher.h; hw_tool.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.Lattice Planner
下面是结果
rviz终端信息
下面是输出的OBVP的解的根和轨迹的cost
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 <hw_tool.h>
#include "backward.hpp"
using namespace std;
using namespace Eigen;
namespace backward {
backward::SignalHandling sh;
}
// simulation param from launch file
double _resolution, _inv_resolution, _cloud_margin;
double _x_size, _y_size, _z_size;
Vector3d _start_pt, _start_velocity;
// useful global variables
bool _has_map = false;
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, _path_vis_pub;
// Integral parameter
// 最大输入加速度,会影响lattice planning离散化后的轨迹的长度
double _max_input_acc = 1.0;
// lattice planning时每个方向离散的个数，2表示x、y、z方向各三条轨迹(0,1,2)，共3*3*3=27条
int _discretize_step = 2;
// 影响lattice planning离散化后的轨迹的长度
double _time_interval = 1.25;
// 影响lattice planning离散化后的轨迹的长度
int _time_step = 50;
Homeworktool * _homework_tool = new Homeworktool();
// 为什么用四级指针？最底层指向struct->TraLibrary，外面三层分别指ax、ay、az
TrajectoryStatePtr *** TraLibrary;
void rcvWaypointsCallback(const nav_msgs::Path & wp);
void rcvPointCloudCallBack(const sensor_msgs::PointCloud2 & pointcloud_map);
void trajectoryLibrary(const Eigen::Vector3d start_pt, const Eigen::Vector3d start_velocity, const Eigen::Vector3d target_pt);
void visTraLibrary(TrajectoryStatePtr *** TraLibrary);
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");
trajectoryLibrary(_start_pt,_start_velocity,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
_homework_tool->setObs(pt.x, pt.y, pt.z);
// for visualize only
Vector3d cor_round = _homework_tool->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;
}
void trajectoryLibrary(const Vector3d start_pt, const Vector3d start_velocity, const Vector3d target_pt)
{
Vector3d acc_input;
Vector3d pos,vel;
int a =0 ;
int b =0 ;
int c =0 ;
double min_Cost = 100000.0;
double Trajctory_Cost;
// 这个地方_discretize_step加1的原因：因为_discretize_step在for循环里是从0开始的，即0、1、2，共三个数，而对应的new在分配空间
// 的时候，_discretize_step的值，2就是2,所以需要+1去做一个补偿
TraLibrary = new TrajectoryStatePtr ** [_discretize_step + 1]; //recored all trajectories after input
for(int i=0; i <= _discretize_step; i++){ //acc_input_ax
TraLibrary[i] = new TrajectoryStatePtr * [_discretize_step + 1];
for(int j=0;j <= _discretize_step; j++){ //acc_input_ay
TraLibrary[i][j] = new TrajectoryStatePtr [_discretize_step + 1];
for(int k=0; k <= _discretize_step; k++){ //acc_input_az
vector<Vector3d> Position;
vector<Vector3d> Velocity;
// 取值为-1、0、1,保证渠道x平面的每个方向
acc_input(0) = double(-_max_input_acc + i * (2 * _max_input_acc / double(_discretize_step)) );
// 取值为-1、0、1,保证渠道y平面的每个方向
acc_input(1) = double(-_max_input_acc + j * (2 * _max_input_acc / double(_discretize_step)) );
// 保证z平面不会取到平面下方
acc_input(2) = double( k * (2 * _max_input_acc / double(_discretize_step) ) + 0.1); //acc_input_az >0.1
// 这里的起点和速度都是由launch文件定义的
pos(0) = start_pt(0);
pos(1) = start_pt(1);
pos(2) = start_pt(2);
vel(0) = start_velocity(0);
vel(1) = start_velocity(1);
vel(2) = start_velocity(2);
Position.push_back(pos);
Velocity.push_back(vel);
bool collision = false;
double delta_time;
delta_time = _time_interval / double(_time_step);
for(int step=0 ; step<=_time_step ; step ++){
/*
STEP 1: finish the forward integration, the modelling has been given in the document
the parameter of forward integration: _max_input_acc|_discretize_step|_time_interval|_time_step all have been given
use the pos and vel to recored the steps in the trakectory
*/
// save the position and velocity
// TraLibrary[i][j][k]->Position = Position;
// TraLibrary[i][j][k]->Velocity = Velocity;
// High School Physics
pos(0) = pos(0) + vel(0) * delta_time + 0.5 * acc_input(0) * delta_time * delta_time;
pos(1) = pos(1) + vel(1) * delta_time + 0.5 * acc_input(1) * delta_time * delta_time;
pos(2) = pos(2) + vel(2) * delta_time + 0.5 * acc_input(2) * delta_time * delta_time;
vel(0) = vel(0) + acc_input(0) * delta_time;
vel(1) = vel(1) + acc_input(1) * delta_time;
vel(2) = vel(2) + acc_input(2) * delta_time;
Position.push_back(pos);
Velocity.push_back(vel);
double coord_x = pos(0);
double coord_y = pos(1);
double coord_z = pos(2);
//check if if the trajectory face the obstacle
if(_homework_tool->isObsFree(coord_x,coord_y,coord_z) != 1){
collision = true;
}
}
/*
STEP 2: go to the hw_tool.cpp and finish the function Homeworktool::OptimalBVP
the solving process has been given in the document
because the final point of trajectory is the start point of OBVP, so we input the pos,vel to the OBVP
after finish Homeworktool::OptimalBVP, the Trajctory_Cost will record the optimal cost of this trajectory
*/
Trajctory_Cost = _homework_tool -> OptimalBVP(pos,vel,target_pt);
//input the trajetory in the trajectory library
// 新建一条轨迹
TraLibrary[i][j][k] = new TrajectoryState(Position,Velocity,Trajctory_Cost);
//if there is not any obstacle in the trajectory we need to set 'collision_check = true', so this trajectory is useable
if(collision)
TraLibrary[i][j][k]->setCollisionfree();
//record the min_cost in the trajectory Library, and this is the part pf selecting the best trajectory cloest to the planning traget
if( Trajctory_Cost<min_Cost && TraLibrary[i][j][k]->collision_check == false ){
a = i;
b = j;
c = k;
min_Cost = Trajctory_Cost;
}
}
}
}
// 找到cost最小路径
TraLibrary[a][b][c] -> setOptimal();
visTraLibrary(TraLibrary);
return;
}
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);
_path_vis_pub = nh.advertise<visualization_msgs::MarkerArray>("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);
nh.param("planning/start_vx", _start_velocity(0), 0.0);
nh.param("planning/start_vy", _start_velocity(1), 0.0);
nh.param("planning/start_vz", _start_velocity(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);
_homework_tool = new Homeworktool();
_homework_tool -> 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 _homework_tool;
return 0;
}
void visTraLibrary(TrajectoryStatePtr *** TraLibrary)
{
double _resolution = 0.2;
visualization_msgs::MarkerArray LineArray;
visualization_msgs::Marker Line;
Line.header.frame_id = "world";
Line.header.stamp = ros::Time::now();
Line.ns = "demo_node/TraLibrary";
Line.action = visualization_msgs::Marker::ADD;
Line.pose.orientation.w = 1.0;
Line.type = visualization_msgs::Marker::LINE_STRIP;
Line.scale.x = _resolution/5;
Line.color.r = 0.0;
Line.color.g = 0.0;
Line.color.b = 1.0;
Line.color.a = 1.0;
int marker_id = 0;
// 显示每一条路径
for(int i = 0; i <= _discretize_step; i++){
for(int j = 0; j<= _discretize_step;j++){
for(int k = 0; k<= _discretize_step;k++){
if(TraLibrary[i][j][k]->collision_check == false)
{
// 设置最优路径的颜色
if(TraLibrary[i][j][k]->optimal_flag == true){
Line.color.r = 0.0;
Line.color.g = 1.0;
Line.color.b = 0.0;
Line.color.a = 1.0;
}
// 设置非最优路径的颜色
else
{
Line.color.r = 0.0;
Line.color.g = 0.0;
Line.color.b = 1.0;
Line.color.a = 1.0;
}
}
// 设置会碰到障碍物路径的颜色
else
{
Line.color.r = 1.0;
Line.color.g = 0.0;
Line.color.b = 0.0;
Line.color.a = 1.0;
}
Line.points.clear();
geometry_msgs::Point pt;
Line.id = marker_id;
for(int index = 0; index < int(TraLibrary[i][j][k]->Position.size());index++){
Vector3d coord = TraLibrary[i][j][k]->Position[index];
pt.x = coord(0);
pt.y = coord(1);
pt.z = coord(2);
Line.points.push_back(pt);
}
LineArray.markers.push_back(Line);
_path_vis_pub.publish(LineArray);
++marker_id;
}
}
}
}
hw_tool.cpp部分代码
该部分代码没有用原作者给出的代码，用的是该文章的代码https://blog.csdn.net/fb_941219/article/details/102991181?spm=1001.2014.3001.5501
double Homeworktool::OptimalBVP(Eigen::Vector3d _start_position,Eigen::Vector3d _start_velocity,Eigen::Vector3d _target_position)
{
double optimal_cost = 100000.0;
double deltaPx = _target_position(0) - _start_position(0);
double deltaPy = _target_position(1) - _start_position(1);
double deltaPz = _target_position(2) - _start_position(2);
double deltaVx = 0 - _start_velocity(0);
double deltaVy = 0 - _start_velocity(1);
double deltaVz = 0 - _start_velocity(2);
double Vx0 = _start_velocity(0);
double Vy0 = _start_velocity(1);
double Vz0 = _start_velocity(2);
double m = deltaPx*deltaPx +deltaPy*deltaPy + deltaPz*deltaPz;
double n = 2*(deltaPx*Vx0 + deltaPy*Vy0 + deltaPz*Vz0) +
deltaPx*deltaVx + deltaPy*deltaVy + deltaPz*deltaVz;
double k = 3*(Vx0*Vx0 + Vy0*Vy0 +Vz0*Vz0 + deltaVx*Vx0 + deltaVy*Vy0 + deltaVz*Vz0) +
deltaVx*deltaVx + deltaVy*deltaVy + deltaVz*deltaVz;
// Eigen库相关使用资料https://blog.csdn.net/shuzfan/article/details/52367329
Eigen::Matrix<double, 4, 4> CompanionMatrix44;
Eigen::Matrix<complex<double>, Eigen::Dynamic, Eigen::Dynamic> CompanionMatrix44EigenValues;//复数动态矩阵
double c = - 4*k;
double d = - 24*n;
double e = - 36*m;
vector<double> tmpOptimalT(4 ,0.0);
vector<double> tmpOptimalCost(4, 100000.0);
double optimalT = 0.0;
CompanionMatrix44 << 0, 0, 0, -e,
1, 0, 0, -d,
0, 1, 0, -c,
0, 0, 1, 0 ;
//std::cout<<"CompanionMatrix44: "<<std::endl<<CompanionMatrix44<<std::endl<<std::endl;
CompanionMatrix44EigenValues = CompanionMatrix44.eigenvalues();
//std::cout<<"matrix_eigenvalues: "<<std::endl<<CompanionMatrix44EigenValues<<std::endl;
cout << "---------------" << endl;
cout << CompanionMatrix44EigenValues << endl;
for(int i = 0; i < CompanionMatrix44EigenValues.size(); i++)
{
//TODO:时间不能为负数
// 判断条件怎么来的？imag() == 0，剔除特征值为复数的情况，real() > 0，时间不能为负数
// ignoring negative roots and complex roots, if all roots are complex, the function J is monotonous
if(CompanionMatrix44EigenValues(i).imag() == 0 && CompanionMatrix44EigenValues(i).real() > 0)
{
// 为什么特征值可以直接拿过来当解？参考：https://blog.csdn.net/fb_941219/article/details/102984587
tmpOptimalT[i] = CompanionMatrix44EigenValues(i).real();
double t = tmpOptimalT[i];
double t2 = t*t;
double t3 = t2*t;
tmpOptimalCost[i] = t + (12*m)/(t3) + (12*n)/(t2) + (4*k)/t;
}
else
{
continue;
}
}
// for(int i = 0; i < tmpOptimalCost.size(); i++)
// {
// optimal_cost = std::min(tmpOptimalCost[i],optimal_cost);
// }
// for(vector<double>::iterator it = tmpOptimalCost.begin(); it != tmpOptimalCost.end(); it++)
for(auto it = tmpOptimalCost.begin(); it != tmpOptimalCost.end(); it++)
{
cout << *it << endl;
}
cout << "---------------" << endl;
optimal_cost = *min_element(tmpOptimalCost.begin(), tmpOptimalCost.end());
//可以不用
int flag = 0;
for(int i = 0; i < tmpOptimalCost.size(); i++)
{
if(optimal_cost == tmpOptimalCost[i])
flag = i;
else continue;
}
optimalT = tmpOptimalT[flag];
return optimal_cost;
}
State.h
#ifndef _STATE_H_
#define _STATE_H_
#include <iostream>
#include <ros/ros.h>
#include <ros/console.h>
#include <Eigen/Eigen>
#include "backward.hpp"
struct TrajectoryState;
typedef TrajectoryState* TrajectoryStatePtr;
struct TrajectoryState
{
// 位置
std::vector<Eigen::Vector3d> Position;
// 速度
std::vector<Eigen::Vector3d> Velocity;
// 轨迹cost
double Trajctory_Cost ;
bool collision_check ; //False -> no collision, True -> collision
bool optimal_flag; //False -> not optimal in TraLibrary, True -> not optimal in TraLibrary,
// 构造函数
TrajectoryState(std::vector<Eigen::Vector3d> _Position, std::vector<Eigen::Vector3d> _Velocity,double _Trajctory_Cost){
Position = _Position;
Velocity = _Velocity;
Trajctory_Cost = _Trajctory_Cost;
collision_check = false;
optimal_flag = false;
}
TrajectoryState(){};
~TrajectoryState(){};
// 将轨迹设置为会发生碰撞
void setCollisionfree(){
collision_check = true;
}
// 设定轨迹为最优轨迹
void setOptimal(){
optimal_flag = true;
}
};
#endif
参考引用
hw_tool.cpp中对OBVP问题的求解参考：https://blog.csdn.net/fb_941219/article/details/102991181?spm=1001.2014.3001.5501.
代码来源：https://github.com/KailinTong/Motion-Planning-for-Mobile-Robots/tree/master/hw_4.
相关知识来源:深蓝学院<<移动机器人运动规划>>
4808
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
