ROS2 foxy 源码解析 -- 当创建自定义消息的pub时，处理msg的流程

ros2中对于消息的需求个人理解有如下几点：

能够转换为不同dds vendor需要的idl
类型支持，对于上层应用统一表现为ros的数据结构，对于下层的dds适配不同的类型转换
由于ros的层次结构，rclcpp->rcl->rmw->dds, 消息类型如何传递是个特别关注的问题

假设我们定义的消息文件为： src/hobot/hb_common_interfaces/hb_sensor_msgs/msg/SensorsDataHeader.msg
那么ros2会在build目录生成一些转换文件(仅以CPP为例):

build/hb_sensor_msgs/rosidl_generator_cpp/hb_sensor_msgs/msg/sensors_data_header.hpp
// 仅包含以下三个头文件
build/hb_sensor_msgs/rosidl_generator_cpp/hb_sensor_msgs/msg/detail/sensors_data_header__builder.hpp
// build模板的偏特化实现
build/hb_sensor_msgs/rosidl_generator_cpp/hb_sensor_msgs/msg/detail/sensors_data_header__struct.hpp
// msg生成的数据结构类，ros的应用层使用的就是该类
build/hb_sensor_msgs/rosidl_generator_cpp/hb_sensor_msgs/msg/detail/sensors_data_header__traits.hpp
// 类型萃取相关的，用于模板中的类型推导。主要是偏特化实现了date_type方法，返回消息的名称描述

以下生成的文件，主要解决类型支持以及在不同层次之间数据的传递问题

build/hb_sensor_msgs/rosidl_typesupport_cpp/hb_sensor_msgs/msg/sensors_data_header__type_support.cpp
// 关键的文件，rosidl_message_type_support_t数据结构的赋值，包含了msg所有的关联信息，尤其是关联到了fastrtps的类型支持
// 从rclcpp转到rcl再到rwm，都是靠这个数据结构完成消息类型信息的传递
build/hb_sensor_msgs/rosidl_typesupport_fastrtps_cpp/hb_sensor_msgs/msg/detail/sensors_data_header__rosidl_typesupport_fastrtps_cpp.hpp
// fastrtps_cpp模式下，序列化相关的函数定义
build/hb_sensor_msgs/rosidl_typesupport_fastrtps_cpp/hb_sensor_msgs/msg/detail/dds_fastrtps/sensors_data_header__type_support.cpp
// fastrtps_cpp模式下，fastrtps类型支持的关键文件。这里也有一个rosidl_message_type_support_t数据结构的赋值，可以理解为重载。
build/hb_sensor_msgs/rosidl_typesupport_introspection_cpp/hb_sensor_msgs/msg/detail/sensors_data_header__rosidl_typesupport_introspection_cpp.hpp
// introspection_cpp模式下的头文件定义，暂时不研究
build/hb_sensor_msgs/rosidl_typesupport_introspection_cpp/hb_sensor_msgs/msg/detail/sensors_data_header__type_support.cpp
// introspection_cpp模式下的类型支持定义，暂时不研究

同时，对应生成以下几个so，用于动态加载

build/hb_sensor_msgs/libhb_sensor_msgs__rosidl_typesupport_cpp.so
build/hb_sensor_msgs/libhb_sensor_msgs__rosidl_typesupport_fastrtps_cpp.so
build/hb_sensor_msgs/libhb_sensor_msgs__rosidl_typesupport_introspection_cpp.so

我们来梳理下，让创建一个消息的publisher时，消息类型是如何展开并能让dds知晓的

在写应用程序时，我们需要调用node节点的create_publisher方法创建一个特定消息的发布端，类似这样
pub = node->create_publisher<MsgT>("chatter", 10); // implicitly KeepLast
该方法实现在src/ros2/rclcpp/rclcpp/include/rclcpp/node_impl.hpp中

template<typename MessageT, typename AllocatorT, typename PublisherT>
std::shared_ptr<PublisherT> Node::create_publisher(...) {
  return rclcpp::create_publisher<MessageT, AllocatorT, PublisherT>(...);
}

进入到 src/ros2/rclcpp/rclcpp/include/rclcpp/create_publisher.hpp

template<
  typename MessageT, typename AllocatorT = std::allocator<void>,
  typename PublisherT = rclcpp::Publisher<MessageT, AllocatorT>, typename NodeT>
std::shared_ptr<PublisherT> create_publisher( NodeT & node, const std::string & topic_name,...) {
  // Create the publisher.
  auto pub = node_topics->create_publisher( topic_name,
    rclcpp::create_publisher_factory<MessageT, AllocatorT, PublisherT>(options), qos);
}

调用了一个工厂方法创建pub，进入到 src/ros2/rclcpp/rclcpp/include/rclcpp/publisher_factory.hpp

template<typename MessageT, typename AllocatorT, typename PublisherT> PublisherFactory
create_publisher_factory(const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options) {
  PublisherFactory factory {
    // factory function that creates a MessageT specific PublisherT
    [options](...) -> std::shared_ptr<PublisherT> {
      auto publisher = std::make_shared<PublisherT>(node_base, topic_name, qos, options);
      //...
      return publisher; }
  };
}

