【rosbridge-websocket】跨网络的ROS1与ROS2通讯法(上)

前言


1 介绍与测试

1-1 介绍
1-1-1 rosbridge
  • rosbridge 是 ROS 生态中的一个通信桥接工具,它能够将 ROS 中的 Topic、Service、Parameter 等接口转换为标准的 JSON 消息,并通过网络进行传输。
  • 在传统 ROS 通信中,节点之间依赖 ROS Master(ROS1)或 DDS(ROS2)进行发现与通信。当不同版本的 ROS(例如 ROS1 Melodic 与 ROS2 Humble)需要跨机器通信时,往往需要使用 ros1_bridge、DDS 配置或者自定义网络协议,配置较为复杂。
  • rosbridge 则提供了一种更加通用的解决方案:
    • ROS 节点负责发布和订阅话题;
    • rosbridge 将 ROS 消息转换为 JSON 格式;
    • 外部程序通过 WebSocket 与 rosbridge 进行通信;
    • 无需关心 ROS 版本差异。
  • 其整体架构如下:
bash 复制代码
ROS Topic
    │
    ▼
rosbridge_server
    │
 JSON
    │
WebSocket
    │
    ▼
外部程序
  • 因此,只要能够连接到 rosbridge 提供的 WebSocket 服务,无论是 Python、C++、JavaScript、Unity 还是 ROS2 节点,都可以访问 ROS1 中的话题数据。

1-1-2 WebSocket
  • WebSocket 是一种基于 TCP 的全双工网络通信协议。
  • 与传统 HTTP 请求-响应模式不同,WebSocket 在建立连接后,客户端与服务器可以持续保持连接,并能够双向主动发送数据。
  • 其特点包括:
    • 长连接通信;
    • 双向实时传输;
    • 延迟低;
    • 开销小;
    • 支持跨平台和跨语言开发。
  • 在 rosbridge 中:
    • rosbridge_server 作为 WebSocket Server;
    • 外部程序作为 WebSocket Client;
    • ROS 消息会被封装为 JSON 数据进行传输。
  • 例如,一个 ROS Topic 消息可能会被转换为:
json 复制代码
{
    "op":"publish",
    "topic":"/odom",
    "msg":{
        "pose":{
            "pose":{
                "position":{
                    "x":1.2,
                    "y":0.8,
                    "z":0.0
                }
            }
        }
    }
}
  • 客户端只需要解析 JSON,即可获得对应的话题数据。

