1.Navigation包 Global_planner全局路径规划源码详细解析
2.cartographer_ros定位功能位姿获取与重定位设置
3.解决python3无法使用ROS中tf的问题

Navigation包 Global_planner全局路径规划源码详细解析

       学习总结，如有错误欢迎指正！

一丶plan_node.cpp

       从程序入口开始，首先在plan_node.cpp的main函数中，初始化了全局路径规划器。

costmap_2d::Costmap2DROS?资金沉淀公式源码lcr("costmap",?buffer);global_planner::PlannerWithCostmap?pppp("planner",?&lcr);

       在函数PlannerWithCostmap中设置了两种调用makePlan的路径：

PlannerWithCostmap::PlannerWithCostmap(string?name,?Costmap2DROS*?cmap)?:GlobalPlanner(name,?cmap->getCostmap(),?cmap->getGlobalFrameID())?{ ros::NodeHandle?private_nh("~");cmap_?=?cmap;make_plan_service_?=?private_nh.advertiseService("make_plan",?&PlannerWithCostmap::makePlanService,?this);pose_sub_?=?private_nh.subscribe<rm::PoseStamped>("goal",?1,?&PlannerWithCostmap::poseCallback,?this);}

       1.通过make_plan服务

req.start.header.frame_id?=?"map";req.goal.header.frame_id?=?"map";bool?success?=?makePlan(req.start,?req.goal,?path);

       2.通过goal回调函数

//得到当前机器人在MAP中的位置cmap_->getRobotPose(global_pose);makePlan(global_pose,?*goal,?path);

       在getRobotPose函数中，通过tf_.transform(robot_pose, global_pose, global_frame_);函数，默认将机器人pose从base_link转换到map坐标系下，可通过参数设置。得到起始点和目标点传入到makePlan中。

二丶 planner_core.cpp//register?this?planner?as?a?BaseGlobalPlanner?pluginPLUGINLIB_EXPORT_CLASS(global_planner::GlobalPlanner,?nav_core::BaseGlobalPlanner)

       global_planner 是基类nav_core :: BaseGlobalPlanner的一个插件子类

       首先在构造函数中需要初始化GlobalPlanner，在initialize中对一些参数进行赋值。在线的oa源码

