提炼总结—ROS2机器人开发（第9章）（下）

写在最前面的话

为什么做该博客？该博客的特点是什么？
随着DeepSeek、ChatGPT等AI技术的崛起，促使机器人技术发展到了新的高度，诞生了宇树科技、特斯拉为代表的人形机器人，四足机器人等等，越来越多的科技巨头涌入机器人赛道，行业对于相关人才的需求也随之达到了顶峰。本博客的内容是替你阅读所有关于机器人的经典书籍，采用书籍瘦身计划，帮你提炼出核心内容，采用最通俗易懂的语言来解释原理，将书读薄。大大缩短学习时间，助你快速成为机器人时代的佼佼者。

---

---

一、书籍介绍

《ROS2机器人开发 从入门到实践》是一本从零基础到实战落地的机器人开发指南，通过"基础理论+工具详解+项目实战"的三阶学习路径，带你掌握机器人操作系统ROS2核心架构。书中不仅包含通信机制、URDF建模等必备知识，更有SLAM导航、机械臂控制等12个工业级案例，配合Git代码库和仿真环境配置指南，帮助开发者快速实现从算法仿真到真机部署的完整闭环。无论是学生、工程师还是科研人员，都能通过本书构建系统的ROS2开发能力，抢占机器人技术前沿阵地。

---

二、第9章 搭建一个实体移动机器人（提炼总结）

9.4节 使用micro-ROS接入ROS2（提炼总结）

• 上一节实现了对机器人速度的控制和里程的计算，如果需要配合导航使用，需要将数据接入到ROS2系统中，将控制指令和里程计变成ROS2的话题。micro-ROS是一组运行在微控制器的软件，通过它可以在微控制器上使用话题订阅发布等API进行通信
  

9.4.1节 第一个节点（提炼总结）

• micro-ROS的整体框架如下图所示：
  
• 深色组件是专为micro-ROS开发的，浅色组件取自标准ROS2软件
• 最左边的ROS2 Agent是micro-ROS在正常系统上的代理，它通过串口、蓝牙或者以太网等协议和微处理器平台的micro-ROS进行连接,并进行数据的转发。
• 中间深色部分，最下面RTOS部分的micro-ROSarduino是基于arduino开发的代码库，中间的中间
  件部分是经过优化的微型DDS适配器和客户端，上面的客户端库则是提供了一套基于rcle的API接口。
• 最右侧是用于嵌入式软件基准测试的工具。
• 要将微控制器连接到ROS2中，需要做两部分工作
  1、在正常系统中安装Agent
  2、在微控制器中编写micro-ROS程序

1、先在系统中安装Agent，在主目录下新建chapt9/fishbot_ws/src目录，接着克隆micro-ROS Agert源码到src目录，指令如下：

cd fishbot_ws/src

git clone https://github.com/micro-ROS/micro-ROS-Agent.git -b $ROS_DISTRO

git clone https://github.com/micro-ROS/micro_ros_msgs.git -b $ROS_DISTRO

2、进行功能包构建，之后就可以直接运行Agent了

colcon build

source install/setup.bash

ros2 run micro_ros_agent micro_ros_agent udp4 --port 8888

启动了节点，并使用udp4作为传输协议，指定端口号为8888

3、在原来的工程里引入micro-ROS，修改原工程platform.ini文件，添加依赖库和配置

board_microros_transport = wifi
lib_deps = 
	https://gitee.com/ohhuo/micro_ros_platformio.git

这里添加了micro-ROS的platformio版本依赖库，并添加了传输协议配置项，指定使用wifi与ROS2 Agent进行连接
4、修改main.cpp代码，来创建第一个micro-ROS节点

#include 
#include 
#include 
#include 
#include 

float out_right_speed;

//声明相关的结构体对象
rcl_allocator t allocator;
rclc_support_t support;
rclc_executor_t executor;
rcl_node_t node;