1-2 测试
  • 我们要做的测试如下:
    • ROS1端启动rosbridge-server服务,并启动一些话题发布
    • ROS2端启动一个简单的websocket-client订阅对应的话题
  • 测试前确保:两边机器连接在同一个局域网下且可以互相ping通彼此的ip(可以通过ifconfig查看自己的ip

1-2-1 ROS1端
  • 安装rosbridge-server
bash 复制代码
sudo apt install ros-melodic-rosbridge-server
  • 启动:rosbridge-server
bash 复制代码
source /opt/ros/melodic/setup.bash
roslaunch rosbridge_server rosbridge_websocket.launch
  • 可以看到websocket服务被启用在端口9090

1-2-2 ROS2端
  • 我们使用pythonwebsocket进行测试,
bash 复制代码
pip install websocket-client
  • 我们简单写一个话题订阅(确保你的ROS1端确实在发送)
python 复制代码
from websocket import create_connection

ws = create_connection("ws://192.168.10.170:9090")

ws.send('{"op":"subscribe","topic":"/odom"}')

while True:
    print(ws.recv())
  • 可以看到我们订阅到了ROS1端发送的odom数据,为json格式

  • 同时我们也可以写一个发布
python 复制代码
from websocket import create_connection
import json
import time

ws = create_connection("ws://192.168.10.170:9090")

# 声明话题
ws.send(json.dumps({
    "op": "advertise",
    "topic": "/test",
    "type": "std_msgs/String"
}))

count = 0

while True:
    ws.send(json.dumps({
        "op": "publish",
        "topic": "/test",
        "msg": {
            "data": f"hello {count}"
        }
    }))

    print(f"publish: hello {count}")

    count += 1
    time.sleep(1)
  • 执行期间确保ROS1端的rosbridge-websocket的服务端是开着的
  • 可以看到ROS1端接收到了python发来的话题通讯

2 C++实现

2-1 API说明
2-1-1 话题发布
  • 声明发布者
json 复制代码
{
  "op": "advertise",
  "topic": "/test",
  "type": "std_msgs/String",
  "id": "publisher_1",
  "latch": false,
  "queue_size": 100
}
  • 参数说明:
参数 类型 说明
op string 固定为 advertise
topic string Topic 名称
type string ROS 消息类型
id string 可选,请求唯一标识
latch bool 是否启用 latched topic
queue_size int 发布队列大小

  • 发布话题
json 复制代码
{
  "op": "publish",
  "topic": "/test",
  "id": "publish_1",
  "msg": {
    "data": "hello"
  }
}

  • 取消发布
json 复制代码
{
  "op": "unadvertise",
  "topic": "/test",
  "id": "publisher_1"
}

2-1-2 话题订阅
  • 订阅话题
json 复制代码
{
  "op": "subscribe",
  "topic": "/odom",
  "id": "subscriber_1",
  "type": "nav_msgs/Odometry",
  "throttle_rate": 100,
  "queue_length": 1,
  "fragment_size": 0,
  "compression": "none"
}
  • 参数说明:
参数 类型 说明
op string 固定为 subscribe
topic string Topic 名称
id string 可选,请求唯一标识
type string 可选,消息类型
throttle_rate int 限流周期(ms),100=10Hz
queue_length int 缓冲队列长度
fragment_size int 消息分片大小(字节)
compression string 压缩方式
  • compression 可选值:
text 复制代码
none
png
cbor
cbor-raw

  • 接收到的格式
json 复制代码
{
  "op": "publish",
  "topic": "/odom",
  "msg": {
    ...
  }
}

  • 取消订阅
json 复制代码
{
  "op": "unsubscribe",
  "topic": "/odom",
  "id": "subscriber_1"
}

2-2 安装必要依赖库
bash 复制代码
sudo apt install libwebsocketpp-dev
sudo apt install nlohmann-json3-dev
sudo apt install libboost-all-dev
  • 项目结构如下:
bash 复制代码
websocket_client_ros2
        ├── CMakeLists.txt
        ├── include
        │   └── websocket_client_ros2
        │       ├── RosbridgeClient.hpp
        │       ├── RosbridgeParser.hpp
        │       ├── RosWsBridgeNode.hpp
        │       ├── TopicDispatcher.hpp
        │       ├── TopicPublishBase
        │       │   ├── OdomPublish.hpp
        │       │   └── TopicPublishBase.hpp
        │       └── TopicSubscribeBase
        │           ├── OdomSubscribe.hpp
        │           └── TopicSubscribeBase.hpp
        ├── package.xml
        ├── scripts
        │   └── main.py
        └── src
            ├── main.cpp
            ├── RosbridgeClient.cpp
            ├── RosbridgeParser.cpp
            └── TopicDispatcher.cpp

2-3 核心结构
2-3-1 总体架构
  • 本系统采用"协议解耦 + 话题分发 + 插件式Bridge扩展"的设计方式,实现 ROS1 ↔ ROS2 通过 rosbridge-websocket 的双向通信。
  • 整体结构如下:
bash 复制代码
        ROS2 Topic
            │
   ┌────────▼────────┐
   │  TopicPublish   │   (ROS2 → WebSocket)
   └────────┬────────┘
            │
            ▼
     RosbridgeClient
   (WebSocket通信层)
            │
            ▼
      JSON Message
            │
   ┌────────▼────────┐
   │ RosbridgeParser │   (JSON解析/封装)
   └────────┬────────┘
            │
            ▼
   ┌─────────────────┐
   │ TopicDispatcher │   (按topic分发)
   └────────┬────────┘
            │
   ┌────────▼────────┐
   │ TopicSubscribe  │   (WebSocket → ROS2)
   └────────┬────────┘
            │
            ▼
        ROS2 Topic

2-3-2 各模块职责划分
2-3-2-1 RosbridgeClient(通信层)
  • 负责:
    • WebSocket 连接管理
    • JSON 字符串收发
    • 连接状态回调
  • 特点:
    • 不关心 ROS
    • 不解析 JSON
    • 只做"TCP级别的数据通道"

2-3-2-2 RosbridgeParser(协议层)
  • 负责:
    • JSON ↔ ROSBridge协议转换
    • 过滤 publish / subscribe
    • 构造标准 rosbridge message
  • 核心能力:
    • encodeSubscribe()
    • encodePublish()
    • parse()

2-3-2-3 TopicDispatcher(路由层)
  • 负责:
    • 根据 topic 名称分发消息
    • 管理 callback registry
  • 本质:把 /odom /cmd_vel /imu 分流到不同 handler
cpp 复制代码
std::unordered_map<std::string, callback>

2-3-2-4 TopicPublishBase(ROS2 → ROS1)
  • 负责:
    • ROS2 subscribe
    • 转 JSON
    • 发 WebSocket
  • 特点:
    • ROS2 → WebSocket 单向桥

2-3-2-5 TopicSubscribeBase(ROS1 → ROS2)
  • 负责:
    • dispatcher 注册 handler
    • JSON → ROS2 msg
    • ROS2 publish
  • 特点:
    • WebSocket → ROS2 单向桥

2-3-2-6 RosWsBridgeNode(系统入口)
  • 负责:
    • 初始化 ROS2 Node
    • 初始化 WebSocket
    • 注册所有 bridge topic
    • 统一生命周期管理
  • 特点:
    • "不做解析,只做组装"

2-4 核心实现
  • RosbridgeClient.hpp
cpp 复制代码
#pragma once

#include <functional>
#include <string>

#include <websocketpp/config/asio_no_tls_client.hpp>
#include <websocketpp/client.hpp>

class RosbridgeClient
{
public:
    using RawCallback = std::function<void(const std::string&)>;
    using ConnectedCallback = std::function<void()>;

    RosbridgeClient();

    bool connect(const std::string& uri);
    void run();
    void send(const std::string& json);

    void setRawCallback(RawCallback cb);
    void setConnectedCallback(ConnectedCallback cb);

private:
    using WsClient =
        websocketpp::client<websocketpp::config::asio_client>;

    void onOpen(websocketpp::connection_hdl hdl);
    void onMessage(websocketpp::connection_hdl hdl, WsClient::message_ptr msg);

private:
    WsClient client_;
    websocketpp::connection_hdl hdl_;

    RawCallback raw_cb_;
    ConnectedCallback connected_cb_;
};

  • RosbridgeClient.cpp
cpp 复制代码
#include "../include/websocket_client_ros2/RosbridgeClient.hpp"
#include <iostream>
RosbridgeClient::RosbridgeClient()
{
    client_.clear_access_channels(websocketpp::log::alevel::all);
    client_.init_asio();

    client_.set_open_handler([this](auto hdl)
    {
        onOpen(hdl);
    });

    client_.set_message_handler([this](auto, auto msg)
    {
        if(raw_cb_)
            raw_cb_(msg->get_payload());
    });
}

bool RosbridgeClient::connect(const std::string& uri)
{
    websocketpp::lib::error_code ec;

    auto con = client_.get_connection(uri, ec);
    if(ec)
    {
        std::cerr << ec.message() << std::endl;
        return false;
    }

    client_.connect(con);
    std::thread([this]()
    {
        client_.run();
    }).detach();

    return true;
}

void RosbridgeClient::run()
{
    client_.run();
}

void RosbridgeClient::send(const std::string& json)
{
    websocketpp::lib::error_code ec;

    client_.send(
        hdl_,
        json,
        websocketpp::frame::opcode::text,
        ec);

    if(ec)
        std::cerr << "[send error] " << ec.message() << std::endl;
}

void RosbridgeClient::setRawCallback(RawCallback cb)
{
    raw_cb_ = std::move(cb);
}

void RosbridgeClient::setConnectedCallback(ConnectedCallback cb)
{
    connected_cb_ = std::move(cb);
}

void RosbridgeClient::onOpen(websocketpp::connection_hdl hdl)
{
    hdl_ = hdl;
    std::cout << "Connected" << std::endl;

    if(connected_cb_)
        connected_cb_();
}

void RosbridgeClient::onMessage(websocketpp::connection_hdl, WsClient::message_ptr msg)
{
    if(raw_cb_)
        raw_cb_(msg->get_payload());
}

  • RosbridgeParser.hpp
cpp 复制代码
#pragma once

#include <functional>
#include <string>
#include <nlohmann/json.hpp>

class RosbridgeParser
{
public:
    struct TopicMessage
    {
        std::string topic;
        nlohmann::json msg;
    };

    using MsgCallback = std::function<void(const TopicMessage&)>;

    void setCallback(MsgCallback cb);

    void parse(const std::string& raw);

    std::string encodeSubscribe(const std::string& topic);
    std::string encodePublish(const TopicMessage& msg);

private:
    MsgCallback cb_;
};

  • RosbridgeParser.cpp
cpp 复制代码
#include "../include/websocket_client_ros2/RosbridgeParser.hpp"

void RosbridgeParser::setCallback(MsgCallback cb)
{
    cb_ = std::move(cb);
}

void RosbridgeParser::parse(const std::string& raw)
{
    try
    {
        auto j = nlohmann::json::parse(raw);

        if(!j.contains("op"))
            return;

        if(j["op"] != "publish")
            return;

        TopicMessage tm;
        tm.topic = j["topic"];
        tm.msg = j["msg"];

        if(cb_)
            cb_(tm);
    }
    catch(...)
    {
    }
}

std::string RosbridgeParser::encodeSubscribe(const std::string& topic)
{
    nlohmann::json j;
    j["op"] = "subscribe";
    j["topic"] = topic;
    return j.dump();
}

std::string RosbridgeParser::encodePublish(const TopicMessage& msg)
{
    nlohmann::json j;
    j["op"] = "publish";
    j["topic"] = msg.topic;
    j["msg"] = msg.msg;
    return j.dump();
}

  • TopicPublishBase.hpp
cpp 复制代码
#pragma once
#include <rclcpp/rclcpp.hpp>
#include "../RosbridgeParser.hpp"

#include <memory>

class TopicPublishBase
{
public:
    TopicPublishBase(rclcpp::Node* node,
                  RosbridgeParser* parser,
                  RosbridgeClient* ws)
        : node_(node), parser_(parser), ws_(ws) {}

    virtual ~TopicPublishBase() = default;

protected:
    rclcpp::Node* node_;
    RosbridgeParser* parser_;
    RosbridgeClient* ws_;
};
  • TopicSubscribeBase.hpp
cpp 复制代码
#pragma once
#include <rclcpp/rclcpp.hpp>
#include "../RosbridgeParser.hpp"

#include <memory>

class TopicSubscribeBase
{
public:
    TopicSubscribeBase(rclcpp::Node* node, RosbridgeParser* parser)
        : node_(node), parser_(parser)
    {}

    virtual ~TopicSubscribeBase() = default;

    // 注册 Dispatcher handler
    virtual void registerHandler() = 0;

protected:
    rclcpp::Node* node_;
    RosbridgeParser* parser_;
};

2-5 测试实现
  • OdomPublish.hpp
cpp 复制代码
#pragma once
#include <nav_msgs/msg/odometry.hpp>
#include "./TopicPublishBase.hpp"
#include "../TopicDispatcher.hpp"


class OdomPublish : public TopicPublishBase
{
public:
    OdomPublish(rclcpp::Node* node,
                  RosbridgeParser* parser,
                  RosbridgeClient* ws)
        : TopicPublishBase(node, parser, ws)
    {
        sub_ = node_->create_subscription<nav_msgs::msg::Odometry>(
            "/odom_ros2", 10,
            std::bind(&OdomPublish::callback, this, std::placeholders::_1));
    }

private:
    void callback(const nav_msgs::msg::Odometry::SharedPtr msg)
    {
        RosbridgeParser::TopicMessage tm;
        tm.topic = "/odom_ros2";

        tm.msg["pose"]["pose"]["position"]["x"] = msg->pose.pose.position.x;
        tm.msg["pose"]["pose"]["position"]["y"] = msg->pose.pose.position.y;
        tm.msg["pose"]["pose"]["position"]["z"] = msg->pose.pose.position.z;

        ws_->send(parser_->encodePublish(tm));
    }

private:
    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_;
};

  • OdomSubscribe.hpp
cpp 复制代码
#pragma once
#include <nav_msgs/msg/odometry.hpp>
#include "./TopicSubscribeBase.hpp"
#include "../TopicDispatcher.hpp"


class OdomSubscribe : public TopicSubscribeBase
{
public:
    OdomSubscribe(rclcpp::Node* node,
               RosbridgeParser* parser,
               TopicDispatcher* dispatcher,
               RosbridgeClient* ws)
        : TopicSubscribeBase(node, parser),
          dispatcher_(dispatcher),
          ws_(ws)
    {
        pub_ = node_->create_publisher<nav_msgs::msg::Odometry>(
            "/odom_ros1", 10);
    }

    void registerHandler() override
    {
        dispatcher_->registerHandler("/odom_ros1", [&](const RosbridgeParser::TopicMessage& msg){
            handle(msg);
        });

        // 自动向 WebSocket 发送订阅请求
        ws_->send(parser_->encodeSubscribe("/odom_ros1"));
    }

private:
    void handle(const RosbridgeParser::TopicMessage& msg)
    {
        nav_msgs::msg::Odometry odom;
        odom.pose.pose.position.x = msg.msg["pose"]["pose"]["position"]["x"];
        odom.pose.pose.position.y = msg.msg["pose"]["pose"]["position"]["y"];
        odom.pose.pose.position.z = msg.msg["pose"]["pose"]["position"]["z"];
        pub_->publish(odom);
    }

private:
    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub_;
    TopicDispatcher* dispatcher_;
    RosbridgeClient* ws_;
};

  • main.cpp
cpp 复制代码
#include <rclcpp/rclcpp.hpp>
#include "../include/websocket_client_ros2/RosWsBridgeNode.hpp"

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

  • CMakeLists.txt
cpp 复制代码
cmake_minimum_required(VERSION 3.8)
project(websocket_client_ros2)

set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)


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(nav_msgs REQUIRED)
find_package(Boost REQUIRED COMPONENTS system thread)

