ROS系列（三）：从零构建机器人通信系统 --- 包创建+发布订阅+Launch实战 & RViz/rqt可视化利器拆解！

一、 创建你的第一个ROS包(Package)

ROS开发的基石始于功能包（Package）。 掌握catkin_ws工作空间的构建、catkin_create_pkg命令创建包、理解CMakeLists.txt与package.xml关键配置，并添加首个Python节点，是构建任何ROS应用的必备起点。

1.1 工作空间(catkin_ws)是什么？（代码仓库）

1. 概念解析

• ROS工作空间是存放ROS包和相关文件的目录结构

• 类似于项目文件夹，包含所有源代码、依赖和构建结果

• 标准工作空间结构：

  catkin_ws/          # 工作空间根目录
      ├── build/      # 存放编译过程中生成的中间文件（自动生成）
      ├── devel/      # 存放最终生成的可执行文件和库（自动生成）
      └── src/        # 用户源代码目录（你在这里创建包）
          └── CMakeLists.txt  # 工作空间的CMake配置文件
  

3. 创建步骤

   # 1. 创建工作空间目录
   mkdir -p ~/catkin_ws/src
   
   # 2. 进入src目录
   cd ~/catkin_ws/src
   
   # 3. 初始化工作空间（创建顶层CMakeLists.txt）
   catkin_init_workspace
   
   # 4. 返回工作空间根目录
   cd ~/catkin_ws
   
   # 5. 构建工作空间（生成build和devel目录）
   catkin_make
   
   # 6. 激活工作空间环境（重要！每次新终端都需要）
   source devel/setup.bash
   
   提示：将激活命令加入`.bashrc`永久生效：
   echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
   source ~/.bashrc
   

4. 验证工作空间

   echo $ROS_PACKAGE_PATH
   # 应该输出包含/home/你的用户名/catkin_ws/src
   

1.2 创建包命令 (catkin_create_pkg)

1. 命令语法

   catkin_create_pkg <包名> [依赖1] [依赖2] ...
   

2. 创建第一个包

   # 进入工作空间的src目录
   cd ~/catkin_ws/src
   
   # 创建名为my_first_package的包，依赖roscpp和rospy
   catkin_create_pkg my_first_package roscpp rospy
   

   • 输出示例

     Created file my_first_package/package.xml
     Created file my_first_package/CMakeLists.txt
     Created folder my_first_package/include/my_first_package
     Created folder my_first_package/src
     Successfully created files in /home/user/catkin_ws/src/my_first_package.
     

     关键目录说明

     • include/包名：存放C++头文件
     • src：存放源代码（.cpp或.py文件）
     • CMakeLists.txt：构建配置文件
     • package.xml：包信息清单
       
       

1.3 关键文件：CMakeLists.txt + package.xml

1. package.xml - 包信息清单

   文件位置： ~/catkin_ws/src/my_first_package/package.xml

   基础配置

   <?xml version="1.0"?>
   <package format="2">
     <!-- 包基本信息 -->
     <name>my_first_package</name>
     <version>0.0.0</version>
     <description>我的第一个ROS包</description>
     <maintainer email="you@example.com">Your Name</maintainer>
     <license>MIT</license>
   
     <!-- 构建依赖 -->
     <buildtool_depend>catkin</buildtool_depend>
     <build_depend>roscpp</build_depend>
     <build_depend>rospy</build_depend>
   
     <!-- 运行时依赖 -->
     <exec_depend>roscpp</exec_depend>
     <exec_depend>rospy</exec_depend>
   </package>
   

   重要字段说明

   • <name>：必须与文件夹名一致
   • <build_depend>：编译时需要的依赖
   • <exec_depend>：运行时需要的依赖
   • 添加新依赖时，需同时添加到CMakeLists.txt
     
     

2. CMakeLists.txt - 构建配置文件

   文件位置： ~/catkin_ws/src/my_first_package/CMakeLists.txt

   基础配置

   cmake_minimum_required(VERSION 3.0.2)
   project(my_first_package)
   
   # 查找依赖包
   find_package(catkin REQUIRED COMPONENTS
     roscpp
     rospy
   )
   
   # 声明为catkin包
   catkin_package()
   
   # 包含目录
   include_directories(
     ${catkin_INCLUDE_DIRS}
   )
   
   # 添加Python脚本示例（取消注释）
   # catkin_install_python(PROGRAMS scripts/simple_node.py
   #   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
   # )
   
   # 添加C++节点示例（取消注释）
   # add_executable(simple_node src/simple_node.cpp)
   # target_link_libraries(simple_node ${catkin_LIBRARIES})
   