//单独创建一个任务运行micro-ROS,相当于一个线程
void micro_ros_task(void *parameter){
  //1.设置传输协议并延时等待设置完成
  IPAddress agent_ip;
  agent_ip.fromstring("192.168.4.136");//替换为你自己主机的IP地址
  set_microros_wifi_transports("WIPI_NAME", "WIFI_PASSSWORD", agent_ip,8888)
  delay(2000);

  //2,初始化内存分配器
  allocator = rcl get_default_allocator();

  //3.初始化support
  rclc_support_init(&support,0,NULL,&allocator)

  //4.初始化节点fishbot_motion_control
  rclc_node_init_default(&node, "fishbot_motion_contrrol","",&support);

  //5.初始化执行器
  unsigned int num handles = 0;
  rclc_executor_init(&executor, &support.context, num_handlLes,&allocator);

  //循环执行器
  rclc_executor_spin(&executor);
}

void setup(){
  //创建任务运行microrostask
  xTaskCreate(micro_ros_task,
  "micro ros",
  10240,
  NULL
  1,
  NULL
  );
}

• 首先引入了三个micro-ROS相关的头文件，接着声明了内存分配器allocator，由于微处理器平台资源有限，所以使用allocator进行内存的分配和回收。声明了support,用于存储时钟、内存分配器和上下文，提供支持。声明了executor，用于管理订阅和计时器回调的执行。声明了node，用于存储节点。
• 创建了一个名为microrostask的单独任务函数，负责初始化并运行micro-ROS节点，任务在嵌入式系统中可以简单理解为线程。
• 在任务函数中
  1、第一步设置传输协议为无线网络，Wi-Fi,这里需要设置Agent所在系统的IP地址、端口和Wi-Fi信息。当开发板连接到Wi-Fi后就会尝试发送数据到Agent所在的IP地址和端口。
  2、第二步rcl_getdefault_allocator函数使用默认设置初始化allocator。
  3、第三步调用rclc_support_init初始化support，该函数的四个参数分别是rclc_support_t结构体指针、参数数量、参数数组指针和分配器指针。
  4、第四步调用rclc_node_init_default初始化节点，该函数有四个参数，分别是rcl_node_t指针、节点名称、命名空间和relc_support_t指针。
  5、第五步调用rclc_executor_init初始化执行器，一共有四个参数，分别是rclcexecutort指针、support中的上下文指针、可处理的句柄数量和分配器的指针。
  6、第六步调用rclc_executor_spin，对执行器的事件进行不断的循环处理。
  7、最后在setup函数中，通过xTaskCreate函数来创建任务，将其添加到系统的任务队列中运行。

5、将代码下载到开发板，连接成功后可以看到micro-ROS Agent终端的提示，表示连接成功
在终端运行agent

ros2 run micro_ros_agent micro_ros_agent udp4 --port 8888

6、打开新的终端查看节点列表，可以看到来自微控制器fishbot_motion_control节点

ros2 node list

9.4.2节 订阅话题控制机器人（提炼总结）

• 上一节将微处理器中的节点通过Agent接入了ROS2系统中，本节在微控制器平台上创建话题订阅者，订阅目标角速度和线速度指令

1、在main.cpp添加如下代码

#include 
rcl subscription t subscriber;//订阅者
geometry_msgs_msg_Twist sub_msg;//存储订阅到的速度消息

void twist_callback(const void *msg_in){
  //将接收到的消息指针转化为geometry_msgs_msg_Twist类型
  const geometry_msgs_msg_Twist *twist_msg = (const geometry_msgs_msg_Twist *)msg_in;
  //运动学逆解并设置速度
  kinematics.kinematic_inverse (twist_msg->linear.x * 1000, twist_msg->angular.z, out_left_speed, out_right_speed);
  pid_controller[0].update_target(out_left_speed);
  pid_controller[1].update_target (out_right_speed);
}
void micro_ros_task(void *parameter){
  //4.初始化执行器
  unsigned int num_handles = 0+1;
  rclc_executor_init(&executor, &support.conntext, num_handles, &allocator);
  //5.初始化订阅者并添加到执行器中
  rclc_subscription init_best_effort(&subscriber, &node,ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),"/cmd_vel");
  rclc_executor_add_subscription(&executor, &subsscriber, &sub_msg , &twist_callback, ON_NEW_DATA);
  rclc_executor_spin(&executor);
}