include_directories(
    ${Boost_INCLUDE_DIRS}
)

add_executable(rosbridge_client
    ./src/main.cpp
    ./src/RosbridgeClient.cpp
    ./src/RosbridgeParser.cpp
    ./src/TopicDispatcher.cpp
)
ament_target_dependencies(rosbridge_client
    rclcpp
    nav_msgs
)

target_link_libraries(rosbridge_client
    ${Boost_LIBRARIES}
    pthread
    
)
install(TARGETS rosbridge_client
  DESTINATION lib/${PROJECT_NAME}
)
if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

2-6 扩展说明
  • 在现有的 RosWsBridgeNode + TopicPublishBase / TopicSubscribeBase 框架中,添加新的 ROS2 ↔ ROS1 通信话题非常简单,只需按照以下步骤操作:
  • 本节示例采用 /cmd_vel 话题,仅作演示,自定义话题替换即可

2-6-1 添加 ROS2 → ROS1 的话题(发布)
  1. 继承 TopicPublishBase
    创建一个新的发布类,例如 CmdVelPublish.hpp
cpp 复制代码
#pragma once
#include <geometry_msgs/msg/twist.hpp>
#include "./TopicPublishBase.hpp"

class CmdVelPublish : public TopicPublishBase
{
public:
    CmdVelPublish(rclcpp::Node* node,
                  RosbridgeParser* parser,
                  RosbridgeClient* ws)
        : TopicPublishBase(node, parser, ws)
    {
        sub_ = node_->create_subscription<geometry_msgs::msg::Twist>(
            "/cmd_vel_ros2", 10,
            std::bind(&CmdVelPublish::callback, this, std::placeholders::_1));
    }

private:
    void callback(const geometry_msgs::msg::Twist::SharedPtr msg)
    {
        RosbridgeParser::TopicMessage tm;
        tm.topic = "/cmd_vel_ros1";

        tm.msg["linear"]["x"] = msg->linear.x;
        tm.msg["linear"]["y"] = msg->linear.y;
        tm.msg["linear"]["z"] = msg->linear.z;

        tm.msg["angular"]["x"] = msg->angular.x;
        tm.msg["angular"]["y"] = msg->angular.y;
        tm.msg["angular"]["z"] = msg->angular.z;

        ws_->send(parser_->encodePublish(tm));
    }

private:
    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr sub_;
};
  1. RosWsBridgeNode 中注册
    在系统入口中创建对象即可:
