用colcon和c++写自己的第一个ROS2包

一、主要参考来源

Writing a simple publisher and subscriber (C++)

二、具体过程

2.1 创建工作空间和包

首先，创建工作空间及

mkdir -p dev_ws/src

然后，进入dev_ws/src目录下，创建我们的功能包

cd dev_ws/src
ros2 pkg create --build-type ament_cmake cpp_pubsub

您的终端将返回一条消息，验证cpp_pubsub包及其所有必要的文件和文件夹的创建。
其中--build-type ament_cmake 好像是对应与C++语言，使用CMake生成。
然后可以用tree命令查看目录，命令如下：

tree -L 2 #显示两级目录

返回如下，我的没有下划线是因为我创建的时候没有打下划线。

.
└── cpppubhub
    ├── CMakeLists.txt
    ├── include
    ├── package.xml
    └── src

3 directories, 2 files

2.2 书写发布节点（node）源码及依赖

接下来，进入源码环节，第一份是C++的发布者的代码，应该放在cpp_pubsub/src下；

cd cpp_pubsub/src #进入源目录
subl  publisher_member_function.cpp #在源目录下创建所需的源文件，不一定要用subl，也可一是gedit等，右键创建或者vs-code里面在对应目录下创建也一样。

在弹窗出来的空文件里面填写代码：

#include  //及时相关头文件
#include 
#include  //内存相关头文件
#include 

//ros2的头文件，一个是cpp的，一个是消息的
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;

/* This example creates a subclass of Node and uses std::bind() to register a
* member function as a callback from the timer. */
// 这个例子创建了Node的一个子类，并使用std::bind()注册一个成员函数作为计时器的回调函数。
class MinimalPublisher : public rclcpp::Node
{
  public:
    /*公共构造函数将节点命名为minimal_publisher，
    并将coun_初始化为0。在构造函数内部，
    使用String消息类型、主题名称topic和
    备份时限制消息所需的队列大小初始化发布者。
    接下来，初始化timer_，这将导致timer_callback函数每秒执行两次。*/
    MinimalPublisher()
    : Node("minimal_publisher"), count_(0)
    {
      publisher_ = this->create_publisher("topic", 10);
      timer_ = this->create_wall_timer(
      500ms, std::bind(&MinimalPublisher::timer_callback, this));
    }

  private:
    /*timer_callback函数是设置消息数据和实际发布消息的地方。
    RCLCPP_INFO宏确保将每个发布的消息打印到控制台。*/
    void timer_callback()
    {
      auto message = std_msgs::msg::String();
      message.data = "Hello, world! " + std::to_string(count_++);
      RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
      publisher_->publish(message);
    }
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher::SharedPtr publisher_;
    size_t count_;
};

/*rclcpp::init初始化ROS 2, rclcpp::spin开始处理节点的数据，包括定时器的回调。*/
int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared());
  rclcpp::shutdown();
  return 0;
}

返回上一级目录去修改CMakeLists.txt和package.xml文件，

cd ..
subl package.xml

在弹出的文件中编辑如下几行，修改描述、邮箱、以及许可。

Examples of minimal publisher/subscriber using rclcpp
Your Name
Apache License 2.0

在ament_cmake下面添加如下两行依赖

rclcpp
std_msgs

最后这个文件看起来应该是这样子的

  cpppubhub
  0.0.0
  Examples of minimal publisher/subscriber using rclcpp
  Your Name
  Apache License 2.0

  ament_cmake
  rclcpp
  std_msgs

  ament_lint_auto
  ament_lint_common

  
    ament_cmake
  

记得保存

然后修改CMakeLists.txt

subl CMakeLists.txt

在注释行# find_package( REQUIRED)后面添加寻找依赖包代码

find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)

然后，添加可执行文件并将其命名为talker，这样你就可以使用ros2 run运行你的节点了:

add_executable(talker src/publisher_member_function.cpp)
ament_target_dependencies(talker rclcpp std_msgs)

然后，添加安装项，方便ros2 run在系统目录下找到执行文件，这一步不是必须，你也可以让ros2 run去制定目录下面执行指定的可执行文件，二不是放到目录下，这点用过点CMake的同学应该都很熟悉了。

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

你的CMakeLists.txt可能是这样的

cmake_minimum_required(VERSION 3.5)
project(cpppubhub)

# Default to C99
if(NOT CMAKE_C_STANDARD)
  set(CMAKE_C_STANDARD 99)
endif()

# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

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

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # uncomment the line when a copyright and license is not present in all source files
  #set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # uncomment the line when this package is not in a git repo
  #set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

add_executable(talker src/publisher_member_function.cpp)
ament_target_dependencies(talker rclcpp std_msgs)


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

可以去掉一些不必要的部分，可能就是这样的了

project(cpp_pubsub)

Default to C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)

add_executable(talker src/publisher_member_function.cpp)
ament_target_dependencies(talker rclcpp std_msgs)

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

ament_package()

2.3 书写接受节点（node）源码及依赖

有了上一小节的经验，这一小节应该会快一些，首先，进入dev_ws/src/cpp_pubsub/src文件夹下创建subscriber_member_function.cpp文件，书写代码如下

