ROS 2边学边练（16）-- 自定义msg和srv文件

前言

        在前面的文章我们在学习主题（topic）和服务（service）通信方法时，使用的一直是ROS 2提供好的消息结构文件（xxx.msg）和服务结构文件（xxx.srv），稀里糊涂的就这样过去了，如果我们有个需求，ROS 2提供的msg和srv无法满足，那我们就得定义自己结构的msg和srv文件了。

动动手

创建一个功能包

        .msg和.srv文件需要分别处于两个文件夹下（msg和srv文件夹，具体功能包目录下同一等级），我们先来创建功能包，进入工作空间根路径(先source install/setup.bash)，执行下面命令创建tutorial_interfaces功能包：

$ros2 pkg create --build-type ament_cmake --license Apache-2.0 tutorial_interfaces

 

完成后再进入功能包tutorial_interfaces的根路径下，新建俩个文件夹用来放置.msg和.srv文件：

$mkdir msg srv

这俩文件夹可以被C++实现的节点模块使用，也能被python实现的节点模块使用。

自定义

msg

        首先在msg文件夹下新建一个叫Num.msg的文件，添加下面这行内容到里面：

int64 num

该消息名称为Num,只有一个类型为int64的元素num。

        然后同样在msg文件夹下新建一个叫Sphere.msg的文件，添加下面两行内容到里面：

geometry_msgs/Point center
float64 radius

Sphere消息包含了两个元素，第一个为系统提供的功能包geometry_msgs中Point类型的center（可以看出我们可以嵌入消息），第二个为float64类型的radius。

srv

        在功能包的srv文件夹下（ros2_ws/src/tutorial_interfaces/srv）新建一个叫AddThreeInts.srv的文件，添加下面内容到里面：

int64 a
int64 b
int64 c
---
int64 sum

请求元素有3个：a、b、c，都是int64类型，回复依然是int64类型的sum。

修改CMakeLists.txt

        要将我们自定义的接口转换为特定语言（C++/PYTHON）的代码，我们需要将下面的内容增加到CMakeLists.txt中去：

find_package(geometry_msgs REQUIRED)
find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/Num.msg"
  "msg/Sphere.msg"
  "srv/AddThreeInts.srv"
  DEPENDENCIES geometry_msgs # Add packages that above messages depend on, in this case geometry_msgs for Sphere.msg
)

对于上面的内容，“rosidl_generate_interfaces()”第一个参数必须是{PROJECT_NAME},否则可能会出现非同一般的错误。

修改package.xml

        将下面内容添加到package.xml内：

<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>

简单解释一下，geometry_msgs在我们自定义的Sphere.msg中有引用，所以需要依赖(库依赖<depend>)，rosidl_default_generators会生成语言的代码（C++/PYTHON），其是一种构建工具（工具依赖<buildtool_depend>），所以需要依赖，rosidl_default_runtime是一个运行时或执行阶段需要的依赖项,需要接口能够在以后被使用（执行依赖<exec_depend>），rosidl_interface_packages是功能包tutorial_interfaces应该与之关联的依赖组的名称，使用<member_of_group>标记声明。

 

构建包

        进入工作空间根路径，按照下面的命令进行包的构建工作：

$colcon build --packages-select tutorial_interfaces

确认自定义的msg和srv被成功构建 

        大家应该还记得如何查看msg和srv具体数据结构的命令吧,新开一个终端，先source环境变量如何进行下面操作：

$ros2 interface show tutorial_interfaces/msg/Num

再来看看另外一个消息Sphere和剩下的AddThreeInts服务：

$ros2 interface show tutorial_interfaces/msg/Sphere

$ros2 interface show tutorial_interfaces/srv/AddThreeInts

 

 Perfect!

测试新的接口

Num.msg

        为方便测试，我们利用之前用过的例子(发布者/订阅者)来测试这个新的消息Num.msg，我们在工作空间的src路径下，进入cpp_pubsub文件夹，先修改发布者节点源文件publisher_member_function.cpp，按照下面的内容进行修改：

#include <chrono>
#include <memory>

#include "rclcpp/rclcpp.hpp"
#include "tutorial_interfaces/msg/num.hpp"                                            // CHANGE

using namespace std::chrono_literals;

class MinimalPublisher : public rclcpp::Node
{
public:
  MinimalPublisher()
  : Node("minimal_publisher"), count_(0)
  {
    publisher_ = this->create_publisher<tutorial_interfaces::msg::Num>("topic", 10);  // CHANGE
    timer_ = this->create_wall_timer(
      500ms, std::bind(&MinimalPublisher::timer_callback, this));
  }

private:
  void timer_callback()
  {
    auto message = tutorial_interfaces::msg::Num();                                   // CHANGE
    message.num = this->count_++;                                                     // CHANGE
    RCLCPP_INFO_STREAM(this->get_logger(), "Publishing: '" << message.num << "'");    // CHANGE
    publisher_->publish(message);
  }
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<tutorial_interfaces::msg::Num>::SharedPtr publisher_;             // CHANGE
  size_t count_;
};

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