cpp 复制代码
auto cmdvel_pub = std::make_shared<CmdVelPublish>(this, &parser_, &ws_);

2-6-2 添加 ROS1 → ROS2 的话题(订阅)
  1. 继承 TopicSubscribeBase
    创建一个订阅类,例如 CmdVelSubscribe.hpp
cpp 复制代码
#pragma once
#include <geometry_msgs/msg/twist.hpp>
#include "./TopicSubscribeBase.hpp"

class CmdVelSubscribe : public TopicSubscribeBase
{
public:
    CmdVelSubscribe(rclcpp::Node* node,
                    RosbridgeParser* parser,
                    TopicDispatcher* dispatcher,
                    RosbridgeClient* ws)
        : TopicSubscribeBase(node, parser),
          dispatcher_(dispatcher),
          ws_(ws)
    {
        pub_ = node_->create_publisher<geometry_msgs::msg::Twist>(
            "/cmd_vel_ros2", 10);
    }

    void registerHandler() override
    {
        dispatcher_->registerHandler("/cmd_vel_ros1", [&](const RosbridgeParser::TopicMessage& msg){
            handle(msg);
        });

        ws_->send(parser_->encodeSubscribe("/cmd_vel_ros1"));
    }

private:
    void handle(const RosbridgeParser::TopicMessage& msg)
    {
        geometry_msgs::msg::Twist twist;
        twist.linear.x = msg.msg["linear"]["x"];
        twist.linear.y = msg.msg["linear"]["y"];
        twist.linear.z = msg.msg["linear"]["z"];

        twist.angular.x = msg.msg["angular"]["x"];
        twist.angular.y = msg.msg["angular"]["y"];
        twist.angular.z = msg.msg["angular"]["z"];

        pub_->publish(twist);
    }

private:
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr pub_;
    TopicDispatcher* dispatcher_;
    RosbridgeClient* ws_;
};
  1. RosWsBridgeNode 中注册
    在节点初始化时创建对象并调用 registerHandler()