#include  //内存相关

//ros2相关
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
using std::placeholders::_1;


class MinimalSubscriber : public rclcpp::Node
{
  public:
    /*
    订阅者节点的代码与发布者的代码几乎相同。
    在该节点被命名为minimal_subscriber，
    构造函数使用该节点的create_subscription类来执行回调。
    发布者和订阅者使用的主题名称和消息类型必须匹配，才能进行通信。
    */
    MinimalSubscriber()
    : Node("minimal_subscriber")
    {
      subscription_ = this->create_subscription(
      "topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1));
    }

  private:
    /*topic_callback函数接收通过主题发布的字符串消息数据，并使用RCLCPP_INFO宏将其写入控制台。*/
    void topic_callback(const std_msgs::msg::String::SharedPtr msg) const
    {
      RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str());
    }

    //订阅
    rclcpp::Subscription::SharedPtr subscription_;
};

/*rclcpp::init初始化ROS 2, 
rclcpp::spin开始处理节点的数据*/
int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared());
  rclcpp::shutdown();
  return 0;
}

继续修改CMakeLists.txt文件，由于ros2依赖包都一样，所以package.xml文件不用再修改了。在CMakeLists.txt中添加如下行

add_executable(listener src/subscriber_member_function.cpp)
ament_target_dependencies(listener rclcpp std_msgs)

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

最后的CMakeLists.txt可能是这样：

cmake_minimum_required(VERSION 3.5)
project(cpppubhub)

# Default to C99
if(NOT CMAKE_C_STANDARD)
  set(CMAKE_C_STANDARD 99)
endif()

# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

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

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # uncomment the line when a copyright and license is not present in all source files
  #set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # uncomment the line when this package is not in a git repo
  #set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

add_executable(talker src/publisher_member_function.cpp)
ament_target_dependencies(talker rclcpp std_msgs)

add_executable(listener src/subscriber_member_function.cpp)
ament_target_dependencies(listener rclcpp std_msgs)

# 只安装talker的
#install(TARGETS
#  talker
#  DESTINATION lib/${PROJECT_NAME})

# 一起安装的
install(TARGETS
  talker
  listener
  DESTINATION lib/${PROJECT_NAME})
  
ament_package()

简化版本可能是这样的

cmake_minimum_required(VERSION 3.5)
roject(cpp_pubsub)

Default to C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

#寻找依赖
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)

#添加talker可执行文件
add_executable(talker src/publisher_member_function.cpp)
ament_target_dependencies(talker rclcpp std_msgs)

#添加listener可执行文件
add_executable(listener src/subscriber_member_function.cpp)
ament_target_dependencies(listener rclcpp std_msgs)

# 安装项目的talker和listener可执行文件到系统目录下
install(TARGETS
  talker
  listener
  DESTINATION lib/${PROJECT_NAME})

ament_package()

2.4 构建和运行（Build and run）

检查rclcpp和std_msgs是不是都正常作为ros2的一部分而正确安装了。

rosdep install -i --from-path src --rosdistro  -y #可以换成你的对应版本，如我的是foxy

如一切顺利，你应该可以看到如下返回:

#All required rosdeps installed successfully

如出现错误，可以返回去看Ubuntu20 ros2安装或者Installing ROS 2 on Ubuntu Linux。

接下来，正式进行构建，先进入工作空间根目录dev_ws，此时目录大体应该如下：

.
└── src
    └── cpp_pubsub
        ├── CMakeLists.txt
        ├── include
        │   └── cpp_pubsub
        ├── package.xml
        └── src
            ├── publisher_member_function.cpp
            └── subscriber_member_function.cpp

5 directories, 4 files

构建

colcon build --packages-select cpp_pubsub

构建完成后，两层目录大抵如下

.
├── build
│   ├── COLCON_IGNORE
│   └── cpp_pubsub
├── install
│   ├── COLCON_IGNORE
│   ├── cpp_pubsub
│   ├── local_setup.bash
│   ├── local_setup.ps1
│   ├── local_setup.sh
│   ├── _local_setup_util_ps1.py
│   ├── _local_setup_util_sh.py
│   ├── local_setup.zsh
│   ├── setup.bash
│   ├── setup.ps1
│   ├── setup.sh
│   └── setup.zsh
├── log
│   ├── build_2021-05-03_15-41-52
│   ├── build_2021-05-03_15-42-25
│   ├── COLCON_IGNORE
│   ├── latest -> latest_build
│   └── latest_build -> build_2021-05-03_15-42-25
└── src
    └── cpp_pubsub

11 directories, 13 files

再打开一个terminal，两个terminal都进入dev_ws目录下，且source

. install/setup.bash

其中一个输入

ros2 run cpp_pubsub talker

另一个输入

ros2 run cpp_pubsub listener

观察运行。
ctrl+c中断运行

运行

三、接下来

接下来可能尝试用Python实现一遍，但更大的可能是尝试C++的服务示例。

四、总结

就熟悉了一下基本的调用以及CMakeLists.txt和package.xml的编写。