建设品牌网站,自己的网站怎么赚钱,手把手制作公司网站,郑州市建设劳务服务网站前文move_base介绍#xff08;4#xff09;简单介绍move_base的全局路径规划配置#xff0c;接下来我们自己实现一个全局的路径规划
1. move_base规划配置
ROS1的move_base可以配置选取不同的global planner和local planner#xff0c; 默认move_base.cpp#L70中可以看到是…前文move_base介绍4简单介绍move_base的全局路径规划配置接下来我们自己实现一个全局的路径规划
1. move_base规划配置
ROS1的move_base可以配置选取不同的global planner和local planner 默认move_base.cpp#L70中可以看到是读取该参数决定的 private_nh.param(base_global_planner, global_planner, std::string(navfn/NavfnROS));private_nh.param(base_local_planner, local_planner, std::string(base_local_planner/TrajectoryPlannerROS));我们可以通过配置base_global_planner和base_local_planner参数修改不同的算法
ros1 navigation中提供了3种base_global_planner, 分别是
navfn/NavfnROSglobal_planner::GlobalPlannercarrot_planner/CarrotPlanner
下面我们自己实现一个全局的路径规划并在模拟器测试其执行效果
2. 实现原理
2.1 加载对象
private_nh.param(base_global_planner, global_planner, std::string(navfn/NavfnROS));上面我们已经知道 通过参数配置来决定加载哪一个全局规划器继续跟踪可以看到
查看源码 move_base.cpp#L125 move_base.h#L210
pluginlib::ClassLoadernav_core::BaseGlobalPlanner bgp_loader_;
planner_ bgp_loader_.createInstance(global_planner);pluginlib可以参见这里
pluginlib::ClassLoadernav_core::BaseGlobalPlanner::createInstance根据输入参数名加载so并且获取到库的导出类且创建该类的一个实例planner_即为该指向该实例的指针 有了这个对象就可以通过该成员干活了
2.2 BaseGlobalPlanner接口
planner_定义在move_base.h#L185
boost::shared_ptrnav_core::BaseGlobalPlanner planner_;前面返回的planner_类型可以看到是nav_core::BaseGlobalPlanner类型我们先来看下该类在nav_core#L48
class BaseGlobalPlanner{public:virtual bool makePlan(const geometry_msgs::PoseStamped start, const geometry_msgs::PoseStamped goal, std::vectorgeometry_msgs::PoseStamped plan) 0;virtual bool makePlan(const geometry_msgs::PoseStamped start, const geometry_msgs::PoseStamped goal, std::vectorgeometry_msgs::PoseStamped plan,double cost){cost 0;return makePlan(start, goal, plan);}virtual void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) 0;virtual ~BaseGlobalPlanner(){}protected:BaseGlobalPlanner(){}};可以看到该类是一个接口类需要继承该接口做相应的实现主要接口比较简单就两个, initialize和makePlan 顾名思义一个初始化一个规划路径
initialize 传入了name 以及地图信息makePlan 传入起点目标点返回plan
我们也可以看看在move_base对应接口的调用 move_base.cpp#L126 在创建完成立即调用完成初始化 move_base.cpp#L496 进行全局路径规划
if(!planner_-makePlan(start, goal, plan) || plan.empty()){...}在MoveBase::makeplan调用了该函数返回的plan 保存后用于local planner的输入
3. 实现global planner
3.1 实现步骤
实现一个自己的全局规划需要下面几个步骤
继承nav_core::BaseGlobalPlanner实现接口导出该实现类添加plugin.xml插件描述文件并导出修改move_base配置使用
3.2 实现接口
创建包
mkdir -p ~/pibot_ros/ros_ws/src
cd ~/pibot_ros/ros_ws/src
catkin_create_pkg sample_global_planner创建完成添加一个cpp和h文件新增一个类继承与nav_core::BaseGlobalPlanner 上面已经看到该接口定义 我们继承并对两个接口initialize和makePlan实现即可
initialize 初始化我们暂时先空实现
void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros)
{
}makePlan 规划路径的接口给我们输入起点和终点我们输出规划出的plan(如可以规划同时返回true反之返回false) 我们暂时不考虑具体实现输出一条从起点到终点的直线路径这应该是初中几何知识比较简单如下
bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped start,const geometry_msgs::PoseStamped goal, std::vectorgeometry_msgs::PoseStamped plan)
{ROS_INFO(make plan start:[%f %f], goal:[%f %f], start.pose.position.x, start.pose.position.y, goal.pose.position.x, goal.pose.position.y);plan.clear();float yaw atan2(goal.pose.position.y - start.pose.position.y, goal.pose.position.x - start.pose.position.x);int n 0;float goal_distance sqrt(pow((start.pose.position.x - goal.pose.position.x), 2) pow((start.pose.position.y - goal.pose.position.y), 2));float delta 0.1; // 间隔delta输出start至end的直线上的点 我们间隔0.1取直线上的所有点放到输出的参数plan里while (n * delta goal_distance){geometry_msgs::PoseStamped pose goal;pose.pose.position.x (n * delta) * cos(yaw) start.pose.position.x;pose.pose.position.y (n * delta) * sin(yaw) start.pose.position.y;n;plan.push_back(pose);}plan.push_back(goal); // 这里别忘了终点return !plan.empty();
}添加相应的CMakeList.txt
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(include${catkin_INCLUDE_DIRS}
)## Declare a C library
add_library(${PROJECT_NAME}src/planner_node.cpp
)## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})3.3 导出类
参考navigation里面, 添加宏导出该类
PLUGINLIB_EXPORT_CLASS(sample_global_planner::GlobalPlanner, nav_core::BaseGlobalPlanner)3.3 添加plugin.xml
添加一个bgp_plugin.xml
library pathlib/libsample_global_plannerclass namesample_global_planner/GlobalPlanner typesample_global_planner::GlobalPlanner base_class_typenav_core::BaseGlobalPlannerdescriptionA sample implementation of a grid based planner /description/class
/library3.4 编译
cd ~/pibot_ros/ros_ws
catkin_make3.5 修改配置测试
修改~/pibot_ros/src/pibot_simulator/move_base_params.yaml
# base_global_planner: global_planner/GlobalPlanner
base_global_planner: sample_global_planner/GlobalPlannerglobal_planner/GlobalPlanner ---- sample_global_planner/GlobalPlanner
启动模拟器
pibot_simulator查看当前的global_planner
❯ rosparam get /move_base/base_global_planner
sample_global_planner/GlobalPlanner # 输出sample_global_planner/GlobalPlanner表示插件已经被正确加载启动rviz发送点位选点导航测试
pibot_view3.6 路径显示
上面测试可以看到可以规划已经完成 dwa的局部规划已经启动 为了方便查看全局全规划路径的输出我们在makeplan完成后发出path的topic
void GlobalPlanner::publishPlan(const std::vectorgeometry_msgs::PoseStamped path)
{nav_msgs::Path gui_path;gui_path.poses.resize(path.size());gui_path.header.frame_id frame_id_;gui_path.header.stamp ros::Time::now();for (unsigned int i 0; i path.size(); i){gui_path.poses[i] path[i];}plan_pub_.publish(gui_path);
}把 rviz Global Map和Local Map中的dwa planner关闭 只显示Full Plan 修改move_base_params.yaml中planner_frequency值 0 只规划一次 0 规划频率 3.7 测试结果
选择空旷区域可以看到可以正常规划同时控制也可以启动完成到达目的地 跨过障碍物可以看到规划出路径显然无法控制过去
4. 总结
本文简单实现了一个global planner的插件显然实际没啥用不过可以作为一个模板基于该模板实现自己的算法。后面我们将基于该模板实现可用的全局规划。
本文代码见sample_global_planner
