[ROS2]Ubuntu22.04+ROS2 Humble安装及基本功能测试（功能包/发布订阅/launch）

写在前面：之前基于ROS+Turtlebot2做了个多机器人的系统，但最近由于项目需要，要自己制作一个小车并实现之前的功能，由于在实际使用中深感ROS1的性能有限，因而决定在新的机器人中尝试ROS2，又要从头干起了（类目了）。此博客只是个人入门的学习记录，不涉及花里胡哨的用法，只是查阅了网上的很多资料，对比补充之后做了个汇总（几个主要的参考都附在文末），先向各位大佬致谢了~orz

ROS2安装

安装配置：Ubuntu22.04+ROS2 Humble+Intel NUC

  由于Ubuntu22暂时没有可用的稳定LTS版本，Ubuntu20升级只会升级到Ubuntu21，建议使用镜像安装Ubuntu22(注意保存好个人文件)。
镜像位于：https://cn.ubuntu.com/download/desktop
ROS2安装指南：https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html
  安装之前需要更换为国内源：

1. 执行：sudo gedit /etc/apt/sources.list
2. 添加如下内容：（清华镜像地址）

deb https://mirrors.tuna.tsinghua.edu.cn/ubuntu/ jammy main restricted universe multiverse
# deb-src https://mirrors.tuna.tsinghua.edu.cn/ubuntu/ jammy main restricted universe multiverse
deb https://mirrors.tuna.tsinghua.edu.cn/ubuntu/ jammy-updates main restricted universe multiverse
# deb-src https://mirrors.tuna.tsinghua.edu.cn/ubuntu/ jammy-updates main restricted universe multiverse
deb https://mirrors.tuna.tsinghua.edu.cn/ubuntu/ jammy-backports main restricted universe multiverse
# deb-src https://mirrors.tuna.tsinghua.edu.cn/ubuntu/ jammy-backports main restricted universe multiverse
deb https://mirrors.tuna.tsinghua.edu.cn/ubuntu/ jammy-security main restricted universe multiverse
# deb-src https://mirrors.tuna.tsinghua.edu.cn/ubuntu/ jammy-security main restricted universe multiverse

  3. 依照官方的安装指南，按顺序执行即可。遇到下载失败的问题可以重新执行一次，或更换手机热点试一试。

sudo apt update && sudo apt install locales
sudo locale-gen en_US en_US.UTF-8
sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
export LANG=en_US.UTF-8
sudo apt install curl gnupg lsb-release
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(source /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
sudo apt update && sudo apt upgrade
sudo apt install ros-humble-desktop
echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc

安装完毕后测试：

安装完毕后重开两个新终端，分别执行以下命令：

ros2 run demo_nodes_cpp talker
ros2 run demo_nodes_py listener

常用命令行测试：

ros2 --help可以看到可用命令列表：

新开终端分别运行：

ros2 run turtlesim turtlesim_node
ros2 run turtlesim turtle_teleop_key

可以实现键盘控制小乌龟

查看节点信息

ros2 node list
ros2 node info /turtlesim

查看当前话题列表，并查看话题的内容：

ros2 topic list
ros2 topic echo /turtle1/pose

查看话题使用的带宽：

ros2 topic bw /turtle1/pose

使用命令行发布速度话题：

ros2 topic pub -r 1 /turtle1/cmd_vel geometry_msgs/msg/Twist "{linear: {x: 2.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 1.8}}"

使用service生成新的小乌龟：

ros2 service
ros2 service call /spawn turtlesim/srv/Spawn "{x: 2, y: 2, theta: 0.2, name: ''}"

Action机制给小乌龟发目标点：

ros2 action
ros2 action send_goal /turtle1/rotate_absolute turtlesim/action/RotateAbsolute "theta: 3"

ROS2基础使用

创建工作空间：

1) 安装colcon:
colcon是在catkn_make、ament基础上发展而来的，功能与前两者类似，编译工作空间中的源文件夹(src)，生成三个文件夹：build, install, log.（与catkin相比没有devel）

sudo apt install python3-colcon-common-extensions

2) 创建工作空间以及功能包：

mkdir -p ~/ros2_ws/src
cd ros2_ws

新建自定义功能包可以参考：
https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Creating-Your-First-ROS2-Package.html

cd src
ros2 pkg create beginner

（默认生成的是C++包，包含了CMakeLists。若想生成python包则需要添加–build-type，命令如下：
ros2 pkg create --build-type ament_python beginner_python
类似的，生成一个C++包，命令如下：
ros2 pkg create --build-type ament_cmake beginner_c）

3) colcon编译：

colcon build --symlink-install