1.4 添加一个简单Python节点

1. 创建脚本目录

   mkdir -p ~/catkin_ws/src/my_first_package/scripts
   

2. 创建Python节点文件 simple_node.py

   #!/usr/bin/env python3
   import rospy
   
   if __name__ == '__main__':
       rospy.init_node('my_first_node')
       rospy.loginfo("我的第一个ROS节点已启动！")
       
       rate = rospy.Rate(1)  # 1Hz
       while not rospy.is_shutdown():
           rospy.loginfo("节点运行中...")
           rate.sleep()
   

3. 赋予执行权限

   chmod +x ~/catkin_ws/src/my_first_package/scripts/simple_node.py
   

4. 在CMakeLists.txt中添加

   catkin_install_python(PROGRAMS scripts/simple_node.py
     DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
   )
   

5. 构建包

   # 1. 返回工作空间根目录
   cd ~/catkin_ws
   
   # 2. 构建所有包
   catkin_make
   
   # 3. 激活环境
   source devel/setup.bash
   
   

6. 运行节点

   rosrun my_first_package simple_node.py
   

   预期输出：

   [INFO] [1680000000.000000]: 我的第一个ROS节点已启动！
   [INFO] [1680000001.000000]: 节点运行中...
   [INFO] [1680000002.000000]: 节点运行中...
   ...
   

1.5 常见问题解决

1. 找不到包/节点：

   # 确保已激活工作空间
   source devel/setup.bash
   
   # 先再第一个终端运行 roscore ，再打开新的一个终端运行节点
   roscore # 第一个终端
   source ~/catkin_ws/devel/setup.bash # 第二个终端
   rosrun my_first_package simple_node.py # 第二个终端
   

2. 构建错误：

   # 清理构建
   cd ~/catkin_ws
   rm -rf build devel
   catkin_make
   

