< >
Home » ROS2入门教程 » ROS2入门教程-使用ROS1bridge记录和回放topic数据

ROS2入门教程-使用ROS1bridge记录和回放topic数据

说明:

  • 介绍如何使用ROS1bridge记录和回放topic数据

步骤:

  • 运行 ROS 1 roscore
# Shell A:
. /opt/ros/kinetic/setup.bash
# Or, on OSX, something like:
# . ~/ros_catkin_ws/install_isolated/setup.bash
roscore
  • 运行ROS 1 <=> ROS 2 dynamic_bridge
# Shell B:
. /opt/ros/kinetic/setup.bash
# Or, on OSX, something like:
# . ~/ros_catkin_ws/install_isolated/setup.bash
. /opt/ros/ardent/setup.bash
# Or, if building ROS 2 from source:
# . <workspace-with-bridge>/install/setup.bash
export ROS_MASTER_URI=http://localhost:11311
ros2 run ros1_bridge dynamic_bridge --bridge-all-topics
  • 启动 ROS 2 程序来模拟我们的乌龟式机器人,将cam2image使用该-b选项运行程序
# Shell C:
. /opt/ros/ardent/setup.bash
# Or, if building ROS 2 from source:
# . <workspace-with-bridge>/install/setup.bash
ros2 run image_tools cam2image -- -b
  • 运行一个简单的 Python 脚本来模拟来自 Kobuki 基础的odom和imu_data主题
#!/usr/bin/env python3

import sys
import time

import rclpy

from nav_msgs.msg import Odometry
from sensor_msgs.msg import Imu

def main():
    rclpy.init(args=sys.argv)

    node = rclpy.create_node('emulate_kobuki_node')

    imu_publisher = node.create_publisher(Imu, 'imu_data')
    odom_publisher = node.create_publisher(Odometry, 'odom')

    imu_msg = Imu()
    odom_msg = Odometry()
    counter = 0
    while True:
        counter += 1
        now = time.time()
        if (counter % 50) == 0:
            odom_msg.header.stamp.sec = int(now)
            odom_msg.header.stamp.nanosec = int(now * 1e9) % 1000000000
            odom_publisher.publish(odom_msg)
        if (counter % 100) == 0:
            imu_msg.header.stamp.sec = int(now)
            imu_msg.header.stamp.nanosec = int(now * 1e9) % 1000000000
            imu_publisher.publish(imu_msg)
            counter = 0
        time.sleep(0.001)


if __name__ == '__main__':
    sys.exit(main())
  • 在新的 ROS 2 shell 中运行此 python 脚本
# Shell D:
. /opt/ros/ardent/setup.bash
# Or, if building ROS 2 from source:
# . <workspace-with-bridge>/install/setup.bash
python3 emulate_kobuki_node.py
  • 在新的 ROS 1 shell 中查看可用主题
# Shell E:
. /opt/ros/kinetic/setup.bash
# Or, on OSX, something like:
# . ~/ros_catkin_ws/install_isolated/setup.bash
rostopic list
  • 结果如下:
% rostopic list
/image
/imu_data
/odom
/rosout
/rosout_agg
  • 在同一个 shell 中记录这些数据
# Shell E:
rosbag record /image /imu_data /odom
  • 几秒钟后,您可以Ctrl-c执行rosbag命令并查看文件有多大,结果如下:
% ls -lh
total 0
-rw-rw-r-- 1 william william  12M Feb 23 16:59 2017-02-23-16-59-47.bag
  • 使用 rosbag 和 ROS 1 Bridge 回放主题数据

  • 在一个新的 shell 中启动roscore

# Shell P:
. /opt/ros/kinetic/setup.bash
# Or, on OSX, something like:
# . ~/ros_catkin_ws/install_isolated/setup.bash
roscore
  • 在另一个shell中运行dynamic_bridge
# Shell Q:
. /opt/ros/kinetic/setup.bash
# Or, on OSX, something like:
# . ~/ros_catkin_ws/install_isolated/setup.bash
. /opt/ros/ardent/setup.bash
# Or, if building ROS 2 from source:
# . <workspace-with-bridge>/install/setup.bash
export ROS_MASTER_URI=http://localhost:11311
ros2 run ros1_bridge dynamic_bridge --bridge-all-topics
  • 在另一个新 shell 中播放包数据
# Shell R:
. /opt/ros/kinetic/setup.bash
# Or, on OSX, something like:
# . ~/ros_catkin_ws/install_isolated/setup.bash
rosbag play --loop path/to/bag_file
  • 确保替换path/to/bag_file为要播放的包文件的路径

  • 现在数据正在播放并且网桥正在运行,结果如下

# Shell S:
. /opt/ros/ardent/setup.bash
# Or, if building ROS 2 from source:
# . <workspace-with-bridge>/install/setup.bash
ros2 topic list
ros2 topic echo /odom

% ros2 topic list
/clock
/image
/imu_data
/odom
/parameter_events
  • 使用showimage工具查看从包中播放的图像
ros2 run image_tools showimage

纠错,疑问,交流: 请进入讨论区点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: ros2入门教程