(–symlink-install表示通过软链接的方式指向脚本文件如python文件，它允许当source空间中的文件改变时安装的文件也会随着改变，colcon的更多功能参考：https://blog.csdn.net/weixin_38274206/article/details/123279572)

. install/setup.bash

（当colcon成功地完成构建后，输出将位于install目录中。在使用任何安装好的可执行文件或库之前，需要将它们添加到路径和库路径中。colcon会在install目录中生成bash / bat文件，以帮助设置环境。
为了方便，可以执行：echo " source ~/ros2_ws/install/setup.bash" >> ~/.bashrc，这样就不再需要每次都source了。多工作空间时需要使用local_setup.bash，需要时自行查阅。）

colcon插件：

1) colcon_cd：快速查询并进入功能包

echo "source /usr/share/colcon_cd/function/colcon_cd.sh" >> ~/.bashrc
echo "export _colcon_cd_root=~/ros2_ws" >> ~/.bashrc

演示一下：

2) colcon_argcomplete：tab补全功能

echo "source /usr/share/colcon_argcomplete/hook/colcon-argcomplete.bash" >> ~/.bashrc

Tips：
在功能包内添加空文件COLCON_IGNORE可以在编译时忽略该功能包（和catkin中的CATKIN_IGNORE一样）

在功能包中添加节点：

以发布/订阅为例分别在Python/C++功能包添加节点，也会同时介绍在两种语言中QoS Policy的使用规则。QoS Policy的更改只是出于个人兴趣，没有改好甚至会出现兼容性问题。因此没有这种需求就可以把下面代码中的qos_profile部分内容删去，publisher/subscriber中的qos_frofile_*更换为队列长度即可。
比如：self.pub_s = self.create_publisher(String, “chatter”, qos_profile_s)
修改为：self.pub_s = self.create_publisher(String, “chatter”, 10)

Python:

colcon_cd beginner_python
cd beginner_python
touch py_node_pub.py
touch py_node_sub.py

py_node_pub.py内容:

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
from geometry_msgs.msg import Twist
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
from rclpy.qos import qos_profile_sensor_data, qos_profile_system_default

class TestPythonNode(Node):
    def __init__(self, name):
        super().__init__(name)
        self.get_logger().info("Testing logger")
        qos_profile_v = QoSProfile(
            reliability = QoSReliabilityPolicy.RELIABLE, 
            history = QoSHistoryPolicy.KEEP_LAST, 
            depth = 10
        )
        qos_profile_s = qos_profile_sensor_data
        self.pub_s = self.create_publisher(String, "chatter", qos_profile_s)
        self.pub_v = self.create_publisher(Twist, "cmd_vel", qos_profile_v)
        self.timer = self.create_timer(0.5, self.timer_callback)
        self.timer_fast = self.create_timer(0.1, self.timer_fast_callback)
    def timer_callback(self):
        msg_s = String()
        msg_s.data = "Testing best-effort publisher"
        self.pub_s.publish(msg_s)
    def timer_fast_callback(self):
        msg_v = Twist()
        msg_v.linear.x = 0.1
        msg_v.angular.z = 0.2
        self.pub_v.publish(msg_v)
def main(args = None):
    rclpy.init(args=args)
    node = TestPythonNode("pub_node")
    rclpy.spin(node)
    node.destroy(node)
    rclpy.shutdown()

py_node_sub.py内容:

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
from geometry_msgs.msg import Twist
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
from rclpy.qos import qos_profile_sensor_data, qos_profile_system_default
class TestPythonSub(Node):
    def __init__(self, name):
        super().__init__(name)
        qos_profile_s = qos_profile_sensor_data
        self.sub_s = self.create_subscription(String, "chatter", self.s_callback, qos_profile_s)
        qos_profile_v = QoSProfile(
            reliability = QoSReliabilityPolicy.RELIABLE, 
            history = QoSHistoryPolicy.KEEP_LAST, 
            depth = 10
        )
        self.sub_v = self.create_subscription(Twist, "cmd_vel", self.v_callback, qos_profile_v)
    def s_callback(self, msg):
        self.get_logger().info("String subscriber get: %s"%msg.data)
    def v_callback(self, msg):
        self.get_logger().info("Velocity subscriber get: %s"%str(msg.linear.x))
        #print(msg)
def main(args = None):
    rclpy.init(args=args)
    node = TestPythonSub("sub_node")
    rclpy.spin(node)
    node.destroy(node)
    rclpy.shutdown()

  对于创建的ROS2 Python包，可以不必添加shebang(即第一行)，但对于混合的功能包则必须添加，且需要将py文件添加可执行权限(后面再说具体操作)。