cpp 复制代码
auto cmdvel_sub = std::make_shared<CmdVelSubscribe>(this, &parser_, &dispatcher_, &ws_);cmdvel_sub->registerHandler();

3 测试

3-1 全程需要开的内容
  • ros1端:
bash 复制代码
roslaunch rosbridge_server rosbridge_websocket.launch
  • ros2端(请自己创建功能包)
bash 复制代码
ros2 run websocket_client_ros2 rosbridge_client 

3-2 ros1发布测试
  • ros1发布:
bash 复制代码
rostopic pub /odom_ros1 nav_msgs/Odometry "header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: 'odom'
child_frame_id: 'base_link'
pose:
  pose:
    position:
      x: 1.0
      y: 2.0
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
  covariance: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
twist:
  twist:
    linear:
      x: 0.0
      y: 0.0
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]" -r 1

  • 然后在ros2端测试
bash 复制代码
ros2 topic hz /odom_ros1

3-3 ros2发布测试
  • 由于rosbridge的特性,ros1接受来自websocket的话题前必须先打开订阅方,否则会出现如下的报错:

  • ros1端测试

cpp 复制代码
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>

void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
    ROS_INFO("Odom received:");
    ROS_INFO("x: %.3f y: %.3f z: %.3f",
             msg->pose.pose.position.x,
             msg->pose.pose.position.y,
             msg->pose.pose.position.z);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "odom_sub_test");
    ros::NodeHandle nh;

    ros::Subscriber sub = nh.subscribe(
        "/odom_ros2",
        10,
        odomCallback);

    ROS_INFO("odom_sub_test started...");

    ros::spin();
    return 0;
}
  • 然后优先打开订阅方