原先调用标准消息的语句是：

publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);

现在使用自定义消息的语句是：

publisher_ = this->create_publisher<tutorial_interfaces::msg::Num>("topic", 10); 

可以发现<..>里面的内容变化了，另外在install路径下对应的tutorial_interfaces包里面的include路径下有自动生成一个Num.hpp头文件。

再来subscriber_member_function.cpp.c文件，内容如下：

#include <functional>
#include <memory>

#include "rclcpp/rclcpp.hpp"
#include "tutorial_interfaces/msg/num.hpp"                                       // CHANGE

using std::placeholders::_1;

class MinimalSubscriber : public rclcpp::Node
{
public:
  MinimalSubscriber()
  : Node("minimal_subscriber")
  {
    subscription_ = this->create_subscription<tutorial_interfaces::msg::Num>(    // CHANGE
      "topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1));
  }

private:
  void topic_callback(const tutorial_interfaces::msg::Num & msg) const  // CHANGE
  {
    RCLCPP_INFO_STREAM(this->get_logger(), "I heard: '" << msg.num << "'");     // CHANGE
  }
  rclcpp::Subscription<tutorial_interfaces::msg::Num>::SharedPtr subscription_;  // CHANGE
};

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

调整改变的地方也是与publisher差不多，都是将之前的std_msgs::msg::String消息换成了tutorial_interfaces::msg::Num消息。

CMakeLists.txt

CMakeLists.txt中将std_msgs替换为新包tutorial_interfaces即可，

#...

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(tutorial_interfaces REQUIRED)                      # CHANGE

add_executable(talker src/publisher_member_function.cpp)
ament_target_dependencies(talker rclcpp tutorial_interfaces)    # CHANGE

add_executable(listener src/subscriber_member_function.cpp)
ament_target_dependencies(listener rclcpp tutorial_interfaces)  # CHANGE

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

ament_package()

package.xml

只需修改一行即可，如下：

<depend>tutorial_interfaces</depend>

构建包cpp_pubsub

进入工作空间根路径，执行下面命令进行构建：

$colcon build --packages-select cpp_pubsub

分别新开两个终端，各自source install/setup.bash，再运行订阅者节点再发布者节点，结果如下：

AddThreeInts.srv 

        测试新定义的服务，我们同样可以修改之前的例子（服务端/客户端），进入cpp_srvcli包，先修改add_two_ints_server.cpp:

#include "rclcpp/rclcpp.hpp"
#include "tutorial_interfaces/srv/add_three_ints.hpp"                                        // CHANGE

#include <memory>

void add(const std::shared_ptr<tutorial_interfaces::srv::AddThreeInts::Request> request,     // CHANGE
          std::shared_ptr<tutorial_interfaces::srv::AddThreeInts::Response>       response)  // CHANGE
{
  response->sum = request->a + request->b + request->c;                                      // CHANGE
  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request\na: %ld" " b: %ld" " c: %ld",  // CHANGE
                request->a, request->b, request->c);                                         // CHANGE
  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");   // CHANGE

  rclcpp::Service<tutorial_interfaces::srv::AddThreeInts>::SharedPtr service =               // CHANGE
    node->create_service<tutorial_interfaces::srv::AddThreeInts>("add_three_ints",  &add);   // CHANGE

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

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

再修改add_two_ints_client.cpp:

#include "rclcpp/rclcpp.hpp"
#include "tutorial_interfaces/srv/add_three_ints.hpp"                                       // CHANGE

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

using namespace std::chrono_literals;

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);

  if (argc != 4) { // CHANGE
      RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "usage: add_three_ints_client X Y Z");      // CHANGE
      return 1;
  }

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

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

  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);
  // Wait for the result.
  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");    // CHANGE
  }

  rclcpp::shutdown();
  return 0;
}

由之前请求两个元素变成请求三个元素，不再赘述。

CMakeLists.txt

将std_msgs替换为新定义的tutorial_interfaces，

#...

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(tutorial_interfaces REQUIRED)         # CHANGE

add_executable(server src/add_two_ints_server.cpp)
ament_target_dependencies(server
  rclcpp tutorial_interfaces)                      # CHANGE

add_executable(client src/add_two_ints_client.cpp)
ament_target_dependencies(client
  rclcpp tutorial_interfaces)                      # CHANGE

install(TARGETS
  server
  client
  DESTINATION lib/${PROJECT_NAME})

ament_package()

package.xml

同样只需修改这一行

<depend>tutorial_interfaces</depend>

构建包cpp_srvcli

工作空间根路径下：

$colcon build --packages-select cpp_srvcli

同样开启两个终端，分别source install/setup.bash,然后先启动服务节点，然后启动客户端节点，结果如下：

 

从以上结果可以看出，我们的自定义msg和srv成功了！

本篇完。