ROS2-包创建

发表于
ROS2-教程（二）

ROS-包创建

1 包结构创建和编译工具

在自己开发 ROS 包的时候，需要先构建一个工作空间，然后再在这个空间下面进行开发，这个工作空间相对于安装在系统的 ROS（称为底层）称为覆盖层。
ROS2 中包结构如下：

1
2
3
4
5
ros2_ws
├── build
├── install
├── log
└── src

在开发自己的包时候通过 colcon 构建工具，编译 src 目录下不同的包，并根据每个包下面的编译规则 CMakeLists.txt/setup.py 完成底层的构建并安装到 install 目录下。
一般步骤包括：

1.1 在工作区下的 src 目录下生成 cpp/py 包结构

1
ros2 pkg create --build-type ament_cmake/ament_python --license Apache-2.0

cpp 包结构：

1
2
3
4
5
6
7
package_name
├── build
├── CMakeLists.txt
├── include
├── LICENSE
├── package.xml
└── src

1.2 在 package.xml 添加依赖项和作者信息

例如：

1
<depend>rclcpp</depend>

1.3 在 CMakeLists.txt 添加依赖项以及安装路径

例如：

1
2
3
4
5
6
7
8
9
10
# 添加依赖包
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})

1.4 在包中的 src 添加内容

1.5 回到工作空间目录 ros2_ws 编译

  • 编译安装前先检查依赖项
1
rosdep install -i --from-path src --rosdistro humble -y
  • 编译
1
colcon build --packages-select <package_name>

1.6 获取覆盖层的环境变量正常运行

1
source /install/local_setup.bash

该文件 local_setup.bash 只包括这个工作空间下的环境变量

2 话题发布订阅通信实现

创建包的步骤与 1 相同，需要修改的是 1.4 步骤里 ros2_ws/src/<package_name>/src 目录下的文件。

2.1 话题发布者（publisher）

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
#include <chrono>
#include <functional>
#include <memory>
#include <string>

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

using namespace std::chrono_literals;

// 发布者类，继承 Node 节点类
class MinimalPublisher : public rclcpp::Node
{
public:
  MinimalPublisher()
  : Node("minimal_publisher"), count_(0)
  {
    // 规定话题缓冲区大小
    publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);
    // 创建一个 500 ms，定时调用的定时器，每隔半秒调用一次 timer_callback()
    timer_ = this->create_wall_timer(
      500ms, std::bind(&MinimalPublisher::timer_callback, this));
  }

private:
  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<std_msgs::msg::String>::SharedPtr publisher_;
  size_t count_;
};

int main(int argc, char * argv[])
{
  // 创建节点
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<MinimalPublisher>());
  rclcpp::shutdown();
  return 0;
}

2.2 话题订阅者（Subscriber）

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
#include <functional>
#include <memory>

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

using std::placeholders::_1;

// 订阅者类，继承 Node 类
class MinimalSubscriber : public rclcpp::Node
{
public:
  MinimalSubscriber()
  : Node("minimal_subscriber")
  {
    // 定义订阅者，消息队列长度为 10，当有消息时自动调用 topic_callback 函数。
    // _1，占位符用于将得到的消息作为第一个参数传递给 topic_callback。
    subscription_ = this->create_subscription<std_msgs::msg::String>(
      "topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1));
  }

private:
  // 第 2 个 const 表示不会修改类内成员变量
  void topic_callback(const std_msgs::msg::String & msg) const
  {
    RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg.data.c_str());
  }
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
};

int main(int argc, char * argv[])
{
  // 开启节点
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<MinimalSubscriber>());
  rclcpp::shutdown();
  return 0;
}

3 服务通信实现

创建包的步骤与 1 相同，需要修改的是 1.4 步骤里 ros2_ws/src/<package_name>/src 目录下的文件。

3.1 服务端（Server）

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
#include "rclcpp/rclcpp.hpp"
#include "tutorial_interfaces/srv/add_three_ints.hpp"

#include <memory>

// 服务函数，将三个书相加
void add(const std::shared_ptr<tutorial_interfaces::srv::AddThreeInts::Request> request,
          std::shared_ptr<tutorial_interfaces::srv::AddThreeInts::Response>      response)
{
  response->sum = request->a + request->b + request->c;
  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request\na: %ld" " b: %ld" " c: %ld",
                request->a, request->b, request->c);
  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sending back response: [%ld]", (long int)response->sum);
}