bash 复制代码
rosrun yalong_controller odom_s_test

  • 最后再打开ros2发布
bash 复制代码
ros2 topic pub -r 1 /odom_ros2 nav_msgs/msg/Odometry \
"{pose: {pose: {position: {x: 1.0, y: 2.0, z: 0.0}, \
orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}, \
twist: {twist: {linear: {x: 0.0, y: 0.0, z: 0.0}, \
angular: {x: 0.0, y: 0.0, z: 0.0}}}}"
  • 测试结果:

小结

  • 本文提供了一种基于rosbridge-websocket实现跨网络的ROS1与ROS2通讯。(c++实现)
  • 下期我们讲解具体实现
  • 如有错误,欢迎提出!
  • 感谢观看!!
相关推荐
圆弧YH1 小时前
python→ Language
python
梁辰兴1 小时前
计算机网络基础:电子邮件的信息格式
网络·计算机网络·电子邮件·计算机网络基础·梁辰兴·信息格式
j7~1 小时前
【C++】类和对象(下)--详解之再探构造函数,友元,static成员,类型转换等
开发语言·c++·类型转换·友元·匿名对象·内部类·编译器优化
装不满的克莱因瓶1 小时前
掌握神经网络的模型结构
人工智能·python·深度学习·神经网络·机器学习·ai
zincsweet1 小时前
Linux线程原理深度剖析:从CPU调度到pthread实现
linux·服务器
稷下元歌1 小时前
7天学会plc加机器视觉关于运动控制部份,配套视频在bib
开发语言·c++·git·vscode·python·docker·pip
薇茗1 小时前
【C++】 类与对象 基础篇
开发语言·c++·基础语法·类与对象
奋斗的小方1 小时前
Java进阶篇1-1:异常
java·开发语言·python
颜酱1 小时前
LangChain 调大模型:模板拼接 + invoke / stream / batch
python·langchain