3. Python节点权限问题：

   # 确保有执行权限
   chmod +x ~/catkin_ws/src/my_first_package/scripts/*.py
   

二、编写节点(Node)：发布者与订阅者

ROS节点间通过话题(Topic)通信。 本节使用Python编写发布速度指令(Publisher)与订阅位置信息(Subscriber)的节点，并通过catkin_make编译、rosrun运行，掌握ROS通信的核心能力。

节点通信的核心概念

在ROS中，节点(Node) 是执行计算的基本单元，而节点间通过话题(Topic) 进行通信。发布者(Publisher)将消息发送到话题，订阅者(Subscriber)从话题接收消息，这种发布-订阅模式是ROS分布式通信的核心。

2.1 Python版：发布速度指令节点

代码实现与解析

#!/usr/bin/env python3
import rospy
from geometry_msgs.msg import Twist  # 导入速度消息类型

def velocity_publisher():
    # 初始化节点，命名为'velocity_publisher'
    rospy.init_node('velocity_publisher', anonymous=True)
    
    # 创建Publisher对象
    # 参数1：话题名称 '/turtle1/cmd_vel'
    # 参数2：消息类型 Twist
    # 参数3：队列大小
    pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
    
    # 设置发布频率 (10Hz)
    rate = rospy.Rate(10) 
    
    # 循环发布消息
    while not rospy.is_shutdown():
        # 创建Twist消息对象
        vel_msg = Twist()
        
        # 设置线速度 (x方向前进)
        vel_msg.linear.x = 0.5  # 0.5 m/s
        
        # 设置角速度 (z轴旋转)
        vel_msg.angular.z = 0.2  # 0.2 rad/s
        
        # 发布消息
        pub.publish(vel_msg)
        
        # 日志输出
        rospy.loginfo("发布速度指令: 线速度=%.2f m/s, 角速度=%.2f rad/s", 
                     vel_msg.linear.x, vel_msg.angular.z)
        
        # 按频率休眠
        rate.sleep()

if __name__ == '__main__':
    try:
        velocity_publisher()
    except rospy.ROSInterruptException:
        pass

可视化解析

2.2 Python版：订阅位置信息节点

代码实现与解析

#!/usr/bin/env python3
import rospy
from turtlesim.msg import Pose  # 导入位置消息类型

def pose_callback(pose):
    # 当收到新消息时自动调用此回调函数
    rospy.loginfo("乌龟位置: x=%.2f, y=%.2f, 朝向=%.2f", 
                 pose.x, pose.y, pose.theta)

def pose_subscriber():
    # 初始化节点，命名为'pose_subscriber'
    rospy.init_node('pose_subscriber', anonymous=True)
    
    # 创建Subscriber对象
    # 参数1：话题名称 '/turtle1/pose'
    # 参数2：消息类型 Pose
    # 参数3：回调函数
    rospy.Subscriber('/turtle1/pose', Pose, pose_callback)
    
    # 保持节点运行
    rospy.spin()

if __name__ == '__main__':
    pose_subscriber()

可视化解析

2.3 编译并运行节点（catkin_make + rosrun）

1. 创建工作空间和包（如果尚未创建）

   mkdir -p ~/catkin_ws/src
   cd ~/catkin_ws/src
   catkin_create_pkg my_robot_control rospy
   

2. 添加Python脚本

   • 在包目录下创建scripts文件夹
   • 将上述两个Python脚本放入scripts目录
   • 设置执行权限：

     chmod +x velocity_publisher.py
     chmod +x pose_subscriber.py
     

3. 编译工作空间

   cd ~/catkin_ws
   catkin_make
   source devel/setup.bash
   

4. 运行节点

   # 终端1: 启动ROS核心
   roscore
   
   # 终端2: 启动小乌龟模拟器
   rosrun turtlesim turtlesim_node
   
   # 终端3: 运行发布者节点
   rosrun my_robot_control velocity_publisher.py
   
   # 终端4: 运行订阅者节点
   rosrun my_robot_control pose_subscriber.py
   

5. 可视化通信关系

   使用rqt_graph工具查看节点和话题的通信关系：

   rosrun rqt_graph rqt_graph
   

6. 运行效果

   • 小乌龟将按照发布的线速度(0.5 m/s)和角速度(0.2 rad/s)做圆周运动
   • 订阅者终端将实时打印小乌龟的位置信息：

     [INFO] [1677727045.123456]: 乌龟位置: x=5.54, y=5.54, 朝向=0.78
     [INFO] [1677727045.223456]: 乌龟位置: x=5.52, y=5.56, 朝向=0.80
     ...
     

   
   
   

三、启动文件(Launch File)：一键启动作业

启动文件是ROS系统部署的核心工具。 通过单命令批量启动节点（小乌龟+控制器），实现参数动态配置(<param>)和话题重映射(<remap>)，完成复杂系统的一键部署。

3.1 Launch文件作用与核心价值

启动文件的本质作用

启动文件(Launch File)是ROS中的系统部署蓝图，它解决了复杂机器人系统中的关键问题：

1. 批量节点管理：同时启动多个相关节点

2. 参数集中配置：统一设置节点参数

3. 命名空间管理：解决节点/话题命名冲突

4. 环境预设：自动配置ROS环境变量

3.2 创建与运行Launch文件全流程（启动小乌龟+控制节点）

步骤1：创建Launch文件

1. 在包中创建 launch 目录

   cd ~/catkin_ws/src/my_robot_control
   mkdir launch
   

2. 创建 Launch 文件

   touch launch/turtle_control.launch
   

3. 编辑文件内容 – 小乌龟系统启动文件示例

   <!-- ~/catkin_ws/src/my_robot_control/launch/turtle_control.launch -->
   <launch>
       <!-- 启动小乌龟模拟器 -->
       <node pkg="turtlesim" type="turtlesim_node" name="turtle_sim" output="screen">
           <param name="background_r" value="50" />
           <param name="background_g" value="100" />
           <param name="background_b" value="150" />
       </node>
       
       <!-- 启动控制节点 -->
       <node pkg="my_robot_control" type="velocity_publisher.py" name="turtle_controller" output="screen">
           <param name="linear_speed" value="0.5" />
           <param name="angular_speed" value="0.2" />
       </node>
       
       <!-- 启动位置监视器 -->
       <node pkg="my_robot_control" type="pose_subscriber.py" name="pose_monitor" />
   </launch>
   

   • 文件结构解析

步骤2：配置包依赖

• 确保package.xml包含必要依赖

  • ~/catkin_ws/src/my_robot_control 路径下 package.xml

  <buildtool_depend>catkin</buildtool_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>turtlesim</exec_depend>
  <exec_depend>geometry_msgs</exec_depend>
  

步骤3：编译工作空间

cd ~/catkin_ws
catkin_make
source devel/setup.bash  # 重要！使系统识别新launch文件

步骤4：运行 Launch 文件

roslaunch my_robot_control turtle_control.launch

实际运行窗口

运行效果可视化

3.3 高级功能：参数传递(<param>) 与重映射 (<remap>)

• 参数传递深度应用

  1. 创建带参数的Launch文件

  <launch>
      <!-- 定义可配置参数 -->
      <arg name="linear_speed" default="0.8" />
      <arg name="angular_speed" default="0.5" />
      <arg name="bg_red" default="50" />
  
      <!-- 启动模拟器 -->
      <node pkg="turtlesim" type="turtlesim_node" name="simulator">
          <param name="background_r" value="$(arg bg_red)" />
          <param name="background_g" value="100" />
          <param name="background_b" value="150" />
      </node>
  
      <!-- 启动控制器 -->
      <node pkg="my_robot_control" type="velocity_publisher.py" name="controller">
          <param name="linear_speed" value="$(arg linear_speed)" />
          <param name="angular_speed" value="$(arg angular_speed)" />
      </node>
  </launch>
  

  2. 运行时传递参数

  # 覆盖默认参数
  roslaunch my_robot_control turtle_control.launch linear_speed:=1.2 angular_speed:=0.8 bg_red:=100
  

  3. 参数层级关系

• 重映射高级应用

  1. 多机器人系统重映射

  <launch>
      <!-- 第一只乌龟 -->
      <group ns="turtle1">
          <node pkg="turtlesim" type="turtlesim_node" name="sim">
              <remap from="turtle1/pose" to="pose" />
              <remap from="turtle1/cmd_vel" to="cmd_vel" />
          </node>
          <node pkg="my_robot_control" type="controller.py" name="ctrl">
              <remap from="turtle1/cmd_vel" to="cmd_vel" />
          </node>
      </group>
   
      <!-- 第二只乌龟 -->
      <group ns="turtle2">
          <node pkg="turtlesim" type="turtlesim_node" name="sim">
              <remap from="turtle1/pose" to="pose" />
              <remap from="turtle1/cmd_vel" to="cmd_vel" />
          </node>
          <node pkg="my_robot_control" type="controller.py" name="ctrl">
              <remap from="turtle1/cmd_vel" to="cmd_vel" />
          </node>
      </group>
  </launch>
  

2. 重映射前后对比

• 启动文件执行流程

四、可视化神器：RViz与rqt

下面我将提供一个完整的ROS包实现，包含所有代码文件和详细路径说明，实现RGB-D相机、IMU数据可视化及曲线绘制。我们将创建一个名为visualization_demo的ROS包。

文件结构

~/catkin_ws/src/
└── visualization_demo/
    ├── launch/
    │   ├── demo.launch
    │   ├── sensors.launch
    │   └── rviz_config.rviz
    ├── scripts/
    │   ├── sensor_simulator.py
    │   └── robot_controller.py
    ├── urdf/
    │   └── my_robot.urdf
    ├── config/
    │   └── robot_params.yaml
    └── CMakeLists.txt
    └── package.xml

1. 创建ROS包

cd ~/catkin_ws/src
catkin_create_pkg visualization_demo rospy sensor_msgs geometry_msgs tf visualization_msgs
cd visualization_demo
mkdir launch scripts urdf config

2. 机器人URDF模型 (urdf/my_robot.urdf)

<?xml version="1.0"?>
<robot name="visualization_robot">

  <!-- Base Link -->
  <link name="base_link">
    <visual>
      <geometry>
        <box size="0.3 0.3 0.1"/>
      </geometry>
      <material name="blue">
        <color rgba="0 0 0.8 1"/>
      </material>
    </visual>
  </link>

  <!-- RGB-D Camera -->
  <link name="camera_link"/>
  <joint name="camera_joint" type="fixed">
    <parent link="base_link"/>
    <child link="camera_link"/>
    <origin xyz="0.2 0 0.15" rpy="0 0.3 0"/>
  </joint>

  <!-- IMU -->
  <link name="imu_link"/>
  <joint name="imu_joint" type="fixed">
    <parent link="base_link"/>
    <child link="imu_link"/>
    <origin xyz="0 0 0.1" rpy="0 0 0"/>
  </joint>

  <!-- Wheels -->
  <link name="wheel_left">
    <visual>
      <geometry>
        <cylinder length="0.05" radius="0.08"/>
      </geometry>
    </visual>
  </link>
  <joint name="wheel_left_joint" type="continuous">
    <parent link="base_link"/>
    <child link="wheel_left"/>
    <origin xyz="0 0.15 -0.05" rpy="1.57 0 0"/>
  </joint>

  <link name="wheel_right">
    <visual>
      <geometry>
        <cylinder length="0.05" radius="0.08"/>
      </geometry>
    </visual>
  </link>
  <joint name="wheel_right_joint" type="continuous">
    <parent link="base_link"/>
    <child link="wheel_right"/>
    <origin xyz="0 -0.15 -0.05" rpy="1.57 0 0"/>
  </joint>

  <!-- Gazebo Plugins -->
  <gazebo>
    <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
      <commandTopic>cmd_vel</commandTopic>
      <odometryTopic>odom</odometryTopic>
      <odometryFrame>odom</odometryFrame>
      <robotBaseFrame>base_link</robotBaseFrame>
      <publishWheelTF>true</publishWheelTF>
      <wheelSeparation>0.3</wheelSeparation>
      <wheelDiameter>0.16</wheelDiameter>
    </plugin>
  </gazebo>

</robot>

3. 传感器模拟节点 (scripts/sensor_simulator.py)

#!/usr/bin/env python3

import rospy
import math
import numpy as np
from sensor_msgs.msg import PointCloud2, PointField, Imu, Image
from geometry_msgs.msg import Twist, PoseStamped
from nav_msgs.msg import Odometry
from std_msgs.msg import Header
import tf2_ros
from tf.transformations import quaternion_from_euler

class SensorSimulator:
    def __init__(self):
        rospy.init_node('sensor_simulator')
        
        # 创建发布器
        self.pointcloud_pub = rospy.Publisher('/camera/depth/points', PointCloud2, queue_size=10)
        self.imu_pub = rospy.Publisher('/imu/data', Imu, queue_size=10)
        self.image_pub = rospy.Publisher('/camera/rgb/image_raw', Image, queue_size=10)
        self.odom_pub = rospy.Publisher('/odom', Odometry, queue_size=10)
        
        # 订阅速度指令
        rospy.Subscriber('/cmd_vel', Twist, self.cmd_vel_callback)
        
        # TF广播器
        self.tf_broadcaster = tf2_ros.TransformBroadcaster()
        
        # 机器人状态
        self.x = 0.0
        self.y = 0.0
        self.th = 0.0
        self.vx = 0.0
        self.vth = 0.0
        self.last_time = rospy.Time.now()
        
        # 模拟参数
        self.rate = rospy.Rate(20)  # 20Hz
        
    def cmd_vel_callback(self, msg):
        self.vx = msg.linear.x
        self.vth = msg.angular.z

    def generate_pointcloud(self):
        """生成模拟点云数据（立方体环境）"""
        header = Header()
        header.stamp = rospy.Time.now()
        header.frame_id = "camera_link"
        
        # 创建点云数据（立方体）
        points = []
        for x in np.arange(-1.0, 1.0, 0.05):
            for y in np.arange(-1.0, 1.0, 0.05):
                for z in [0.5, 1.5]:  # 两个平面
                    points.append([x, y, z])
        
        # 转换为PointCloud2格式
        fields = [
            PointField('x', 0, PointField.FLOAT32, 1),
            PointField('y', 4, PointField.FLOAT32, 1),
            PointField('z', 8, PointField.FLOAT32, 1)
        ]
        
        cloud = PointCloud2()
        cloud.header = header
        cloud.height = 1
        cloud.width = len(points)
        cloud.fields = fields
        cloud.is_bigendian = False
        cloud.point_step = 12  # 12 bytes per point (3 floats)
        cloud.row_step = cloud.point_step * cloud.width
        cloud.is_dense = True
        cloud.data = np.array(points, dtype=np.float32).tobytes()
        
        return cloud

    def generate_imu_data(self):
        """生成模拟IMU数据（含噪声）"""
        imu = Imu()
        imu.header.stamp = rospy.Time.now()
        imu.header.frame_id = "imu_link"
        
        # 添加俯仰角模拟机器人倾斜
        pitch = 0.1 * math.sin(rospy.get_time() * 0.5)
        
        # 方向四元数
        q = quaternion_from_euler(0, pitch, self.th)
        imu.orientation.x = q[0] + np.random.normal(0, 0.01)
        imu.orientation.y = q[1] + np.random.normal(0, 0.01)
        imu.orientation.z = q[2] + np.random.normal(0, 0.01)
        imu.orientation.w = q[3] + np.random.normal(0, 0.01)
        
        # 角速度
        imu.angular_velocity.x = np.random.normal(0, 0.01)
        imu.angular_velocity.y = np.random.normal(0, 0.01)
        imu.angular_velocity.z = self.vth + np.random.normal(0, 0.02)
        
        # 线加速度
        imu.linear_acceleration.x = self.vx * 0.1 + np.random.normal(0, 0.05)
        imu.linear_acceleration.y = np.random.normal(0, 0.05)
        imu.linear_acceleration.z = 9.8 + np.random.normal(0, 0.1)
        
        return imu

    def generate_image(self):
        """生成模拟RGB图像（梯度图）"""
        img = Image()
        img.header.stamp = rospy.Time.now()
        img.header.frame_id = "camera_link"
        img.height = 480
        img.width = 640
        img.encoding = "rgb8"
        img.is_bigendian = 0
        img.step = 640 * 3  # width * channels
        
        # 创建梯度图像
        data = bytearray()
        for y in range(480):
            for x in range(640):
                r = int(255 * x / 640)
                g = int(255 * y / 480)
                b = 128 + int(64 * math.sin(rospy.get_time()))
                data.extend([r, g, b])
        
        img.data = bytes(data)
        return img

    def update_odometry(self):
        """更新机器人里程计"""
        current_time = rospy.Time.now()
        dt = (current_time - self.last_time).to_sec()
        self.last_time = current_time
        
        # 更新位置
        delta_x = self.vx * math.cos(self.th) * dt
        delta_y = self.vx * math.sin(self.th) * dt
        delta_th = self.vth * dt
        
        self.x += delta_x
        self.y += delta_y
        self.th += delta_th
        
        # 发布TF变换
        transform = tf2_ros.TransformStamped()
        transform.header.stamp = current_time
        transform.header.frame_id = "odom"
        transform.child_frame_id = "base_link"
        transform.transform.translation.x = self.x
        transform.transform.translation.y = self.y
        transform.transform.translation.z = 0.0
        
        q = quaternion_from_euler(0, 0, self.th)
        transform.transform.rotation.x = q[0]
        transform.transform.rotation.y = q[1]
        transform.transform.rotation.z = q[2]
        transform.transform.rotation.w = q[3]
        
        self.tf_broadcaster.sendTransform(transform)
        
        # 发布里程计
        odom = Odometry()
        odom.header.stamp = current_time
        odom.header.frame_id = "odom"
        odom.child_frame_id = "base_link"
        odom.pose.pose.position.x = self.x
        odom.pose.pose.position.y = self.y
        odom.pose.pose.orientation = transform.transform.rotation
        odom.twist.twist.linear.x = self.vx
        odom.twist.twist.angular.z = self.vth
        
        self.odom_pub.publish(odom)

    def run(self):
        while not rospy.is_shutdown():
            # 发布传感器数据
            self.pointcloud_pub.publish(self.generate_pointcloud())
            self.imu_pub.publish(self.generate_imu_data())
            self.image_pub.publish(self.generate_image())
            
            # 更新并发布里程计
            self.update_odometry()
            
            self.rate.sleep()

if __name__ == '__main__':
    try:
        simulator = SensorSimulator()
        simulator.run()
    except rospy.ROSInterruptException:
        pass

4. 机器人控制器 (scripts/robot_controller.py)

#!/usr/bin/env python3

import rospy
from geometry_msgs.msg import Twist
import math

class RobotController:
    def __init__(self):
        rospy.init_node('robot_controller')
        self.cmd_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        self.rate = rospy.Rate(10)  # 10Hz
        self.duration = 30  # 演示时长(秒)

    def run_demo(self):
        start_time = rospy.get_time()
        
        while not rospy.is_shutdown() and (rospy.get_time() - start_time) < self.duration:
            elapsed = rospy.get_time() - start_time
            
            # 分段控制
            if elapsed < 10:
                # 第一阶段：直线运动
                self.move_robot(0.5, 0.0)
            elif elapsed < 20:
                # 第二阶段：旋转
                self.move_robot(0.0, 0.5)
            else:
                # 第三阶段：正弦运动
                speed = 0.5 * math.sin(elapsed - 20)
                self.move_robot(speed, speed)
            
            self.rate.sleep()
        
        # 停止机器人
        self.move_robot(0, 0)
        rospy.loginfo("演示结束")

    def move_robot(self, linear, angular):
        twist = Twist()
        twist.linear.x = linear
        twist.angular.z = angular
        self.cmd_pub.publish(twist)

if __name__ == '__main__':
    try:
        controller = RobotController()
        controller.run_demo()
    except rospy.ROSInterruptException:
        pass

5. 启动文件 (launch/sensors.launch)

<launch>
    <!-- 启动传感器模拟节点 -->
    <node name="sensor_simulator" pkg="visualization_demo" type="sensor_simulator.py" output="screen"/>
    
    <!-- 启动机器人控制器 -->
    <node name="robot_controller" pkg="visualization_demo" type="robot_controller.py" output="screen"/>
    
    <!-- 启动静态TF发布 -->
    <node pkg="tf" type="static_transform_publisher" name="map_to_odom" 
        args="0 0 0 0 0 0 map odom 100"/>
    
    <!-- 加载机器人模型 -->
    <param name="robot_description" textfile="$(find visualization_demo)/urdf/my_robot.urdf"/>
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
</launch>

6. 主启动文件 (launch/demo.launch)

<launch>
    <!-- 启动传感器和控制器 -->
    <include file="$(find visualization_demo)/launch/sensors.launch"/>
    
    <!-- 启动RViz -->
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find visualization_demo)/launch/rviz_config.rviz"/>
    
    <!-- 启动rqt_graph -->
    <node name="rqt_graph" pkg="rqt_graph" type="rqt_graph" output="screen"/>
    
    <!-- 启动rqt_plot -->
    <node name="rqt_plot" pkg="rqt_plot" type="rqt_plot" 
        args="/cmd_vel/linear/x /cmd_vel/angular/z /imu/data/orientation/y /odom/pose/pose/position/x"/>
</launch>

7. RViz配置文件 (launch/rviz_config.rviz)

Panels:
  - Class: rviz/Displays
    Help Height: 78
    Name: Displays
    Property Tree Widget:
      Expanded:
        - /Global Options1
        - /Status1
      Splitter Ratio: 0.5
    Tree Height: 523
  - Class: rviz/Selection
    Name: Selection
  - Class: rviz/Tool Properties
    Expanded:
      - /2D Pose Estimate1
      - /2D Nav Goal1
      - /Publish Point1
    Name: Tool Properties
    Splitter Ratio: 0.5886790156364441
  - Class: rviz/Views
    Expanded:
      - /Current View1
    Name: Views
    Splitter Ratio: 0.5
  - Class: rviz/Time
    Name: Time
    SyncMode: 0
    SyncSource: ""
Visualization Manager:
  Class: ""
  Displays:
    - Alpha: 0.5
      Cell Size: 1
      Class: rviz/Grid
      Color: 160; 160; 164
      Enabled: true
      Line Style:
        Line Width: 0.03
        Value: Lines
      Name: Grid
      Normal Cell Count: 0
      Offset:
        X: 0
        Y: 0
        Z: 0
      Plane: XY
      Plane Cell Count: 10
      Reference Frame: <Fixed Frame>
      Value: true
    - Class: rviz/RobotModel
      Enabled: true
      Name: Robot Model
      TF Prefix: ""
      Update Interval: 0
      Visual Enabled: true
    - Class: rviz/TF
      Enabled: true
      Frame Timeout: 15
      Frames:
        All Enabled: true
      Marker Scale: 1
      Name: TF
      Show Arrows: true
      Show Axes: true
      Show Names: true
      Tree:
        base_link: {parent: '', view: true}
        camera_link: {parent: base_link, view: true}
        imu_link: {parent: base_link, view: true}
        odom: {parent: map, view: true}
        wheel_left: {parent: base_link, view: true}
        wheel_right: {parent: base_link, view: true}
      Update Interval: 0
      Value: true
    - Class: rviz/PointCloud2
      Enabled: true
      Name: RGB-D PointCloud
      Queue Size: 10
      Selectable: true
      Size (Pixels): 3
      Style: Flat Squares
      Topic: /camera/depth/points
      Unreliable: false
      Value: true
    - Class: rviz/Image
      Enabled: true
      Image Topic: /camera/rgb/image_raw
      Max Value: 1
      Median window: 5
      Min Value: 0
      Name: RGB Camera
      Normalize Range: false
      Queue Size: 2
      Transport Hint: raw
      Unreliable: false
      Value: true
    - Class: rviz/Imu
      Enabled: true
      Name: IMU Orientation
      Scale: 0.3
      Show Arrow: true
      Show Covariance: false
      Show History: false
      Topic: /imu/data
      Value: true
    - Class: rviz/Path
      Alpha: 1
      Buffer Length: 100
      Color: 255; 0; 0
      Enabled: true
      Name: Robot Path
      Offset:
        X: 0
        Y: 0
        Z: 0
      Pose Style: Lines
      Queue Size: 10
      Topic: /odom
      Value: true
      Width: 0.05
  Enabled: true
  Global Options:
    Background Color: 48; 48; 48
    Default Light: true
    Fixed Frame: odom
    Frame Rate: 30
  Name: root
  Tools:
    - Class: rviz/Interact
      Hide Inactive Objects: false
    - Class: rviz/MoveCamera
    - Class: rviz/Select
    - Class: rviz/FocusCamera
    - Class: rviz/Measure
      Line color: 128; 128; 0
    - Class: rviz/SetInitialPose
      Topic: /initialpose
    - Class: rviz/SetGoal
      Topic: /move_base_simple/goal
    - Class: rviz/PublishPoint
      Single click: true
      Topic: /clicked_point
  Value: true
  Views:
    Current:
      Class: rviz/Orbit
      Distance: 4
      Enable Stereo Rendering:
        Stereo Eye Separation: 0.06
        Stereo Focal Distance: 1
        Swap Stereo Eyes: false
        Value: false
      Field of View: 0.785398
      Focal Point:
        X: 0
        Y: 0
        Z: 0.3
      Focal Shape Fixed Size: true
      Focal Shape Size: 0.05
      Invert Z Axis: false
      Name: Current View
      Near Clip Distance: 0.01
      Pitch: 0.5
      Target Frame: base_link
      Value: Orbit (rviz)
      Yaw: 0
    Saved: ~

8. 安装与运行步骤

• 设置文件权限

  cd ~/catkin_ws/src/visualization_demo/scripts
  chmod +x sensor_simulator.py
  chmod +x robot_controller.py
  

• 编译包

  cd ~/catkin_ws
  catkin_make
  source devel/setup.bash
  

• 运行完整演示

  roslaunch visualization_demo demo.launch
  

9. 可视化效果说明

1. RViz (4.1):

   • 中央：机器人模型和环境点云（立方体结构）
   • 左下角：RGB相机实时图像（彩色梯度）
   • 右下角：IMU方向指示器（显示机器人俯仰角变化）
   • 轨迹：红色线条显示机器人运动路径

2. rqt_graph (4.2):

   • 显示节点间通信关系：

     /robot_controller → /cmd_vel
     /sensor_simulator → /camera/depth/points, /imu/data, /odom, TF
     /rviz 订阅所有可视化话题
     

3. rqt_plot (4.3):

   • 4条数据曲线：
     • 蓝色：线速度 (/cmd_vel/linear/x)
     • 绿色：角速度 (/cmd_vel/angular/z)
     • 红色：俯仰角 (/imu/data/orientation/y)
     • 黄色：X轴位置 (/odom/pose/pose/position/x)