int main(int argc, char **argv)
{
  // 创建节点
  rclcpp::init(argc, argv);

  std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("add_three_ints_server");
  
  // 创建服务，当有 request 时，自动调用 add 函数
  rclcpp::Service<tutorial_interfaces::srv::AddThreeInts>::SharedPtr service =
    node->create_service<tutorial_interfaces::srv::AddThreeInts>("add_three_ints", &add);

  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready to add three ints.");

  rclcpp::spin(node);
  rclcpp::shutdown();
}

3.2 客户端（Client）

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
#include "rclcpp/rclcpp.hpp"
#include "tutorial_interfaces/srv/add_three_ints.hpp"

#include <chrono>
#include <cstdlib>
#include <memory>

using namespace std::chrono_literals;

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
  
  // 获取判断命令行参数数量
  if (argc != 4) {
      RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "usage: add_three_ints_client X Y Z");
      return 1;
  }

  std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("add_three_ints_client");
  rclcpp::Client<tutorial_interfaces::srv::AddThreeInts>::SharedPtr client =
    node->create_client<tutorial_interfaces::srv::AddThreeInts>("add_three_ints");

  auto request = std::make_shared<tutorial_interfaces::srv::AddThreeInts::Request>();
  request->a = atoll(argv[1]);
  request->b = atoll(argv[2]);
  request->c = atoll(argv[3]);

  // 1s 后如果没有创建该服务，则退出
  while (!client->wait_for_service(1s)) {
    if (!rclcpp::ok()) {
      RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting.");
      return 0;
    }
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again...");
  }

  auto result = client->async_send_request(request);
  // 等待服务响应
  if (rclcpp::spin_until_future_complete(node, result) ==
    rclcpp::FutureReturnCode::SUCCESS)
  {
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Sum: %ld", result.get()->sum);
  } else {
    RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service add_three_ints");
  }

  rclcpp::shutdown();
  return 0;
}

4 接口实现

4.1 在包目录下直接创建 msg/srv 文件夹

在对应文件夹下创建 .msg 或者 .srv 文件

4.1.1 <name>.msg 文件

例如：

1
int64 num

4.1.2 <name>.srv 文件

例如：

1
2
3
4
int64 a
int64 b
---
int64 sum

4.2 修改 CMakeLists.txt

1
2
3
4
5
6
7
8
9
10
# rosidl_default_generators 这个包用来生成实际的接口 cpp/python 代码文件
find_package(rosidl_default_generators REQUIRED)

# 生成接口
rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/Num.msg"
  "msg/Sphere.msg"
  "srv/AddThreeInts.srv"
  DEPENDENCIES geometry_msgs # Sphere.msg 用到这个依赖项
)

4.2 修改 package.xml

添加依赖并声明是接口包

1
2
3
4
5
<depend>geometry_msgs</depend>
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<!-- 声明这个包是接口包 -->
<member_of_group>rosidl_interface_packages</member_of_group>

4.3 在包内使用自定义接口

需要重新修改 CMakeLists.txt

1
2
3
4
5
6
7
8
9
10
ament_export_dependencies(rosidl_default_runtime)
set(msg_files
  "msg/AddressBook.msg"
)

# 在生成其它包后，例如 publish_address_book，将接口链接到该包
rosidl_get_typesupport_target(cpp_typesupport_target
${PROJECT_NAME} rosidl_typesupport_cpp)

target_link_libraries(publish_address_book "${cpp_typesupport_target}")

5 参数定义实现

在节点内容创建时定义参数即可。
例如：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
#include <chrono>
#include <functional>
#include <string>

#include <rclcpp/rclcpp.hpp>

using namespace std::chrono_literals;

class MinimalParam : public rclcpp::Node
{
public:
  MinimalParam()
  : Node("minimal_param_node")
  {
    // 声明参数名
    this->declare_parameter("my_parameter", "world");

    timer_ = this->create_wall_timer(
      1000ms, std::bind(&MinimalParam::timer_callback, this));
  }

  void timer_callback()
  {
    // 获取参数
    std::string my_param = this->get_parameter("my_parameter").as_string();

    RCLCPP_INFO(this->get_logger(), "Hello %s!", my_param.c_str());

    // 设置参数
    std::vector<rclcpp::Parameter> all_new_parameters{rclcpp::Parameter("my_parameter", "world")};
    this->set_parameters(all_new_parameters);
  }

private:
  rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<MinimalParam>());
  rclcpp::shutdown();
  return 0;
}

参数设置可以查看 ROS-命令行工具 实现。