2、将代码下载到开发板，查看话题，可以看到多出了一个/cmd_vel话题的订阅者，消息接口类型是geometry_msgs/msg/Twist，可以使用ROS2的键盘控制节点来向这个话题发布速度指令

ros2 run telop_twist_keyboard telop_twist_keyboard

9.4.3节 发布机器人里程计话题（提炼总结）

• 在ROS2中，里程计消息接口为nav_msgs.msg/Odometry

1、修改main.cpp，添加发布者、进行时间同步和创建定时器

#include 
#include 

rcl_publisher_t odom_publisher;//发布者
nav_msgs_msg_Odometry odom_msg;//里程计消息
rcl_timer_t timer;//定时器,可以定时调用某个函数

//在定时器回调函数中完成话题发布
void callback_publisher(rcl_timer_t *timer, int64_t last_call_time){
  odom t odom=kinematics.get_odom();//获取里程计
  int64_tstamp=rmw_uros_epoch_millis();//获取当前时间
  odom_msg.header.stamp.sec = static_cast<int32_t>(stamp/1000);//秒部分
  //纳秒部分
  odom_msg.header.stamp.nanosec = static_cast<uint322_t>((stamp ¥1000) * 1e6)
  odom_msg.pose.pose.position.x = odom.x;
  odom_msg.pose.pose.position.y = odom.y;
  odom_msg.pose.pose.orientation.w = cos (odom.angle * 0.5);
  odom_msg.pose.pose.orientation.x = 0;
  odom_msg.pose.pose.orientation.y = 0;
  odom_msg.pose.pose.orientation.z = sin(odom.angle*0.5)
  odom_msg.twist.twist.angular.z = odom.angle_speed 
  odom_msg.twist.twist.linear.x = odom.linear_speed
  //发布里程计
  if(rcl_publish (&odom_publisher, &odom_msg, NULL)!==RCL_RET_OK){
    Serial.printf("error: odom publisher failed!\n");
  }
}

void micro_ros_task(void *parameter){
  unsigned int num_handles = 0 + 2;
  rclc_executor_init(&executor,&support.conteext, num_handles, &allocator);
  //6.初始化发布者和定时器
  odom_msg.header.frame_id = micro_ros_string_utilities_set(odom_msg.header.frame_id, "odom");
  odom_msg.child_frame_id = micro_ros_string_utilities_set(odom_msg.child_frrame_id, "base_footprint");
  rclc_publisher_init_best_effort(&odom_publisher, &node,ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs, msg, odomnetry),"/odom");
  //7.时间同步
  while(!rmw_uros_epoch_synchronized()){//如果没有同步
    rmw_uros_sync_session(1000);//尝试进行时间同步
    delay (10);
  }
  //8.创建定时器,间隔50ms发布调用一次callback_publisher发布里程计话题
  rclc_timer_init_default(&timer, &support,RCL_MSS_TO_NS (50), callback_publisher);
  rclc_executor_add_timer(&executor,&timer);
  //循环执行器
  rclc_executor_spin(&executor);
}

2、重新构建工程并下载到开发板，在主机端运行Agent，连接成功后可以看到里程计话题已经出现了

完成里程计话题发布和速度命令的控制就完成了机器人底盘控制系统了，下面可以实现机器人建图和导航

9.5节 移动机器人建图与导航实现（提炼总结）

• 前几节完成了机器人底盘控制系统的开发，实现了里程计话题和速度话题发布，但是要运行建图和导航，还需要准备雷达话题和坐标变换

9.5.1节 驱动并显示雷达点云（提炼总结）