ROS2为QoS提供了默认的配置（比如此处用到的qos_profile_sensor_data），用户也可以自行更改，不过要注意兼容性。
QoS可配置参数及兼容性参考：
https://docs.ros.org/en/humble/Concepts/About-Quality-of-Service-Settings.html
内容填写完毕后，在setup.py作如下修改：

    entry_points={
        'console_scripts': [
            'py_node_pub = beginner_python.py_node_pub:main', 
            'py_node_sub = beginner_python.py_node_sub:main'
        ],
    },

修改完毕后编译：

colcon_cd
colcon build --symlink-install
. install/setup.bash

打开两个终端，分别运行：

ros2 run beginner_python py_node_pub
ros2 run beginner_python py_node_sub

得到订阅如下：

C++

为了使用VSCode的自动补全，需要打开工作空间文件夹，然后在VSCode的include path添加：/opt/ros/humble/include/**

生成节点文件：

colcon_cd beginner_c
cd src
touch c_node_pub.cpp
touch c_node_sub.cpp

生成文件后填入以下内容：
c_node_pub.cpp:

#include 
#include 
#include 
#include 
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "geometry_msgs/msg/twist.hpp"

using namespace std::chrono_literals;

class TestCppNode : public rclcpp::Node
{
    public:
        TestCppNode(std::string name) : Node(name), cnt_(0)
        {
            rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data;
            rclcpp::QoS qos_s = rclcpp::QoS(
                rclcpp::QoSInitialization(
                    qos_profile.history, //RMW_QOS_POLICY_HISTORY_KEEP_LAST
                    qos_profile.depth    //10
                ), 
                qos_profile
            );  //create a QoS profile using sensor_profile(BEST_EFFORT)
            rclcpp::QoS qos_v = rclcpp::QoS(10);  //create a QoS profile using default_profile(RELIABLE)
            pub_s = this->create_publisher<std_msgs::msg::String>("chatter", qos_s);
            pub_v = this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", qos_v);
            timer_s = this->create_wall_timer(500ms, std::bind(&TestCppNode::timer_s_callback, this));
            timer_v = this->create_wall_timer(100ms, std::bind(&TestCppNode::timer_v_callback, this));
        }
    private:
        rclcpp::TimerBase::SharedPtr timer_s;
        rclcpp::TimerBase::SharedPtr timer_v;
        rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_s;
        rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr pub_v;
        size_t cnt_;
        void timer_s_callback()
        {
            rclcpp::Clock sys_time(RCL_SYSTEM_TIME);
            auto msg_s = std_msgs::msg::String();
            msg_s.data = "Testing publisher"+std::to_string(cnt_++);
            pub_s->publish(msg_s);
            RCLCPP_INFO_THROTTLE(this->get_logger(), sys_time, 2000, "Publishing string: %s", msg_s.data.c_str());
        }
        void timer_v_callback()
        {
            rclcpp::Clock steady_time(RCL_STEADY_TIME);
            auto msg_v = geometry_msgs::msg::Twist();
            msg_v.linear.x = 0.1;
            msg_v.angular.z = 0.2;
            pub_v->publish(msg_v);
            RCLCPP_INFO_THROTTLE(this->get_logger(), steady_time, 2000, "Publishing linear vel: %f", msg_v.linear.x);
        }
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<TestCppNode>("talker"));
    rclcpp::shutdown();
    return 0;
}

c_node_sub.cpp:

#include 
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "geometry_msgs/msg/twist.hpp"
using std::placeholders::_1;

class TestCppNode : public rclcpp::Node
{
public:
    TestCppNode(std::string name): Node(name)
    {
        rclcpp::QoS qos_s(10);
        qos_s.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT);
        rclcpp::QoS qos_v(10);
        sub_s = this->create_subscription<std_msgs::msg::String>(
            "chatter", qos_s, std::bind(&TestCppNode::string_callback, this, _1)
        );
        sub_v = this->create_subscription<geometry_msgs::msg::Twist>(
            "cmd_vel", qos_v, std::bind(&TestCppNode::twist_callback, this, _1)
        );
    }
private:
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_s;
    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr sub_v;
    void string_callback(const std_msgs::msg::String& msg) const
    {
        RCLCPP_INFO(this->get_logger(), "I heard string: %s", msg.data.c_str());
    }
    void twist_callback(const geometry_msgs::msg::Twist& msg) const
    {
        RCLCPP_INFO(this->get_logger(), "I heard linear vel: %f", msg.linear.x);
    }
};

int main(int argc, char* argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<TestCppNode>("listener"));
    rclcpp::shutdown();
    return 0;
}

关于QoS Policy结构体的内容可以参考：
https://surfertas.github.io/ros2/2019/08/17/ros2-qos.html
在package.xml的下面添加如下几行(注意行前的两个空格)：

  <depend>rclcppdepend>
  <depend>std_msgsdepend>
  <depend>geometry_msgsdepend>

在CMakeLists.txt中find dependencies下面添加：

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package( REQUIRED)

add_executable(talker src/c_node_pub.cpp)
ament_target_dependencies(talker rclcpp std_msgs geometry_msgs)
add_executable(listener src/c_node_sub.cpp)
ament_target_dependencies(listener rclcpp std_msgs geometry_msgs)

install(TARGETS
  talker
  listener
  DESTINATION lib/${PROJECT_NAME})

修改完毕后保存，再进行编译：

colcon_cd
colcon build

编译之后再新开两个终端，分别运行：

ros2 run beginner_c talker
ros2 run beginner_c listener

C++和Python的混合包：

  对于一个工程而言不可能只存在一种语言编写的文件，下面创建一个C++、Python通用的功能包。前面在创建功能包时使用默认设置(ament_cmake)生成了一个beginner功能包，在此基础上更改为通用的。首先需要生成两个文件夹，为了与C++形成格式上的对应， beginner存放Python的库函数，scripts存放执行的文件。

colcon_cd beginner
mkdir beginner
mkdir scripts
touch beginner/__init__.py
touch beginner/module_to_import.py

将c_node_pub复制进src， 将py_node_sub复制进scripts，并在py文件末尾添加内容：

if __name__=='__main__':
main()

(需要检查一下python文件第一行是否有shebang)
再为文件夹添加可执行权限：

sudo chmod -R a+x scripts

修改package.xml：

  <buildtool_depend>ament_cmakebuildtool_depend>
  <buildtool_depend>ament_cmake_pythonbuildtool_depend>
  <depend>rclcppdepend>
  <depend>std_msgsdepend>
  <depend>geometry_msgsdepend>
  <depend>rclpydepend>

修改CMakeLists.txt，添加内容如下：

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(ament_cmake_python REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclpy REQUIRED)
find_package(std_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package( REQUIRED)

# Include Cpp "include" directory
include_directories(include)

# Create Cpp executable
add_executable(talker src/c_node_pub.cpp)
ament_target_dependencies(talker rclcpp std_msgs geometry_msgs)

# Install Cpp executables
install(TARGETS
  talker
  DESTINATION lib/${PROJECT_NAME}
)

# Install Python modules
ament_python_install_package(${PROJECT_NAME})

# Install Python executables
install(PROGRAMS
  scripts/py_node_sub.py
  DESTINATION lib/${PROJECT_NAME}
)

新开两个终端执行命令：

ros2 run beginner talker
ros2 run beginner py_node_sub.py

launch文件的使用：

  如何在一个终端里同时运行多个节点？换句话说ROS2中怎样使用launch文件？不同于ROS1中将launch文件写在节点对应的文件夹中，ROS2的惯例是新建一个功能包专门用于存放launch文件。下面以C++类型的功能包为例演示。

colcon_cd
cd src
ros2 pkg create beginner_launch
cd beginner_launch
rm -rf include/
rm -rf src/
mkdir launch
touch launch/demo_launch.py

在demo_launch.py中填入：

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
from re import sub
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    ld = LaunchDescription()
    pub_node = Node(
        package="beginner_python", 
        executable="py_node_pub"
    )
    sub_node = Node(
        package="beginner_python", 
        executable="py_node_sub"
    )
    ld.add_action(pub_node)
    ld.add_action(sub_node)
    return ld

注意：launch文件是有固定格式要求的
启动文件必须包含函数：generate_launch_description()，并且必须返回一个 LaunchDescription 对象。
在package.xml添加：（每一个用到的功能包都要添加）

  <exec_depend>beginner_pythonexec_depend>

在CmakeLists.txt添加：

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

现在可以同时启动python功能包中的收发节点：

ros2 run beginner_launch demo_launch.py

关于launch文件的重命名、重映射、参数加载，参考链接：
https://roboticsbackend.com/ros2-launch-file-example/
https://www.guyuehome.com/38308
launch文件在ROS2中还有更多的用法，比如LifecycleNode, 待我后续学习后再补充吧

参考资料：

[1] ROS2 Humble官方文档 https://docs.ros.org/en/humble/index.html
[2] 古月居ROS2 21讲 https://www.guyuehome.com/blog/column/id/147
[3] Robotics Back-end https://roboticsbackend.com/category/ros2/
[4] 个人博客 http://www.lxshaw.com/category/tech/ros/