最近在摸OriginBot的自动导航功能,写一篇博客记录一下。
分为两部分,
- 目录结构解释: 说明每一个文件或者代码的作用
- 代码和配置文件解: 对具体的代码和配置项进行解释。
后面的解释不一定正确,欢迎指正。
目录结构解释
当前originbot自动导航的代码在这里,目录结构如下,我对其中一些目录和文件的作用添加了注释,可以帮助在高层级理解整个目录的结构,至于具体的代码的注释看后面。
.
├── CMakeLists.txt
├── package.xml
├── config <-- 存放配置文件的目录,如地图服务器、机器人定位节点配置文件等
│ ├── ekf.yaml <-- 扩展卡尔曼滤波器 (EKF) 配置文件
│ └── lds_2d.lua <-- Cartographer配置文件
├── launch <-- 存放launch文件目录, 这些文件帮助启动多个ROS节点, 包括Cartographer SLAM, EKF nodes 和数据发布。
│ ├── cartographer.launch.py
│ ├── ekf.md
│ ├── nav_bringup.launch.py
│ ├── occupancy_grid.launch.py
│ └── odom_ekf.launch.py
├── maps <-- 存留生成地图的目录
│ ├── my_map.pgm <-- 图像格式的栅格地图(.png/.pgm)
│ └── my_map.yaml <-- 地图元数据,用于描述地图(分辨率、原点坐标等信息)
├── param <-- 存放参数文件
│ └── originbot_nav2.yaml <-- Navigation2参数文件
├── send_goal <-- 目标发送节点(包括C++和Python实现)
│ ├── CMakeLists.txt
│ ├── include <-- 允许使用`#include <my_cpp_lib/my_header.hpp>`
| 将源代码与头文件进行分离。去除依赖复杂性。
│ │ 此处未列出以简化输出。
│ ├── package.xml
│ ├── setup.py
│ └── src
│ ├── send_goal_node.cpp <-- 用C ++编写的send goal功能源代码
代码和配置文件的注释
launch文件的注释
-
/occupancy_grid.launch.py
此文件用于创建一个节点,从Cartographer中发布占用栅格地图(Occupancy Grid)。在生成的启动描述中,定义了一些启动参数,如地图栅格的分辨率和发布周期。然后,为cartographer_ros包中的occupancy_grid_node创建一个节点,并为节点提供相应的参数和参数值。from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch_ros.actions import Node from launch.substitutions import LaunchConfiguration # 定义启动函数 def generate_launch_description(): # 设置参数的默认值 use_sim_time = LaunchConfiguration('use_sim_time', default='false') resolution = LaunchConfiguration('resolution', default='0.05') publish_period_sec = LaunchConfiguration('publish_period_sec', default='1.0') # 返回包含节点和相关声明参数的启动描述对象 return LaunchDescription([ # 声明参数:网格单元格分辨率,发布周期 DeclareLaunchArgument( 'resolution', default_value=resolution, description='Resolution of a grid cell in the published occupancy grid'), DeclareLaunchArgument( 'publish_period_sec', default_value=publish_period_sec, description='OccupancyGrid publishing period'), # 是否使用仿真(Gazebo)时钟作为默认值 DeclareLaunchArgument( 'use_sim_time', default_value='false', description='Use simulation (Gazebo) clock if true'), # 定义并运行 cartographer_ros 的 occupancy_grid_node Node( package='cartographer_ros', executable='occupancy_grid_node', name='occupancy_grid_node', output='screen', parameters=[{'use_sim_time': use_sim_time}], arguments=['-resolution', resolution, '-publish_period_sec', publish_period_sec]), ])
-
/cartographer.launch.py
此文件用于运行Cartographer节点并包含occupancy_grid节点。首先,它定义了Cartographer的配置文件及其路径。然后,创建Cartographer节点并为之提供相应的参数。最后,包含occupancy_grid.launch.py
并将参数传递给它。 -
/ekf.md
此文件提供了关于如何使用扩展卡尔曼滤波器(EKF)节点的信息。首先,启动底盘和IMU。然后,可以使用EKF节点融合IMU和里程计数据,并发布odom到base_footprint的tf。还提供了如何调整EKF参数的说明。 -
/nav_bringup.launch.py
此文件用于启动导航系统。首先定义地图和导航参数文件的路径。然后包含nav2_bringup的launch文件,并将参数传递给它。此launch文件将启动所有与导航相关的节点,如全局规划器、局部规划器和控制器。 -
/odom_ekf.launch.py
此文件用于启动一个扩展卡尔曼滤波器(EKF)节点,用于融合里程计和IMU数据。首先,定义EKF配置文件的路径。然后,创建一个robot_localization
包中的ekf_node
节点。将use_sim_time设置为默认值’false’,当使用仿真时,可以修改为’true’。
originbot_nav2.yaml
这个配置文件是用于配置Navigation2的参数的,参数非常做,对于一个新人来说,没有注释很难理解,我把我对每一个参数的理解直接写在了参数的后面,请看下面的yaml文件
amcl:
ros__parameters:
use_sim_time: False # 是否使用仿真时间 (true 使用 gazebo 时间,false 使用系统时间)
alpha1: 0.2 # 机器人旋转时里程计的噪声系数
alpha2: 0.2 # 机器人平移时里程计的噪声系数
alpha3: 0.2 # 机器人平移时方位角 (yaw) 的噪声系数
alpha4: 0.2 # 机器人旋转时方位角 (yaw) 的噪声系数
alpha5: 0.2 # 指示传感器范围测距仪在检测到障碍物前后的误差的噪声参数
base_frame_id: "base_footprint" # 与机器人姿态相关的参考框架ID
beam_skip_distance: 0.5 # 跳过激光束的几何距离阈值
beam_skip_error_threshold: 0.9 # 允许错过的光束数量的上限阈值(百分比)
beam_skip_threshold: 0.3 # 射线跳过门限百分比(允许部分射线被障碍物遮挡)
do_beamskip: false # 是否启用射线跳过策略
global_frame_id: "map" # 定义全局地图坐标系ID
lambda_short: 0.1 # 高斯模型短波长因子
laser_likelihood_max_dist: 2.0 # 设置将柱子集最大似然成本的光束接收器截止距离
laser_max_range: 100.0 # 接收激光扫描数据的最大距离
laser_min_range: -1.0 # 接收激光扫描数据的最小距离
laser_model_type: "likelihood_field" # 错误模型类型 (设置为 likelihood_field)
max_beams: 60 # 要采样的最大小段宽度数量
max_particles: 2000 # 粒子滤波器中最多粒子数量
min_particles: 500 # 粒子滤波器所需的最少粒子数量
odom_frame_id: "odom" # 里程计传感器坐标系ID
pf_err: 0.05 # 预测漂移误差阈值
pf_z: 0.99 # 存活粒子占总粒子的比例
recovery_alpha_fast: 0.0 # 大幅变化恢复率
recovery_alpha_slow: 0.0 # 缓慢恢复速率
resample_interval: 1 # 重新采样间隔
robot_model_type: "differential" # 机器人运动模型类型 ("differential": 差速驱动;"omni": 全向驱动或非完整约束式)
save_pose_rate: 0.5 # 到 amcl_pose上的发布频率
sigma_hit: 0.2 # 高斯模型的标准偏差
tf_broadcast: true # 是否广播TF-transforms
transform_tolerance: 1.0 # TF-transforms容差时间(秒)
update_min_a: 0.2 # 触发位置更新所需的最小角度
update_min_d: 0.25 # 触发位置更新所需的最小距离
z_hit: 0.5 # 高斯模型的Hit组件权重
z_max: 0.05 # 最大似然成本分类的权重
z_rand: 0.5 # 均匀分布的权重
z_short: 0.05 # 决定缩短模型权重
# 下面这三个没太多意义,只要保持三者一致即可。
amcl_map_client:
ros__parameters:
use_sim_time: False
amcl_rclcpp_node:
ros__parameters:
use_sim_time: False
bt_navigator:
ros__parameters:
use_sim_time: False
global_frame: map # 全局导航坐标系
robot_base_frame: base_footprint # 机器人底盘坐标系
odom_topic: /odom # 订阅到里程计主题
default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" # 行为树文件名
plugin_lib_names:
- nav2_compute_path_to_pose_action_bt_node # 寻找路径插件
- nav2_follow_path_action_bt_node # 跟踪路径插件
- nav2_back_up_action_bt_node # 后退插件
- nav2_spin_action_bt_node # 自旋插件
- nav2_wait_action_bt_node # 等待插件
- nav2_clear_costmap_service_bt_node # 清除代价地图插件
- nav2_is_stuck_condition_bt_node # 卡住判断插件
- nav2_goal_reached_condition_bt_node # 达到目标判断插件
- nav2_goal_updated_condition_bt_node # 目标点更新判断插件
- nav2_initial_pose_received_condition_bt_node # 初始位姿已接收判断插件
- nav2_reinitialize_global_localization_service_bt_node # 重新初始化全局/局部定位服务插件
- nav2_rate_controller_bt_node # 比例控制插件
- nav2_distance_controller_bt_node # 距离控制插件
- nav2_speed_controller_bt_node # 速度控制插件
- nav2_recovery_node_bt_node # 恢复节点插件
- nav2_pipeline_sequence_bt_node # 管道序列插件
- nav2_round_robin_node_bt_node # 循环调度插件
- nav2_transform_available_condition_bt_node # 变换是否可用条件插件
- nav2_time_expired_condition_bt_node # 时间到期条件判断插件
- nav2_distance_traveled_condition_bt_node # 行进距离满足条件判断插件
bt_navigator_rclcpp_node:
ros__parameters:
use_sim_time: False
controller_server:
ros__parameters:
use_sim_time: False
controller_frequency: 10.0 # 控制器运行频率(Hz)
min_x_velocity_threshold: 0.001 # x轴最小速度阈值
min_y_velocity_threshold: 0.5 # y轴最小速度阈值
min_theta_velocity_threshold: 0.001 # theta轴最小速度阈值
controller_plugins: ["FollowPath"] # 控制器插件
# DWB parameters
FollowPath:
plugin: "dwb_core::DWBLocalPlanner"
debug_trajectory_details: True
min_vel_x: 0.0 # X轴最小速度限制
min_vel_y: 0.0 # Y轴最小速度限制
max_vel_x: 0.22 # X轴最大速度限制
max_vel_y: 0.0 # Y轴最大速度限制
max_vel_theta: 0.8 # Theta轴最大速度限制
min_speed_xy: 0.0 # XY plane最小速度限制
max_speed_xy: 0.44 # XY plane最大速度限制
min_speed_theta: 0.0 # Theta轴最小速度限制
acc_lim_x: 2.5 # X轴加速度限制
acc_lim_y: 0.0 # Y轴加速度限制
acc_lim_theta: 0.2 # Theta轴加速度限制
decel_lim_x: -0.5 # X轴减速度限制
decel_lim_y: 0.0 # Y轴减速度限制
decel_lim_theta: -0.5 # Theta轴减速度限制
vx_samples: 20 # 速度X采样量
vy_samples: 0 # 速度Y采样量
vtheta_samples: 40 # 速度Theta采样量
sim_time: 1.5 # 任务模拟时间
linear_granularity: 0.05 # 生成计划路径的线性颗粒度
angular_granularity: 0.025 # 生成计划路径的角颗粒度
transform_tolerance: 0.2 # 在执行任何操作之前等待变换的持续时间(单位:s)
xy_goal_tolerance: 0.05 # 到达目标的xy公差限值
trans_stopped_velocity: 0.25 # 机器人静止状态的最大速度
short_circuit_trajectory_evaluation: True # 启用对未完成特征的评估
stateful: True # 如果 planner 跨多个计算周期,则使得其知道自己的历史状态
controller_server_rclcpp_node:
ros__parameters:
use_sim_time: False
send_goal_node.cpp
这个文件是用于向originbot发送一个目标点,然后让其自动导航过去的。
它创建 “GoalCoordinate” 节点, 它通过将nav2_msgs::action::NavigateToPose消息类型传递给 “navigate_to_pose” 动作服务器向机器人发出目标位置指令。这个节点定期地(每500毫秒)更新目标点并发送给导航系统,让机器人移动到新的目标坐标。
具体的代码注释如下:
#include <functional>
#include <future>
#include <memory>
#include <thread>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <rclcpp_components/register_node_macro.hpp>
#include <std_msgs/msg/string.hpp>
#include <nav2_msgs/action/navigate_to_pose.hpp>
class GoalCoordinate : public rclcpp::Node
{
public:
using NavigateToPose = nav2_msgs::action::NavigateToPose; // 定义导航到特定姿态动作类型别名
using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle<NavigateToPose>; // 定义动作客户端的目标句柄类型
explicit GoalCoordinate(const rclcpp::NodeOptions &options)
: Node("GoalCoordinate", options),x(0.0)
{
// 创建动作客户端: "navigate_to_pose"
this->client_ptr_ = rclcpp_action::create_client<NavigateToPose>(
this, "navigate_to_pose");
// 使用"send_goal"方法创建定时器,并设置触发周期为500ms
this->timer_ = this->create_wall_timer(
std::chrono::milliseconds(500), std::bind(&GoalCoordinate::send_goal, this));
}
float x = 1.0;
void send_goal()
{
using namespace std::placeholders;
// 取消定时器发送任务
this->timer_->cancel();
if (!this->client_ptr_->wait_for_action_server())
{
RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting.");
rclcpp::shutdown();
}
auto goal_msg = NavigateToPose::Goal(); // 初始化达成导航目标
goal_msg.pose.header.frame_id = "map";
// 这里是hard-coding的,实际上这里应该从SLAM地图上获取一个有意义的值传过来,
// 比如
goal_msg.pose.pose.position.x = 10.0; // 设置机器人目标位置 X 坐标值
goal_msg.pose.pose.position.y = 20.0; // 设置机器人目标位置 Y 坐标值
goal_msg.pose.pose.orientation.w = 1.0; // 设置机器人目标朝向四元数 W 分量(保持不变)
RCLCPP_INFO(this->get_logger(), "Sending goal"); // 在控制台显示已发送的目标信息
auto send_goal_options = // 初始化动作客户端选项
rclcpp_action::Client<NavigateToPose>::SendGoalOptions();
send_goal_options.goal_response_callback =
std::bind(&GoalCoordinate::goal_response_callback, this, _1); // 绑定跳转目标响应回调函数
send_goal_options.feedback_callback =
std::bind(&GoalCoordinate::feedback_callback, this, _1, _2); // 绑定反馈信息处理回调函数
send_goal_options.result_callback =
std::bind(&GoalCoordinate::result_callback, this, _1); // 结果回调函数绑定结果状态处理
// 异步发送导航目标给服务器
this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
}
private:
rclcpp_action::Client<NavigateToPose>::SharedPtr client_ptr_; // 动作客户端智能指针实例
rclcpp::TimerBase::SharedPtr timer_; // 计时器智能指针实例
void goal_response_callback(
std::shared_future<GoalHandleFibonacci::SharedPtr> future)
{
auto goal_handle = future.get();
if (!goal_handle)
{
// 若目标被服务器拒绝,则输出错误信息
RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
}
else
{
// 若目标被接受,等待服务器返回结果
RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
}
}
void feedback_callback(
GoalHandleFibonacci::SharedPtr,
const std::shared_ptr<const NavigateToPose::Feedback> feedback)
{
auto distance_feedback_msg = std_msgs::msg::String();
distance_feedback_msg.data =
"Remaining Distance from Destination: " + std::to_string(feedback->distance_remaining);
RCLCPP_INFO(this->get_logger(), distance_feedback_msg.data);
}
void result_callback(const GoalHandleFibonacci::WrappedResult &result)
{
switch (result.code)
{
case rclcpp_action::ResultCode::SUCCEEDED:
break;
case rclcpp_action::ResultCode::ABORTED:
RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
return;
case rclcpp_action::ResultCode::CANCELED:
RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
return;
default:
RCLCPP_ERROR(this->get_logger(), "Unknown result code");
return;
}
rclcpp::shutdown(); // 结果完成后关闭节点
}
};
// 注册节点
RCLCPP_COMPONENTS_REGISTER_NODE(GoalCoordinate)
实现连续接收目标点导航
上面的send_goal_node.cpp里面的目标点是hard-coding的,但是在实际开发中肯定不会这会这么做,要有一个办法能让originbot动态地接收到目标点。
这里给一个简单的实现思路和代码。
用socket在originbot起一个端口用于接收目标点,再起一个ros2 服务或者node用于将目标点发送给导航程序。
socket服务接收目标点:
import socket
import json
def receive_goals():
port = 8888
buffer_size = 1024
server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
server_address = ('', port)
print(f'Starting up on {server_address[0]}:{str(server_address[1])}')
server_socket.bind(server_address)
server_socket.listen(1)
while True:
print('Waiting for a connection...')
client_connection, client_address = server_socket.accept()
try:
print(f'Connection from {client_address}')
data = b""
while True:
part_data = client_connection.recv(buffer_size)
data += part_data
if len(part_data) < buffer_size:
break
received_goals = json.loads(data.decode(encoding='utf-8'))
with open("/tmp/received_goals.json", "w+") as f:
json.dump(received_goals, f)
finally:
print("Closing the Connection")
# Clean up the connection
client_connection.close()
if __name__ == "__main__":
receive_goals()
下面的代码可以用于处理接收到的目标点,然后在发给originbot:
import rclpy
import json
from rclpy.node import Node
from nav2_msgs.action import NavigateToPose
from rclpy.action import ActionClient
class RemoteSendGoal(Node):
def __init__(self):
super().__init__('send_remote_goal')
self.action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
self._goals_list = []
def load_goals_from_server(self):
with open('/tmp/received_goals.json', 'r') as f:
goals_json = json.load(f)
self._goals_list = goals_json["goals"]
def execute_navigation(self):
return self.get_future_context(self._goals_list)
async def get_future_context(self, goals_list):
for goal in goals_list:
success = await self.set_goal(goal)
if not success:
break
async def set_goal(self, goal):
goal_msg = NavigateToPose.Goal()
goal_msg.pose.header.frame_id = 'map'
goal_msg.pose.pose.position.x = float(goal['x'])
goal_msg.pose.pose.position.y = float(goal['y'])
goal_msg.pose.pose.orientation.w = 1.0
self.get_logger().info(f"Sending goal (X: {goal['x']}, Y: {goal['y']})")
result_future = self.action_client.send_goal_async(goal_msg)
is_action_available = self.action_client.wait_for_server(timeout_sec=30.0)
if not is_action_available:
self.get_logger().error('Action Server not available!')
return False
_, result = await result_future.result()
if result.status == MoveBase.Result.STATUS_REACHED_GOAL:
self.get_logger().info('Target Pose Reached.')
elif result.status == MoveBase.Result.STATUS_DISCONNECTED_TIMEOUT:
self.get_logger().warn('Not connected to action server for longer than timeout time.')
else:
self.get_logger().info('Failure.')
return True
def main(args=None):
rclpy.init(args=args)
navigation_executor = RemoteSendGoal()
try:
navigation_executor.load_goals_from_server()
future_execution = navigation_executor.execute_navigation()
rclpy.spin_until_future_complete(navigation_executor, future_execution)
except Exception as e:
navigation_executor.get_logger().fatal(str(e))
exit(1)
navigation_executor.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
评论(0)
您还未登录,请登录后发表或查看评论