• FishBot采用的是YdLidarX2单线激光雷达，配合串口转无线转接板，实现将雷达的数据通过无线网络转发到计算机上
  

1、下载雷达串口转wifi转接板的驱动

cd chapt9/fishbot_ws/src

git clone https://github.com/fishros/ros_serial2wifi.git

构建工作空间，运行串口转wifi驱动，确保8889端口映射到本地的/tmp/tty_laser

colcon build

source install/setup.bash

ros2 run ros_serial2wifi tcp_server --ros-args -p serial_port:=/tmp/tty_laser

之后使用cat /tmp/tty_laser可以查看到来自雷达的数据，但是需要雷达驱动才能解析出数据的内容
2、下载雷达驱动

cd fishbot_ws/src

git clone https://github.com/fishros/ydlidar_ros2.git -b fishbot

修改雷达驱动的配置文件ydlidar_ros2/params/ydlidar.yaml，修改端口号为/tmp/tty_laser，修改frame_id为laser_link，重新构建工作空间并运动雷达驱动

colcon build

source install/setup.bash

ros2 launch ydlidar ydlidar_launch.py

接着就可以看到雷达话题/scan了，通过命令行就可以输出一帧话题数据

ros2 topic echo /scan --once

至此雷达的话题有了，目前需要建图和导航，还需要准备坐标变换

9.5.2节 移动机器人的坐标系框架介绍（提炼总结）

• REP-105为移动平台的坐标系框架，主要规定了移动机器人坐标系的位置、连接规范和连接维护组件
  1、基坐标系base_link，该坐标系固定在移动机器人的基座上，为了让机器人的轮子贴合地面，还会使用base_footprint
  作为base_link的父坐标系。
  2、里程计坐标系odom，里程计坐标系是一个固定在世界位置的坐标系，odom坐标系会随着时间变化而漂移，所以它无法作为长期的的全局坐标系使用。但机器人在odom坐标系中的位置都是连续变化的，并不会发生跳跃。这是其优点，所以在自定义控制
  器计算速度时，采用的是里程计位置作为当前位置计算速度。
  3、地图坐标系map，地图坐标系也是一个固定在世界位置的坐标系，z轴向上，机器人在map坐标系的姿态不会随着时间而漂移，所以map坐标系作为长期全局参考使用。但map坐标系不是连续变化的，即机器人在map坐标系中的姿态会随时发生跳
  跃性的变化。
  4、地球坐标系earth，该坐标系固定在地心位置，当同时使用多个地图时，则可以通过earth坐标系进行连接。
  
• base_link到机器人传感器sensor和执行器actuator之间的变换一般使用URDF进行描述，然后使用robot_state_publisher节点进行广播
• odom到base_link之间的坐标变换通过测距模块发布，这个测距模块可能是轮式里程计，也有可能是视觉里程计或其他模块。在FishBot中，我们需要根据里程计数据发布坐标变换。
• map到odom之间的坐标变换则是由定位组件基于传感器观测并不断重新计算机器人在map坐标系中的姿态，但定位组件并不会广播从map到base_link的变换。它首先接收从odom到base_link的变换，然后计算map到odom之间的坐标变换并发布，在Navigation2中，由AMCL模块来完成这个工作，但使用slam_toobox建图时，则由slam_toolbox内部组件完成这部分工作。

9.5.3节 准备机器人URDF（提炼总结）

1、在chapt9/fishbot_ws/src下新建fishbot_description功能包，采用默认的构建类型，在src/fishbot_description下新建urdf目录，新建fishbot.urdf文件，并编写如下代码

ros2 pkg create fishbot_description --build-type ament_cmake --license Apache-2.0

<?xml version="1.0"?>
<robot name="fishbot">
    <link name="base_footprint" />
    
    <!-- base link -->
    <link name="base_link">
    <visual>
        <origin xyz="0 0 0.0" rpy="0 0 0" />
        <geometry>
            <cylinder length="0.12" radius="0.10" />
        </geometry>
        <material name="blue">
            <color rgba="0.1 0.1 1.0 0.5" />
        </material>
    </visual>