将创建pub的过程包装成了可调用对象，存放在PublisherFactory数据结构中。实际创建的步骤就是匿名函数了
PublisherT的定义是在create_publisher.hpp中，typename PublisherT = rclcpp::Publisher<MessageT, AllocatorT>
OK，跟进到src/ros2/rclcpp/rclcpp/include/rclcpp/publisher.hpp

template<typename MessageT, typename AllocatorT = std::allocator<void>>
class Publisher : public PublisherBase {
public:
  Publisher(...) : PublisherBase( node_base, topic,
      *rosidl_typesupport_cpp::get_message_type_support_handle<MessageT>(),
      options.template to_rcl_publisher_options<MessageT>(qos)), options_(options),
    message_allocator_(new MessageAllocator(*options.get_allocator().get())) {}

注意，这里的*rosidl_typesupport_cpp::get_message_type_support_handle<MessageT>()是关键，这个表达式返回一个rosidl_message_type_support_t的结构体，里边包含了改消息的类型支持所有信息。
该函数的定义在src/ros2/rosidl/rosidl_runtime_cpp/include/rosidl_typesupport_cpp/message_type_support.hpp

template<typename T>
const rosidl_message_type_support_t * get_message_type_support_handle();

是一个模板函数，实际上，自定义的msg生成的代码中，有对此函数模板的特化实现。注意，之前生成的文件中有多个get_message_type_support_handle函数的特化，但在不同的命名空间中，这里是先调用的rosidl_typesupport_cpp命名空间下的函数，也就是说，会调用到build/hb_sensor_msgs/rosidl_typesupport_cpp/hb_sensor_msgs/msg/sensors_data_header__type_support.cpp中的实现，它返回一个static的rosidl_message_type_support_t结构体，我们来看看这个结构体里有什么：

static const rosidl_message_type_support_t SensorsDataHeader_message_type_support_handle = {
  ::rosidl_typesupport_cpp::typesupport_identifier, // 类型支持的标识 字符串：rosidl_typesupport_cpp
  reinterpret_cast<const type_support_map_t *>(&_SensorsDataHeader_message_typesupport_map),  // 类型支持的详细信息map表
  ::rosidl_typesupport_cpp::get_message_typesupport_handle_function, // func 获取类型支持的处理函数
};
// 展开说下这个map里有什么信息
static const type_support_map_t _SensorsDataHeader_message_typesupport_map = {
  2,                 // 默认有两种类型支持，fastrtps_cpp 和 introspection_cpp
  "hb_sensor_msgs",  // package_name
  &_SensorsDataHeader_message_typesupport_ids.typesupport_identifier[0], 
  // 支持的类型标志，二维数组，"rosidl_typesupport_fastrtps_cpp" "rosidl_typesupport_introspection_cpp"
  &_SensorsDataHeader_message_typesupport_symbol_names.symbol_name[0],
  // 实际的类型支持接口的符号表，这里的内容是用来跟之后运行时加载动态库找对应的函数入口。这个符号表的内容是什么呢？
  // 可以到build/hb_sensor_msgs/rosidl_typesupport_fastrtps_cpp/hb_sensor_msgs/msg/detail/dds_fastrtps/sensors_data_header__type_support.cpp 中发现，有相同的宏，也就是说，这个符号表实际指向的是不同版本的类型支持接口，用来获取rosidl_message_type_support_t这一个通用的数据结构的
  &_SensorsDataHeader_message_typesupport_data.data[0],
  // 初始时为0，运行时会存放入口地址
};

有了这个结构体，基本就能拿到所有的信息了。继续看下入。
PublisherBase的构造函数中，会把这个结构体传给rcl_publisher_init，哈，终于调用到rcl层的方法了。
rcl_publisher_init也不处理该结构体，继续传给rmw层的rmw_create_publisher。这里方法有不同的实现了，我们关注fastrtps_cpp的 src/ros2/rmw_fastrtps/rmw_fastrtps_cpp/src/rmw_publisher.cpp, 然后继续调用到rmw_fastrtps_cpp::create_publisher() in src/ros2/rmw_fastrtps/rmw_fastrtps_cpp/src/publisher.cpp
关键代码：

