最近在摸OriginBot的自动导航功能,写一篇博客记录一下。

分为两部分,

  1. 目录结构解释: 说明每一个文件或者代码的作用
  2. 代码和配置文件解: 对具体的代码和配置项进行解释。

后面的解释不一定正确,欢迎指正。

目录结构解释

当前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文件的注释

  1. /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]),
        ])
    
  2. /cartographer.launch.py
    此文件用于运行Cartographer节点并包含occupancy_grid节点。首先,它定义了Cartographer的配置文件及其路径。然后,创建Cartographer节点并为之提供相应的参数。最后,包含occupancy_grid.launch.py并将参数传递给它。

  3. /ekf.md
    此文件提供了关于如何使用扩展卡尔曼滤波器(EKF)节点的信息。首先,启动底盘和IMU。然后,可以使用EKF节点融合IMU和里程计数据,并发布odom到base_footprint的tf。还提供了如何调整EKF参数的说明。

  4. /nav_bringup.launch.py
    此文件用于启动导航系统。首先定义地图和导航参数文件的路径。然后包含nav2_bringup的launch文件,并将参数传递给它。此launch文件将启动所有与导航相关的节点,如全局规划器、局部规划器和控制器。

  5. /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()