</link>

<joint name="base_joint" type="fixed">
    <parent link="base_footprint" />
    <child link="base_link" />
    <origin xyz="0.0 0.0 0.076" rpy="0 0 0" />
</joint>

<!-- laser link -->
<link name="laser_link">
    <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
            <cylinder length="0.02" radius="0.02" />
        </geometry>
        <material name="black">
            <color rgba="0.0 0.0 0.0 0.5" />
        </material>
    </visual>
</link>
<joint name="laser_joint" type="fixed">
    <parent link="base_link" />
    <child link="laser_link" />
    <origin xyz="0 0 0.075" rpy="0 0 0" />
</joint>

</robot>

2、在CMakeLists.txt中添加复制urdf目录到install下的指令

install(DIRECTORY urdf DESTINATION share/${PROJECT_NAME})

3、接着新建fishbot_bringup功能包，采用默认构建类型，将加载URDF和启动相关的命令放到该功能包中，在src/fishbot_bringup下新建launch目录，接着新建urdf2tf.launch.py，编写如下内容：

ros2 pkg create fishbot_bringup --build-type ament_cmake --license Apache-2.0

import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    #获取默认路径
    urdf_tutorial_path = get_package_share_directory('fishbot_description')
    fishbot_model_path = urdf_tutorial_path + '/urdf/fishbot.urdf'
    #为launch声明参数
    action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument(name='model',default_value=str(fishbot_model_path),description='URDF的绝对路径')
    #获取文件内容生成新的参数
    robot_description = launch_ros.parameter_descriptions.ParameterValue(launch.substitutions.Command(['cat ', launch.substitutions.LaunchConfiguration('model')]),value_type=str)
    #状态发布节点
    robot_state_publisher_node = launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description': robot_description}]
    )
    #关节状态发布节点
    joint_state_publisher_node = launch_ros.actions.Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',
    )
    return launch.LaunchDescription([
        action_declare_arg_mode_path, joint_state_publisher_node,
        robot_state_publisher_node,
    ])

4、在CMakeLists.txt中添加复制launch目录到install下的指令

install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})

5、重新构建工程，启动launch文件

colcon build

source install/setup.bash

ros2 launch fishbot_bringup urdf2tf.launch.py

就可以看到TF结构了

9.5.4节 发布里程计TF（提炼总结）

• 我们需要将里程计话题转换成里程计和机器人的坐标变换

1、在fishbot_ws/src/fishbot_bringup/src下新建odom2tf.cpp，编写如下代码

#include 
#include 
#include 
#include 
#include 