GlobalPlanner::GlobalPlanner(std::string?name,?costmap_2d::Costmap2D*?costmap,?std::string?frame_id)?:GlobalPlanner()?{ //initialize?the?plannerinitialize(name,?costmap,?frame_id);}

       当调用makePlan时，首先就是判断是否已经被初始化：

//?code?line?~makePlan()if?(!initialized_)?{ ROS_ERROR("This?planner?has?not?been?initialized?yet,?but?it?is?being?used,?please?call?initialize()?before?use");return?false;}m

       初始化完成之后，清除之前规划的Plan，以防万一。然后检查起点和终点是否在我们所需要的坐标系下，一般在map系下。

//clear?the?plan,?just?in?case?，?code?line?makePlan()plan.clear();if?(goal.header.frame_id?!=?global_frame)?{ ...}if?(start.header.frame_id?!=?global_frame){ ...}

       将世界坐标系的点(map 坐标系)转换成图像坐标系(图像左下角)下的点(以像素表示)

if?(!costmap_->worldToMap(wx,?wy,?goal_x_i,?goal_y_i))?{ ?ROS_WARN_THROTTLE(1.0,"The?goal?sent?to?the?global?planner?is?off?the?global?costmap.?Planning?will?always?fail?to?this?goal.");?return?false;}

       在Costmap2D和GlobalPlanner中都有实现worldToMap，其实都是一样的，在GlobalPlanner中则需要通过调用Costmap2D来获取局部地图的起始点和分辨率，而在Costmap2D则可以直接使用全局变量。open函数实现源码

bool?Costmap2D::worldToMap(double?wx,?double?wy,?unsigned?int&?mx,?unsigned?int&?my)?const{ if?(wx?<?origin_x_?||?wy?<?origin_y_)return?false;mx?=?(int)((wx?-?origin_x_)?/?resolution_);my?=?(int)((wy?-?origin_y_)?/?resolution_);if?(mx?<?size_x_?&&?my?<?size_y_)return?true;return?false;}

       old_navfnbehavior ?作为一种旧式规划行为：

       The start of the path does not match the actual start location.

       The very end of the path moves along grid lines.

       All of the coordinates are slightly shifted by half a grid cell

       现在在worldToMap所使用的convert_offset_ = 0

       接下来将机器人Robot所在的位置，在costmap中设置成free，当前位置不可能是一个障碍物。 即在clearRobotCell()函数中：mx，my即当前机器人位置。

PlannerWithCostmap::PlannerWithCostmap(string?name,?Costmap2DROS*?cmap)?:GlobalPlanner(name,?cmap->getCostmap(),?cmap->getGlobalFrameID())?{ ros::NodeHandle?private_nh("~");cmap_?=?cmap;make_plan_service_?=?private_nh.advertiseService("make_plan",?&PlannerWithCostmap::makePlanService,?this);pose_sub_?=?private_nh.subscribe<rm::PoseStamped>("goal",?1,?&PlannerWithCostmap::poseCallback,?this);}0

       设置规划地图边框：outlineMap，此函数由参数outline_map_决定。 根据costmap跟起始终止点计算网格的potential，计算的算法有两种：Dijkstra和A*，具体算法便不再讨论，资料很多。大番薯完整源码 当提取到plan之后，调用getPlanFromPotential，把path转换变成geometry_msgs::PoseStamped数据类型。

PlannerWithCostmap::PlannerWithCostmap(string?name,?Costmap2DROS*?cmap)?:GlobalPlanner(name,?cmap->getCostmap(),?cmap->getGlobalFrameID())?{ ros::NodeHandle?private_nh("~");cmap_?=?cmap;make_plan_service_?=?private_nh.advertiseService("make_plan",?&PlannerWithCostmap::makePlanService,?this);pose_sub_?=?private_nh.subscribe<rm::PoseStamped>("goal",?1,?&PlannerWithCostmap::poseCallback,?this);}1

       此时便得到所需要的路径plan，最终调用OrientationFilter平滑之后发布出去。

PlannerWithCostmap::PlannerWithCostmap(string?name,?Costmap2DROS*?cmap)?:GlobalPlanner(name,?cmap->getCostmap(),?cmap->getGlobalFrameID())?{ ros::NodeHandle?private_nh("~");cmap_?=?cmap;make_plan_service_?=?private_nh.advertiseService("make_plan",?&PlannerWithCostmap::makePlanService,?this);pose_sub_?=?private_nh.subscribe<rm::PoseStamped>("goal",?1,?&PlannerWithCostmap::poseCallback,?this);}2

cartographer_ros定位功能位姿获取与重定位设置

       小白在机器人定位项目中使用cartographer进行定位，面临位姿获取频率低的问题，测试显示激光频率~hz时，获取位姿频率仅为5hz。为解决此问题，尝试了以下方法：

       首先，仿小米企业源码对cartographer进行源码安装。

       接着，运行cartographer_rosd的demo，参考.launch运行文件和.lua配置文件，并开启pure localization功能。

       尝试通过tf变换获取位姿坐标，但发现频率受限于5hz左右。为解决此问题，建立tf变换节点，发布2D位姿topic，实现高频率的位姿获取。

       深入源码修改，添加::ros::Publisher _pose_pub，对 node.cc进行调整，从而获取高频率的位姿topic。

       在rviz中，利用“2D Pose Estimate”功能，发布/initialpose话题，并根据机器人当前位置进行重定位设置。通过点击地图位置，发布包括x,y和theta的topic进行重定位。

       总结流程，通过源码修改和rviz重定位设置，实现了高频率的位姿获取及重定位功能。若有疑问，欢迎交流，共同学习。

解决python3无法使用ROS中tf的问题

       解决在ubuntu.中Python3无法使用ROS中tf的问题，通常需要对环境进行适当的调整。

       操作如下：

       首先，检查.bashrc文件中的workspace路径设置。若之前在.bashrc中添加了workspace的路径，为了避免后续可能出现的问题，应考虑在进行后续操作前将.bashrc中source的路径进行注释。

       确保系统能够正常执行catkin build命令。若能，则可直接进行后续操作；若不能，应首先安装catkin。

       接下来，返回主目录并创建一个新的仓库，并进行相关配置。

       随后，执行克隆几何学相关库的操作。

       编译操作完成后，如果遇到报错No module named em的情况，说明需要安装empy模块。

       完成上述步骤后，编译工作就告一段落，配置成功。

       为了确保在Python3环境下能够顺畅使用ROS中的tf模块，建议将新创建的secondary_build_ws仓库添加到.bashrc中，且注意调整.bashrc中的顺序。

       正确的顺序至关重要，错误的顺序可能导致在后续使用过程中出现import失败的问题。

       完成.bashrc中的配置调整后，关闭当前命令行窗口，重新打开一个新的命令行窗口。

       执行测试操作，若无报错出现，则证明配置过程已完成，Python3环境能够正常调用ROS中的tf模块。