Fast-DDS & ros2 ros与fastdds通讯

系列文章目录

  

---

---

前言

本文介绍了ros环境中的节点与无ros环境程序之间的通讯。
ros = plumbing + tools + capabilities + community
其中plumbing采用DDS协议，ros2默认其中Fast-DDS，因此ros2节点默认是通过fastdds与其他节点(应用)进行通讯的，本文介绍的是在ros2环境下编写的节点与远程主机无ros环境通过fastrtps库编写的程序之间的通讯。

---

一、创建用户自定义的msg和srv文件

ros2中的类型与C++, python, DDS type 之间的对应关系
https://docs.ros.org/en/humble/Concepts/Basic/About-Interfaces.html

1. 创建用于接口的package

该文件是DDS与ros2节点之间通讯的接口文件
.msg:

string name
uint32 uintA
uint32 payload_size
byte[] payload

在ros中新建接口文件：

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

切换至test_interface目录，并创建.msg文件放入msg目录内：

cd test_interface
mkdir msg
cd msg
touch TestEvt.msg  # 将上面.msg的内容填入

2. 修改ros相关的配置文件

在包test_interface内修改：

CMakeLists.txt内的find_package(ament_cmake REQUIRED)下一行添加

find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/TestEvt.msg"
  # DEPENDENCIES geometry_msgs # Add packages that above messages depend on, in this case geometry_msgs for Sphere.msg
)

在package.xml的<package>标签下添加：

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

3. 构建test_interface包

cd <your ros root dir>
colcon build --packages-select test_interface

这一步会生成支持C++和Python的接口文件，即可用Python使用该接口也可用C++。

二、编写pub/sub程序

在目录build/test_interface/rosidl_adapter/test_interface/msg下找到Test.idl文件，其内容如下：

module test_interface {
  module msg {
    struct TestEvt {
      string name;

      uint32 uintA;

      uint32 payload_size;

      sequence<octet> payload;
    };
  };
};

通过该idl文件生成fastdds接口代码

fastddsgen -example CMake -typeros2 -d <target_dir> TestEvt.idl

需要注意，先找到接口包生成的头文件<workspace>/build/test_interface/rosidl_generator_cpp/test_interface/msg/test_evt.hpp该文件就是从.msg生成的头文件，文件名为TestEvt.msg生成的头文件test_interface/msg/test_evt.hpp，类名TestEvt

publisher.cpp

#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "test_interface/msg/test_evt.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. */

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

private:
  void timer_callback()
  {
    auto message = test_interface::msg::TestEvt();// std_msgs::msg::String();
    message.name = "Hello, world! " + std::to_string(count_);
    message.second = count_++;
    RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.name.c_str());
    publisher_->publish(message);
  }
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<test_interface::msg::TestEvt>::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;
}

subscriber.cpp

#include <functional>
#include <memory>

#include "rclcpp/rclcpp.hpp"
#include "test_interface/msg/test_evt.hpp"                                       // CHANGE

using std::placeholders::_1;

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

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

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

修改cmake文件：

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(test_interface REQUIRED)

add_executable(talker src/publisher.cpp)
ament_target_dependencies(talker rclcpp test_interface)
add_executable(listener src/subscriber.cpp)
ament_target_dependencies(listener rclcpp test_interface)

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

修改xml文件：

  <buildtool_depend>ament_cmake</buildtool_depend>
  <depend>test_interface</depend>

构建并验证是否正常

cd <workspace>
colcon build

on one terminal:

source install/setup.bash
ros2 run cpp_pubsub talker

on another terminal:

source install/setup.bash
ros2 run cpp_pubsub listener

此时两个终端应该能看到数据收发

三、配置discovery server

在一个新终端中开启discovery服务

fastdds discovery -i 0 -l 0.0.0.0 -p 11811
# 输入fastdds discovery -h有更多帮助信息

如果服务在本机上，则需要设置下面的地址为127.0.0.1

talker终端：

export ROS_DISCOVERY_SERVER=127.0.0.1:11811
ros2 run cpp_pubsub talker

listener终端：

export ROS_DISCOVERY_SERVER=127.0.0.1:11811
ros2 run cpp_pubsub listener

此时应该能看到两个终端的收发。

此时ros2 topic list不能看到对应topic，需要开启SUPER_CLIENT模式，具体详见下面的references
5 ros2 检视

export FASTRTPS_DEFAULT_PROFILES_FILE=<path/to/>super_client_configuration_file.xml

四、编写fastrtps程序

相关库安装见：https://blog.csdn.net/surfaceyan/article/details/137162608

在ros2中显示的topic为InterfaceTestTopic，fastdds中对应的topic为rt/InterfaceTestTopic，data type通过工具生成已经填入为test_interface::msg::dds_::test_evt_

据此修改例子：https://github.com/eProsima/Fast-DDS/tree/master/examples/cpp/discovery_server

五、配置ros与远程fastdds程序连接

ros端

new terminal

fastdds discovery -i 0 -l 0.0.0.0 -p 11811

new terminal

# udpv4
export ROS_DISCOVERY_SERVER=127.0.0.1:11811
ros2 run cpp_pubsub talker

export ROS_DISCOVERY_SERVER=TCPv4:[127.0.0.1]:42100

远程

./DiscoveryServerExample publisher -c <ros主机ip> -p <fastdds discovery端口号:11811 >

此时应该能看到ros2节点和远程无ros主机进行通讯

以固定周期发送数据：

yes | while read line; do echo "$line"; sleep 0.01; done | ./TestEvt publisher

注意项

不同版本的DDS存在兼容性问题。
需要注意要保持通讯实体之间用相同的DDS版本

根据topic所属的ROS2子系统，ROS 2 会向DDS中的topic添加特殊的token。更多信息请参考references [6, 7]。

有关fastddsgen的选项参考[5]

---

references

1. https://github.com/briancaglioni/FASTDDS_ROS2_Pub_Sub_Guide
2. https://docs.ros.org/en/humble/Concepts/Basic/About-Interfaces.html
3. https://docs.ros.org/en/humble/Tutorials/Advanced/Discovery-Server/Discovery-Server.html
4. https://fast-dds.docs.eprosima.com/en/stable/fastdds/ros2/discovery_server/ros2_discovery_server.html
5. https://fast-dds.docs.eprosima.com/en/latest/fastddsgen/usage/usage.html
6. https://fast-dds.docs.eprosima.com/en/stable/fastdds/use_cases/rosbag_capture/rosbag_capture.html
7. https://design.ros2.org/articles/topic_and_service_names.html#examples-of-ros-names-to-dds-concepts