class OdomTopic2TF : public rclcpp::Node
{ 
    public:
        OdomTopic2TF(std::string name) : Node(name)
        { 
            odom_subscribe_ = this->create_subscription<nav_msgs::msg::Odometry>(
                "odom", rclcpp::SensorDataQoS(),
                std::bind(&OdomTopic2TF::odom_callback_, this, std::placeholders::_1));
                //创建一个tf2_ros::TransformBroadcaster用于广播坐标变换
            tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(this);
        }
    private:
        rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_subscribe_;
        std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_ ;
        //回调函数,处理接收到的odom消息,并发布tf
        void odom_callback_(const nav_msgs::msg::Odometry::SharedPtr msg){
            geometry_msgs::msg::TransformStamped transform;
            transform.header=msg->header;//使用消息的时间戳利和框架ID
            transform.child_frame_id = msg->child_frame_id;
            transform.transform.translation.x = msg->pose.pose.position.x;
            transform.transform.translation.y = msg->pose.pose.position.y;
            transform.transform.translation.z = msg->pose.pose.position.z;
            transform.transform.rotation.x = msg->pose.pose.orientation.x;
            transform.transform.rotation.y = msg->pose.pose.orientation.y;
            transform.transform.rotation.z = msg->pose.pose.orientation.z;
            transform.transform.rotation.w = msg->pose.pose.orientation.w;
            //广播坐标变换信息
            tf_broadcaster_->sendTransform(transform);
        };
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<OdomTopic2TF>("odom2tf");
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

2、修改CMakeLists.txt，注册odom2tf节点，添加如下代码：

find_package(rclcpp REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(nav_msgs REQUIRED)

add_executable(odom2tf src/odom2tf.cpp)

ament_target_dependencies(odom2tf rclcpp tf2 nav_msgs geometry_msgs tf2_ros)
install(TARGETS odom2tf DESTINATION lib/${PROJECT_NAME})

3、重新构建功能包

colcon build

source install/setup.bash

先运行micro_ros_agent让机器人接入，确保odom话题数据正常

ros2 run micro_ros_agent micro_ros_agent udp4 --port 8888

然后运行odom2tf节点

ros2 run fishbot_bringup odom2tf

9.5.5节 完成机器人建图并保存地图（提炼总结）

1、将启动底盘和雷达的指令都放到一个launch里面，在fishbot_ws/src/fishbot_bringup/launch下新建bringup.launch.py，编写如下代码：

import launch 
import launch_ros
from ament_index_python.packages import get_package_share_directory
from launch.launch_description_sources import PythonLaunchDescriptionSource

def generate_launch_description():
    fishbot_bringup_dir = get_package_share_directory('fishbot_bringup')
    ydlidar_ros2_dir = get_package_share_directory('ydlidar')
    urdf2tf = launch.actions.IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            [fishbot_bringup_dir, '/launch', '/urdf2tf.launch.py']),
        )
    odom2tf = launch_ros.actions.Node(
        package='fishbot_bringup',
        executable='odom2tf',
        output='screen'
        )
    microros_agent = launch_ros.actions.Node(
        package='micro_ros_agent',
        executable='micro_ros_agent',
        arguments=['udp4', '--port', '8888'],
        output='screen'
        )
    ros_serial2wifi = launch_ros.actions.Node(
        package='ros_serial2wifi',
        executable='tcp_server',
        parameters= [{'serial_port' : '/tmp/tty_laser'}],
        output='screen'
        )
    ydlidar = launch.actions.IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            [ydlidar_ros2_dir, '/launch', '/ydlidar_launch.py']),
        )
    ydlidar_delay = launch.actions.TimerAction(
        period=5.0,
        actions=[ydlidar]
        )
    return launch.LaunchDescription([
        urdf2tf,
        odom2tf,
        microros_agent,
        ros_serial2wifi,
        ydlidar_delay
        ])

2、保存并重新构建功能包，运行该节点，接着给机器人重新上电，就可以建图了
3、使用slam_toolbox进行建图，在新的终端输入下面的指令

ros2 launch slam_toolbox online_async_launch.py use_sim_time:=False

打开Rviz，修改Fixed Frame为map，添加地图插件，就可以控制机器人进行建图了
4、使用nav2_map_server保存地图
在chapt9/fishbot_ws/src下新建功能包fishbot_navigation2，在功能包下新建maps目录，打开终端，进入maps目录，运行命令来保存地图

ros2 run nav2_map_server map_saver_cli -f room

9.5.6节 完成机器人导航（提炼总结）

1、在功能包fishbot_navigation2下创建config目录，然后将nav2_bringup提供的默认参数复制到config目录下

cp /opt/ros/$ROS_DISTRO/share/nav2_bringup/params/nav2_params.yaml src/fishbot_navigation2/config

2、在fishbot_navigation2功能包下创建launch目录，然后再该目录下新建navigation2.launch.py，将7.3.3节代码直接复制即可
3、修改CMakeLists.txt，添加launch、config、maps三个目录安装到install下，然后重新构建功能包
4、启动导航

ros2 launch fishbot_navigation2 navigation2.launch.py use_sim_time:=False

9.6节 小结与点评（提炼总结）

• 开发真实的移动机器人
• micro-ROS框架将机器人接入ROS2中