48.在ROS中实现local planner（1）- 实现一个
2023-06-27 本文已影响0人
PIBOT导航机器人
有了之前45.在ROS中实现global planner（1）- 实现一个可以用模板的global planner的经验, 现在再去创建一个local planner的包就容易多了
1. 创建包
- 创建
cd ~/pibot_ros/ros_ws/src # 这里可以使用自己的ros workspace
catkin_create_pkg sample_local_planner
-
添加类
我们需要实现一个从nav_core::BaseLocalPlanner继承的类,nav_core::BaseLocalPlanner接口类定义在这里base_local_planner.h#L50)可以看到 -
修改编译
修改CMakeLists.txt,添加相关编译参数和选项 -
添加
bgp_plugin.xml文件
指定导出的类名称
<library path="lib/libsample_local_planner">
<class name="sample_local_planner/LocalPlanner" type="sample_local_planner::LocalPlanner" base_class_type="nav_core::BaseLocalPlanner">
<description>
A sample implementation of a grid local planner
</description>
</class>
</library>
目录结构这样
❯ tree sample_local_planner
sample_local_planner
├── bgp_plugin.xml
├── CMakeLists.txt
├── include
│ └── sample_local_planner
│ └── planner_node.h
├── package.xml
└── src
└── planner_node.cpp
- 导出类
参考navigation里面, 添加宏导出该类
PLUGINLIB_EXPORT_CLASS(sample_local_planner::LocalPlanner, nav_core::BaseLocalPlanner)
2. 接口实现
2.1 接口
base_local_planner.h#L50)可以看到接口类
namespace nav_core {
class BaseLocalPlanner{
public:
virtual bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel) = 0;
virtual bool isGoalReached() = 0;
virtual bool setPlan(const std::vector<geometry_msgs::PoseStamped>& plan) = 0;
virtual void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros) = 0;
};
}; // namespace nav_core
通过命名大概就知道其定义，
-
initialize
初始化接口，给我们传相关功能接口的，如tf，costmap -
setPlan
规划控制接口，给我们提供一个plan，这个应该是global planner的输出，通过move_base转了一手给到我们，后面可以看下move_base源码 -
computeVelocityCommands
计算速度，传入的参数是一个引用，应该是输出函数，我们把计算好的速度填进去就可以 -
isGoalReached
获取是否以及到达目标点
2.2 不同ros版本接口差异
BaseLocalPlanner在ros kinetic 中的initialize接口稍有差异 见base_local_planner.h#L78
// kinetic
virtual void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros) = 0;
// melodic&noetic
virtual void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros) = 0;
后面我们以melodic&noetic实现
2.3 实现
主要代码如下，stopwatch_为计时器，我们在setPlan调用后，设置变量，computeVelocityCommands接口中设置固定的速度，在时间到达后，输出0，同时isGoalReached接口返回true
void LocalPlanner::initialize(std::string name, tf::TransformListener *tf,
costmap_2d::Costmap2DROS *costmap_ros)
{
ROS_INFO("LocalPlanner initialize");
}
bool LocalPlanner::computeVelocityCommands(geometry_msgs::Twist &cmd_vel)
{
ROS_INFO("LocalPlanner computeVelocityCommands");
if (start_flag_) {
cmd_vel.linear.x = 0.2;
cmd_vel.linear.y = 0;
cmd_vel.angular.z = 0.8;
} else {
cmd_vel.linear.x = 0;
cmd_vel.linear.y = 0;
cmd_vel.angular.z = 0;
}
return true;
}
bool LocalPlanner::setPlan(const std::vector<geometry_msgs::PoseStamped> &orig_global_plan)
{
ROS_INFO("LocalPlanner setPlan");
if (!start_flag_) {
start_flag_ = true;
stopwatch_.reset();
}
return true;
}
bool LocalPlanner::isGoalReached()
{
if (stopwatch_.elapsed(std::chrono::seconds(2)))
{
ROS_INFO("LocalPlanner GoalReached");
return true;
}
return false;
}
通过查看move_base源码，上面几个接口是在同一个线程被调用，所有后续不需要考虑资源竞争，即变量无需加锁
3. 测试
3.1 编译
cd ~/pibot_ros/ros_ws
catkin_make
3.2 测试
修改~/pibot_ros/src/pibot_simulator/move_base_params.yaml
# base_local_planner: "dwa_local_planner/DWAPlannerROS"
base_local_planner: sample_local_planner/LocalPlanner
dwa_local_planner/DWAPlannerROS ----> sample_local_planner/LocalPlanner
- 启动模拟器
pibot_simulator
- 查看当前的
local_planner
❯ rosparam get /move_base/base_local_planner
sample_local_planner/LocalPlanner # 输出sample_local_planner/LocalPlanner表示插件已经被正确加载
- 启动rviz发送点位，选点导航测试
pibot_view
3.3 测试结果
[ INFO] [1676647988.863610652]: make plan start:[0.000000 0.000000], goal:[-2.986773 4.282055]
[ INFO] [1676647989.063781836]: LocalPlanner setPlan
[ INFO] [1676647989.064015702]: LocalPlanner computeVelocityCommands
[ INFO] [1676647989.263707871]: LocalPlanner computeVelocityCommands
[ INFO] [1676647989.463771479]: LocalPlanner computeVelocityCommands
[ INFO] [1676647989.663754028]: LocalPlanner computeVelocityCommands
[ INFO] [1676647989.863583610]: LocalPlanner computeVelocityCommands
[ INFO] [1676647989.864067517]: make plan start:[0.000000 0.000000], goal:[-2.986773 4.282055]
[ INFO] [1676647990.063701815]: LocalPlanner setPlan
[ INFO] [1676647990.063874092]: LocalPlanner computeVelocityCommands
[ INFO] [1676647990.263710418]: LocalPlanner computeVelocityCommands
[ INFO] [1676647990.463773749]: LocalPlanner computeVelocityCommands
[ INFO] [1676647990.663630163]: LocalPlanner computeVelocityCommands
[ INFO] [1676647990.863635728]: LocalPlanner computeVelocityCommands
[ INFO] [1676647990.864087581]: make plan start:[0.000000 0.000000], goal:[-2.986773 4.282055]
[ INFO] [1676647991.063713670]: LocalPlanner setPlan
[ INFO] [1676647991.063894899]: LocalPlanner computeVelocityCommands
[ INFO] [1676647991.263639509]: LocalPlanner GoalReached
通过日志可以看出
- 在全局规划(
make plan start是我们前面文章新增的astar planner输出)后LocalPlanner的接口setPlan被调用 -
computeVelocityCommands函数没0.2s被调用一次, 期间机器人也在做圆周运动 - 全局规划再次被调用(
move_bsae里配置了规划频率1hz，这里可以看到间隔1s全局规划一次)，重复前面的 - 直到超时
GoalReached返回true完成
4. 总结
本文简单实现了一个local planner的插件，显然实际没啥用，不过可以作为一个模板，基于该模板实现自己的算法。后面我们将基于该模板实现可用的局部规划控制。
本文代码见sample_local_planner
