526互联

ROS2-发布矩阵类消息

发布时间 2023-04-08 19:16:30作者: XnobodyT

这个困扰我两个周的难题终于在翻阅官方文档之后得到了解答，简而言之就是对于相关头文件的理解（其实是我懒得自定义头文件），下面解释一下相关代码。

发布者代码

#include <chrono>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"

int main(int argc, char **argv)
{
  // 初始化ROS2节点
  rclcpp::init(argc, argv);
  auto node = std::make_shared<rclcpp::Node>("matrix_publisher");

  // 创建发布者
  auto publisher = node->create_publisher<std_msgs::msg::Float64MultiArray>("matrix_topic", 10);

  // 定义矩阵数据
  std::vector<double> data {1.0, 2.0, 3.0, 4.0, 5.0, 6.0};
  auto message = std::make_shared<std_msgs::msg::Float64MultiArray>();
  message->layout.dim.push_back(std_msgs::msg::MultiArrayDimension());
  message->layout.dim[0].size = 2;
  message->layout.dim[0].stride = 3;
  message->layout.dim[0].label = "rows";
  message->layout.dim.push_back(std_msgs::msg::MultiArrayDimension());
  message->layout.dim[1].size = 3;
  message->layout.dim[1].stride = 1;
  message->layout.dim[1].label = "cols";
  message->data = data;

  // 发布矩阵数据
  rclcpp::WallRate loop_rate(10);
  while (rclcpp::ok())
  {
    publisher->publish(*message);
    rclcpp::spin_some(node);
    loop_rate.sleep();
  }

  return 0;
}

订阅者代码

#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"

void callback(const std_msgs::msg::Float64MultiArray::SharedPtr message)
{
  // 获取矩阵数据
  std::vector<double> data = message->data;

  // 打印矩阵数据
  std::cout << "Received matrix:" << std::endl;
  for (size_t i = 0; i < message->layout.dim[0].size; i++)
  {
    for (size_t j = 0; j < message->layout.dim[1].size; j++)
    {
      std::cout << data[i * message->layout.dim[1].size + j] << " ";
    }
    std::cout << std::endl;
  }
}

int main(int argc, char **argv)
{
  // 初始化ROS2节点
  rclcpp::init(argc, argv);
  auto node = std::make_shared<rclcpp::Node>("matrix_subscriber");

  // 创建订阅者
  auto subscriber = node->create_subscription<std_msgs::msg::Float64MultiArray>(
    "matrix_topic", 10, callback);

  // 进入ROS2事件循环
  rclcpp::spin(node);

  return 0;
}

CmakeLists.txt

cmake_minimum_required(VERSION 3.5)
project(example_topic_rclcpp)

# 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)
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(topic_publisher_01 src/topic_publisher_01.cpp)
add_executable(topic_subscribe_01 src/topic_subscribe_01.cpp)
ament_target_dependencies(topic_publisher_01 rclcpp std_msgs)
ament_target_dependencies(topic_subscribe_01 rclcpp std_msgs)

install(TARGETS
  topic_publisher_01
  DESTINATION lib/${PROJECT_NAME}
)
install(TARGETS
topic_subscribe_01
  DESTINATION lib/${PROJECT_NAME}
)

ament_package()

package.xml

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>example_topic_rclcpp</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="moon@todo.todo">moon</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rclcpp</depend>
  <depend>std_msgs</depend>
  
  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

代码解释

ROS2发布者订阅者的基本框架大家都理解，这里只说一下特殊的点,这里用面向过程的思想编写，希望大家见谅一下。

  std::vector<double> data {1.0, 2.0, 3.0, 4.0, 5.0, 6.0};
  auto message = std::make_shared<std_msgs::msg::Float64MultiArray>();
  message->layout.dim.push_back(std_msgs::msg::MultiArrayDimension());
  message->layout.dim[0].size = 2;
  message->layout.dim[0].stride = 3;
  message->layout.dim[0].label = "rows";
  message->layout.dim.push_back(std_msgs::msg::MultiArrayDimension());
  message->layout.dim[1].size = 3;
  message->layout.dim[1].stride = 1;
  message->layout.dim[1].label = "cols";
  message->data = data;

std_msgs::msg::Float64MultiArray消息类型其实是一种多维数组类型，上述代码简单来说就是创建一个2*3的矩阵类型进行发布
订阅者不详细说了，大家都可以理解。