  const rosidl_message_type_support_t * type_support = 
      get_message_typesupport_handle(type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_C);
  if (!type_support) {
    type_support = get_message_typesupport_handle(type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_CPP);
  }
  // 调用get_message_typesupport_handle，获取对应的fasttrpe_cpp类型的结构体
const rosidl_message_type_support_t * get_message_typesupport_handle(
  const rosidl_message_type_support_t * handle, const char * identifier) {
  rosidl_message_typesupport_handle_function func =
    (rosidl_message_typesupport_handle_function)(handle->func);
  return func(handle, identifier);  // 调用了结构体里的类型推断处理函数，相当于是一个回调
}

好嘛，又绕回来了，我们看下这个回调吧

const rosidl_message_type_support_t *get_message_typesupport_handle_function(
  const rosidl_message_type_support_t * handle, const char * identifier) {
  return rosidl_typesupport_cpp::get_typesupport_handle_function<rosidl_message_type_support_t>(handle, identifier);
}

注意这里传入的模板参数是rosidl_message_type_support_t， identifier是rosidl_typesupport_fastrtps_cpp
接下来是核心的类型支持处理函数了, 在文件src/ros2/rosidl_typesupport/rosidl_typesupport_cpp/src/type_support_dispatch.hpp

template<typename TypeSupport>
const TypeSupport * get_typesupport_handle_function(const TypeSupport * handle, const char * identifier) {
  if (handle->typesupport_identifier == rosidl_typesupport_cpp::typesupport_identifier) {
    // 判断数据结构体的类型支持标志是否一致
    const type_support_map_t * map = static_cast<const type_support_map_t *>(handle->data);
    // 拿到类型支持的详细信息关系表
    for (size_t i = 0; i < map->size; ++i) {
      if (strcmp(map->typesupport_identifier[i], identifier) != 0) {
        continue;
      }  // 找到rosidl_typesupport_fastrtps_cpp对应的index，开始加载对应的动态库
      rcpputils::SharedLibrary * lib = nullptr;
      if (!map->data[i]) {
        char library_name[1024];
        snprintf(library_name, 1023, "%s__%s", map->package_name, identifier);
        // 包名字 + 标识， hb_sensor_msgs__rosidl_typesupport_fastrtps_cpp，对应上了动态库的name
        std::string library_path = rcpputils::find_library_path(library_name);
        try {
          lib = new rcpputils::SharedLibrary(library_path.c_str());
        } catch (...) {...} 
        map->data[i] = lib; // 之前预留的初始化为0的data成员，现在指向了动态库入口地址
      }
      auto clib = static_cast<const rcpputils::SharedLibrary *>(map->data[i]);
      lib = const_cast<rcpputils::SharedLibrary *>(clib);
      void * sym = lib->get_symbol(map->symbol_name[i]); 
      // 找到对应的获取rosidl_message_type_support_t结构体的函数接口入口地址
      typedef const TypeSupport * (* funcSignature)(void); // 定义与接口函数一致的函数
      funcSignature func = reinterpret_cast<funcSignature>(sym); // 将入口地址强制转换
      const TypeSupport * ts = func(); // 然后就可以调用，执行获得关键结构体了
      return ts;
    }
  }
  return nullptr;
}

至此，rmw层已经从so中拿到了ros自定义消息的关键结构体，先看下typesupport_fastrtps_cpp中的结构体信息都包含哪些内容：

static message_type_support_callbacks_t _SensorsDataHeader__callbacks = {
  "hb_sensor_msgs::msg",    // message_namespace_
  "SensorsDataHeader",      // message_name_
  _SensorsDataHeader__cdr_serialize,
  _SensorsDataHeader__cdr_deserialize,
  _SensorsDataHeader__get_serialized_size,
  _SensorsDataHeader__max_serialized_size
};
static rosidl_message_type_support_t _SensorsDataHeader__handle = {
  rosidl_typesupport_fastrtps_cpp::typesupport_identifier,
  &_SensorsDataHeader__callbacks,
  get_message_typesupport_handle_function,  // 目前没用到
};

接下来我们看下rmw_fastrtps_cpp::create_publisher之后做了哪些处理

  auto callbacks = static_cast<const message_type_support_callbacks_t *>(type_support->data);
  // 拿到callback的信息map
  std::string type_name = _create_type_name(callbacks);
  // 获取msg的类型名，展开为dds命名空间 hb_sensor_msgs::msg::dds_::SensorsDataHeader_
  info->type_support_ = new (std::nothrow) MessageTypeSupport_cpp(callbacks);
  // 创建一个MessageTypeSupport对象，然后注册到dds中
  _register_type(participant_info->participant, info->type_support_);
  // 该注册函数调用eprosima::fastrtps::Domain::registerType(participant, typed_typesupport);

也就是说，rmw层对于自定义的msg处理时，先封装成fastdds::dds::TopicDataType的一个基类，然后注册到fastdds中
我们来看MessageTypeSupport_cpp的继承关系

using MessageTypeSupport_cpp = rmw_fastrtps_cpp::MessageTypeSupport;
namespace rmw_fastrtps_cpp {
  class MessageTypeSupport : public TypeSupport {
    public:
    explicit MessageTypeSupport(const message_type_support_callbacks_t * members);
  };
  class TypeSupport : public rmw_fastrtps_shared_cpp::TypeSupport {};
}
name rmw_fastrtps_shared_cpp {
  class TypeSupport : public eprosima::fastrtps::TopicDataType {};
}

在fastdds的TopicDataType接口基础上，rmw层的TypeSupport多了三个ros消息相关的方法：

  size_t getEstimatedSerializedSize(const void * ros_message, const void * impl) const override;
  bool serializeROSmessage(const void * ros_message, eprosima::fastcdr::Cdr & ser, const void * impl) const override;
  bool deserializeROSmessage(eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) const override;

MessageTypeSupport的构造函数会把消息类型支持的回调数据结构体保存下来，并在以上三个方法中回调使用。

暂时先解析到这里。