第3章 ROS可视化工具与系统调试技巧

3.1 日志系统深度解析与rqt_console实战

ROS日志系统是机器人应用调试的基础设施，提供了多级别的消息记录功能。理解ROS日志机制对于有效调试至关重要。

ROS日志级别：

• DEBUG：详细调试信息，通常在生产环境中关闭
• INFO：常规操作信息，表明系统正常运行
• WARN：警告信息，表示可能的问题但不会阻止系统运行
• ERROR：错误信息，表示功能无法正常工作
• FATAL：严重错误，通常会导致节点关闭

C++日志使用实例：

// 文件名：advanced_logging_demo.cpp
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <log4cxx/logger.h>

class AdvancedLoggingDemo {
private:
    ros::NodeHandle nh;
    ros::Publisher log_test_pub;
    int counter;

public:
    AdvancedLoggingDemo() : counter(0) {
        log_test_pub = nh.advertise<std_msgs::String>("log_test", 10);
        
        // 设置日志级别（需要在launch文件中配置）
        if(ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, 
           ros::console::levels::Debug)) {
            ros::console::notifyLoggerLevelsChanged();
        }
    }

    void run() {
        ros::Rate rate(2); // 2Hz
        
        while(ros::ok()) {
            // 不同级别的日志示例
            ROS_DEBUG_STREAM("Debug counter: " << counter);
            
            if(counter % 5 == 0) {
                ROS_INFO("Counter reached milestone: %d", counter);
            }
            
            if(counter % 13 == 0) {
                ROS_WARN("Counter is divisible by 13: %d", counter);
            }
            
            if(counter % 17 == 0) {
                ROS_ERROR("Counter is divisible by 17: %d", counter);
            }
            
            // 条件日志
            ROS_INFO_COND(counter > 50, "Counter has exceeded 50");
            
            // 限流日志，避免日志过多
            ROS_INFO_STREAM_THROTTLE(10, "Throttled message, counter: " << counter);
            
            // 一次性日志，只打印一次
            ROS_INFO_ONCE("This message will appear only once");
            
            publishTestMessage();
            counter++;
            
            ros::spinOnce();
            rate.sleep();
        }
    }

private:
    void publishTestMessage() {
        std_msgs::String msg;
        msg.data = "Log test message " + std::to_string(counter);
        log_test_pub.publish(msg);
    }
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "advanced_logging_demo");
    
    AdvancedLoggingDemo demo;
    demo.run();
    
    return 0;
}

Python日志配置实例：

#!/usr/bin/env python
# 文件名：python_logging_demo.py
import rospy
import logging
from std_msgs.msg import String

class PythonLoggingDemo:
    def __init__(self):
        rospy.init_node('python_logging_demo')
        
        # 配置Python标准日志
        self.setup_python_logging()
        
        self.pub = rospy.Publisher('python_log_test', String, queue_size=10)
        self.counter = 0
        
    def setup_python_logging(self):
        """配置Python标准日志与ROS日志集成"""
        # 创建文件处理器
        file_handler = logging.FileHandler('/tmp/ros_python_log.txt')
        file_handler.setLevel(logging.DEBUG)
        
        # 创建格式器
        formatter = logging.Formatter(
            '%(asctime)s - %(name)s - %(levelname)s - %(message)s'
        )
        file_handler.setFormatter(formatter)
        
        # 获取ROSPY的logger并添加处理器
        ros_logger = logging.getLogger('rosout')
        ros_logger.addHandler(file_handler)
        
    def run(self):
        rate = rospy.Rate(1)  # 1Hz
        
        while not rospy.is_shutdown():
            # 使用rospy日志
            rospy.logdebug("Python debug message: %d", self.counter)
            rospy.loginfo("Python info message: %d", self.counter)
            
            if self.counter % 7 == 0:
                rospy.logwarn("Python warning: counter divisible by 7")
                
            if self.counter % 11 == 0:
                rospy.logerr("Python error: counter divisible by 11")
            
            # 发布测试消息
            msg = String()
            msg.data = f"Python log test {self.counter}"
            self.pub.publish(msg)
            
            self.counter += 1
            rate.sleep()

if __name__ == '__main__':
    try:
        demo = PythonLoggingDemo()
        demo.run()
    except rospy.ROSInterruptException:
        pass

rqt_console高级使用配置：

#!/usr/bin/env python
# 文件名：rqt_console_plugin_demo.py
import rospy
import threading
import time
from rqt_console.console import Console
from python_qt_binding import QtCore, QtGui

class CustomConsolePlugin:
    """
    自定义rqt_console插件示例
    """
    def __init__(self, context):
        super(CustomConsolePlugin, self).__init__(context)
        
        # 设置对象名称
        self.setObjectName('CustomConsolePlugin')
        
        # 创建控制台部件
        self._widget = Console()
        
        # 自定义过滤器
        self.setup_custom_filters()
        
        # 启动日志生成线程
        self.log_thread = threading.Thread(target=self.generate_log_messages)
        self.log_thread.daemon = True
        self.log_thread.start()
    
    def setup_custom_filters(self):
        """设置自定义日志过滤器"""
        # 添加严重错误过滤器
        self._widget.add_filter('severity >= 4')  # ERROR和FATAL
        
        # 添加特定节点过滤器
        self._widget.add_filter('message: ~ "counter"')
        
        # 添加时间范围过滤器
        self._widget.add_filter('time >= now-60')  # 最近60秒
    
    def generate_log_messages(self):
        """生成测试日志消息"""
        node_name = "test_log_generator"
        rospy.init_node(node_name, anonymous=True)
        
        severity_levels = [rospy.DEBUG, rospy.INFO, rospy.WARN, rospy.ERROR]
        messages = [
            "System initialized successfully",
            "Sensor data received",
            "Low battery warning",
            "Motor controller timeout",
            "Navigation system error"
        ]
        
        count = 0
        while not rospy.is_shutdown():
            severity = severity_levels[count % len(severity_levels)]
            message = f"{messages[count % len(messages)]} - Count: {count}"
            
            rospy.loglog(severity, message)
            
            count += 1
            time.sleep(2)

3.2 数据可视化与rqt_plot高级应用

rqt_plot是ROS中用于实时绘制数据趋势的强大工具，支持多种消息类型和数据处理功能。

rqt_plot支持的数据类型：

• 基本数值类型：int32, float64等
• 数组类型：std_msgs/Float64MultiArray
• 自定义消息类型中的数值字段

C++数据发布示例：

// 文件名：sensor_data_publisher.cpp
#include <ros/ros.h>
#include <std_msgs/Float64.h>
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/Vector3.h>
#include <math.h>

class SensorDataPublisher {
private:
    ros::NodeHandle nh;
    ros::Publisher temp_pub;
    ros::Publisher voltage_pub;
    ros::Publisher imu_pub;
    ros::Publisher sine_wave_pub;
    
    double time_counter;

public:
    SensorDataPublisher() : time_counter(0) {
        temp_pub = nh.advertise<std_msgs::Float64>("sensor/temperature", 10);
        voltage_pub = nh.advertise<std_msgs::Float64>("sensor/voltage", 10);
        imu_pub = nh.advertise<sensor_msgs::Imu>("sensor/imu", 10);
        sine_wave_pub = nh.advertise<std_msgs::Float64>("signal/sine_wave", 10);
    }

    void publishTemperature() {
        std_msgs::Float64 temp_msg;
        // 模拟温度数据：25°C ± 2°C的随机波动
        temp_msg.data = 25.0 + 2.0 * sin(time_counter * 0.1) + 0.5 * (rand() % 100 - 50) / 50.0;
        temp_pub.publish(temp_msg);
    }

    void publishVoltage() {
        std_msgs::Float64 voltage_msg;
        // 模拟电压数据：12V ± 1V的衰减
        voltage_msg.data = 12.0 - 0.01 * time_counter + sin(time_counter * 0.5);
        voltage_pub.publish(voltage_msg);
    }

    void publishIMUData() {
        sensor_msgs::Imu imu_msg;
        imu_msg.header.stamp = ros::Time::now();
        imu_msg.header.frame_id = "imu_link";
        
        // 模拟角速度数据
        imu_msg.angular_velocity.x = 0.1 * sin(time_counter * 0.5);
        imu_msg.angular_velocity.y = 0.2 * cos(time_counter * 0.3);
        imu_msg.angular_velocity.z = 0.05 * sin(time_counter * 0.7);
        
        // 模拟线性加速度
        imu_msg.linear_acceleration.x = 9.8 + 0.5 * sin(time_counter);
        imu_msg.linear_acceleration.y = 0.3 * cos(time_counter * 0.8);
        imu_msg.linear_acceleration.z = 0.2 * sin(time_counter * 1.2);
        
        imu_pub.publish(imu_msg);
    }

    void publishSineWave() {
        std_msgs::Float64 sine_msg;
        // 生成多频率正弦波
        sine_msg.data = sin(time_counter) + 0.5 * sin(2 * time_counter) + 0.2 * sin(5 * time_counter);
        sine_wave_pub.publish(sine_msg);
    }

    void run() {
        ros::Rate rate(10); // 10Hz
        
        while(ros::ok()) {
            publishTemperature();
            publishVoltage();
            publishIMUData();
            publishSineWave();
            
            time_counter += 0.1; // 每次增加0.1秒
            ros::spinOnce();
            rate.sleep();
        }
    }
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "sensor_data_publisher");
    
    SensorDataPublisher publisher;
    publisher.run();
    
    return 0;
}

Python多维度数据可视化：

#!/usr/bin/env python
# 文件名：multi_dimensional_plotter.py
import rospy
import math
import numpy as np
from std_msgs.msg import Float64, Float64MultiArray
from geometry_msgs.msg import Point, Vector3
from sensor_msgs.msg import LaserScan

class MultiDimensionalPlotter:
    def __init__(self):
        rospy.init_node('multi_dimensional_plotter')
        
        # 创建多个发布者用于不同数据类型
        self.scalar_pub = rospy.Publisher('plot/scalar', Float64, queue_size=10)
        self.array_pub = rospy.Publisher('plot/array', Float64MultiArray, queue_size=10)
        self.vector_pub = rospy.Publisher('plot/vector', Vector3, queue_size=10)
        self.laser_pub = rospy.Publisher('plot/laser', LaserScan, queue_size=10)
        
        self.time_counter = 0
        
    def publish_scalar_data(self):
        """发布标量数据"""
        msg = Float64()
        # 复合信号：基础正弦波 + 噪声
        base_signal = math.sin(self.time_counter)
        harmonic = 0.3 * math.sin(3 * self.time_counter)
        noise = 0.1 * math.sin(20 * self.time_counter)
        msg.data = base_signal + harmonic + noise
        self.scalar_pub.publish(msg)
        
    def publish_array_data(self):
        """发布数组数据"""
        msg = Float64MultiArray()
        # 生成包含多个信号的数组
        num_signals = 5
        signals = []
        for i in range(num_signals):
            signal = math.sin(self.time_counter + i * 0.5) * (i + 1) * 0.2
            signals.append(signal)
        msg.data = signals
        self.array_pub.publish(msg)
        
    def publish_vector_data(self):
        """发布向量数据"""
        msg = Vector3()
        msg.x = math.sin(self.time_counter)
        msg.y = math.cos(self.time_counter)
        msg.z = math.sin(self.time_counter) * math.cos(self.time_counter)
        self.vector_pub.publish(msg)
        
    def publish_laser_data(self):
        """发布模拟激光雷达数据"""
        msg = LaserScan()
        msg.header.stamp = rospy.Time.now()
        msg.header.frame_id = "laser_frame"
        
        msg.angle_min = -math.pi / 2
        msg.angle_max = math.pi / 2
        msg.angle_increment = math.pi / 180  # 1度分辨率
        msg.range_min = 0.1
        msg.range_max = 10.0
        msg.scan_time = 0.1
        msg.time_increment = 0.1 / 360
        
        # 生成模拟激光数据
        num_readings = int((msg.angle_max - msg.angle_min) / msg.angle_increment)
        msg.ranges = []
        
        for i in range(num_readings):
            angle = msg.angle_min + i * msg.angle_increment
            # 模拟墙壁在5米处，加上噪声
            base_range = 5.0 / abs(math.cos(angle)) if abs(math.cos(angle)) > 0.1 else 10.0
            noise = 0.1 * math.sin(self.time_counter * 2 + i * 0.1)
            range_val = max(msg.range_min, min(msg.range_max, base_range + noise))
            msg.ranges.append(range_val)
            
        self.laser_pub.publish(msg)
        
    def run(self):
        rate = rospy.Rate(10)  # 10Hz
        
        while not rospy.is_shutdown():
            self.publish_scalar_data()
            self.publish_array_data()
            self.publish_vector_data()
            self.publish_laser_data()
            
            self.time_counter += 0.1
            rate.sleep()

if __name__ == '__main__':
    try:
        plotter = MultiDimensionalPlotter()
        plotter.run()
    except rospy.ROSInterruptException:
        pass

rqt_plot配置脚本：

#!/bin/bash
# 文件名：advanced_rqt_plot_setup.sh

echo "启动高级rqt_plot配置..."

# 启动多个rqt_plot实例用于不同数据可视化
echo "1. 启动标量数据绘图..."
rqt_plot /plot/scalar/data &
sleep 2

echo "2. 启动数组数据绘图..."
rqt_plot /plot/array/data[0] /plot/array/data[1] /plot/array/data[2] /plot/array/data[3] /plot/array/data[4] &
sleep 2

echo "3. 启动向量数据绘图..."
rqt_plot /plot/vector/x /plot/vector/y /plot/vector/z &
sleep 2

echo "4. 启动激光数据特定角度绘图..."
rqt_plot /plot/laser/ranges[0] /plot/laser/ranges[45] /plot/laser/ranges[90] /plot/laser/ranges[135] /plot/laser/ranges[179] &

echo "所有绘图窗口已启动"
echo "使用命令查看主题列表: rostopic list"
echo "使用命令查看数据: rostopic echo /plot/scalar"

3.3 系统拓扑分析与rqt_graph深度应用

rqt_graph能够可视化ROS节点之间的通信关系，是理解系统架构和调试通信问题的重要工具。

C++复杂通信模式示例：

// 文件名：complex_communication.cpp
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Int32.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Image.h>

class CommunicationNode {
private:
    ros::NodeHandle nh;
    
    // 多个发布者
    ros::Publisher cmd_pub;
    ros::Publisher status_pub;
    ros::Publisher processed_image_pub;
    
    // 多个订阅者
    ros::Subscriber sensor_sub;
    ros::Subscriber control_sub;
    ros::Subscriber config_sub;
    
    std::string node_name;

public:
    CommunicationNode(const std::string& name) : node_name(name) {
        // 初始化发布者
        cmd_pub = nh.advertise<geometry_msgs::Twist>(node_name + "/cmd_vel", 10);
        status_pub = nh.advertise<std_msgs::String>(node_name + "/status", 10);
        processed_image_pub = nh.advertise<sensor_msgs::Image>(node_name + "/processed_image", 10);
        
        // 初始化订阅者
        sensor_sub = nh.subscribe("/sensor/data", 10, &CommunicationNode::sensorCallback, this);
        control_sub = nh.subscribe("/control/commands", 10, &CommunicationNode::controlCallback, this);
        config_sub = nh.subscribe("/config/updates", 10, &CommunicationNode::configCallback, this);
        
        ROS_INFO("节点 %s 初始化完成", node_name.c_str());
    }

    void sensorCallback(const std_msgs::String::ConstPtr& msg) {
        ROS_DEBUG("节点 %s 收到传感器数据: %s", node_name.c_str(), msg->data.c_str());
        
        // 处理传感器数据并发布控制命令
        geometry_msgs::Twist cmd_msg;
        cmd_msg.linear.x = 0.5;
        cmd_msg.angular.z = 0.1;
        cmd_pub.publish(cmd_msg);
        
        // 发布状态
        std_msgs::String status_msg;
        status_msg.data = node_name + " processing sensor data";
        status_pub.publish(status_msg);
    }

    void controlCallback(const geometry_msgs::Twist::ConstPtr& msg) {
        ROS_DEBUG("节点 %s 收到控制命令: linear.x=%.2f, angular.z=%.2f", 
                 node_name.c_str(), msg->linear.x, msg->angular.z);
        
        // 转发处理后的控制命令
        geometry_msgs::Twist processed_cmd = *msg;
        processed_cmd.linear.x *= 1.1; // 简单处理
        cmd_pub.publish(processed_cmd);
    }

    void configCallback(const std_msgs::String::ConstPtr& msg) {
        ROS_INFO("节点 %s 配置更新: %s", node_name.c_str(), msg->data.c_str());
    }

    void run() {
        ros::Rate rate(1);
        int counter = 0;
        
        while(ros::ok()) {
            // 定期发布状态
            if(counter % 5 == 0) {
                std_msgs::String status_msg;
                status_msg.data = node_name + " heartbeat " + std::to_string(counter);
                status_pub.publish(status_msg);
            }
            
            counter++;
            ros::spinOnce();
            rate.sleep();
        }
    }
};

// 传感器数据生成节点
class SensorSimulator {
private:
    ros::NodeHandle nh;
    ros::Publisher sensor_pub;
    ros::Publisher image_pub;

public:
    SensorSimulator() {
        sensor_pub = nh.advertise<std_msgs::String>("/sensor/data", 10);
        image_pub = nh.advertise<sensor_msgs::Image>("/camera/image_raw", 10);
    }

    void run() {
        ros::Rate rate(2); // 2Hz
        int counter = 0;
        
        while(ros::ok()) {
            // 发布传感器数据
            std_msgs::String sensor_msg;
            sensor_msg.data = "Sensor data package " + std::to_string(counter);
            sensor_pub.publish(sensor_msg);
            
            // 发布模拟图像数据
            sensor_msgs::Image image_msg;
            image_msg.header.stamp = ros::Time::now();
            image_msg.header.frame_id = "camera";
            image_msg.height = 480;
            image_msg.width = 640;
            image_msg.encoding = "rgb8";
            image_msg.step = 640 * 3; // width * 3 (RGB)
            image_msg.data.resize(480 * 640 * 3, 100); // 简单的灰色图像
            image_pub.publish(image_msg);
            
            counter++;
            ros::spinOnce();
            rate.sleep();
        }
    }
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "complex_communication_demo");
    
    // 创建多个通信节点
    CommunicationNode node1("navigation_node");
    CommunicationNode node2("perception_node");
    CommunicationNode node3("control_node");
    
    SensorSimulator sensor_sim;
    
    // 在多线程中运行节点
    ros::MultiThreadedSpinner spinner(4); // 使用4个线程
    
    ROS_INFO("启动复杂通信演示系统");
    ROS_INFO("使用 'rqt_graph' 查看节点通信图");
    
    spinner.spin();
    
    return 0;
}

Python系统拓扑分析工具：

#!/usr/bin/env python
# 文件名：topology_analyzer.py
import rospy
import subprocess
import json
import threading
import time
from xml.etree import ElementTree

class ROSTopologyAnalyzer:
    def __init__(self):
        rospy.init_node('topology_analyzer')
        
        self.system_graph = {
            'nodes': {},
            'topics': {},
            'services': {},
            'connections': []
        }
        
        self.analysis_thread = threading.Thread(target=self.continuous_analysis)
        self.analysis_thread.daemon = True
        
    def get_system_state(self):
        """获取ROS系统状态"""
        try:
            # 获取节点列表
            nodes_result = subprocess.check_output(['rosnode', 'list'], 
                                                 stderr=subprocess.STDOUT, 
                                                 text=True)
            nodes = [node.strip() for node in nodes_result.split('\n') if node.strip()]
            
            # 获取主题列表
            topics_result = subprocess.check_output(['rostopic', 'list'], 
                                                  stderr=subprocess.STDOUT, 
                                                  text=True)
            topics = [topic.strip() for topic in topics_result.split('\n') if topic.strip()]
            
            return nodes, topics
        except subprocess.CalledProcessError as e:
            rospy.logerr("获取系统状态失败: %s", e.output)
            return [], []
    
    def analyze_node_connections(self, node_name):
        """分析节点连接关系"""
        try:
            # 获取节点信息
            info_result = subprocess.check_output(['rosnode', 'info', node_name], 
                                                stderr=subprocess.STDOUT, 
                                                text=True)
            
            connections = {
                'publications': [],
                'subscriptions': [],
                'services': []
            }
            
            current_section = None
            for line in info_result.split('\n'):
                line = line.strip()
                if line.startswith('Publications:'):
                    current_section = 'publications'
                elif line.startswith('Subscriptions:'):
                    current_section = 'subscriptions'
                elif line.startswith('Services:'):
                    current_section = 'services'
                elif line and current_section and line.startswith('*'):
                    topic = line[1:].strip()  # 移除星号
                    if topic and not topic.startswith('---'):
                        connections[current_section].append(topic)
            
            return connections
        except subprocess.CalledProcessError:
            return None
    
    def generate_topology_report(self):
        """生成拓扑分析报告"""
        nodes, topics = self.get_system_state()
        
        self.system_graph['nodes'] = {}
        self.system_graph['topics'] = {topic: {'publishers': [], 'subscribers': []} for topic in topics}
        self.system_graph['connections'] = []
        
        # 分析每个节点的连接
        for node in nodes:
            connections = self.analyze_node_connections(node)
            if connections:
                self.system_graph['nodes'][node] = connections
                
                # 构建连接关系
                for pub_topic in connections['publications']:
                    if pub_topic in self.system_graph['topics']:
                        self.system_graph['topics'][pub_topic]['publishers'].append(node)
                        self.system_graph['connections'].append({
                            'from': node,
                            'to': pub_topic,
                            'type': 'publication'
                        })
                
                for sub_topic in connections['subscriptions']:
                    if sub_topic in self.system_graph['topics']:
                        self.system_graph['topics'][sub_topic]['subscribers'].append(node)
                        self.system_graph['connections'].append({
                            'from': sub_topic,
                            'to': node,
                            'type': 'subscription'
                        })
        
        return self.system_graph
    
    def detect_issues(self):
        """检测系统拓扑问题"""
        issues = []
        
        # 检查没有发布者的主题
        for topic, info in self.system_graph['topics'].items():
            if not info['publishers']:
                issues.append(f"主题 '{topic}' 没有发布者")
            
            if not info['subscribers']:
                issues.append(f"主题 '{topic}' 没有订阅者")
        
        # 检查孤立的节点
        for node, connections in self.system_graph['nodes'].items():
            if (not connections['publications'] and 
                not connections['subscriptions'] and 
                not connections['services']):
                issues.append(f"节点 '{node}' 是孤立的，没有连接")
        
        return issues
    
    def generate_dot_graph(self):
        """生成Graphviz DOT格式的图形"""
        dot_lines = [
            'digraph ROS_Topology {',
            '  rankdir=TB;',
            '  node [shape=box, style=filled, fillcolor=lightblue];',
            '  edge [arrowsize=0.8];',
            ''
        ]
        
        # 添加节点
        for node in self.system_graph['nodes']:
            dot_lines.append(f'  "{node}" [shape=box];')
        
        # 添加主题（作为椭圆节点）
        for topic in self.system_graph['topics']:
            dot_lines.append(f'  "{topic}" [shape=ellipse, fillcolor=lightgreen];')
        
        dot_lines.append('')
        
        # 添加连接关系
        for conn in self.system_graph['connections']:
            if conn['type'] == 'publication':
                dot_lines.append(f'  "{conn["from"]}" -> "{conn["to"]}" [color=blue];')
            elif conn['type'] == 'subscription':
                dot_lines.append(f'  "{conn["from"]}" -> "{conn["to"]}" [color=red];')
        
        dot_lines.append('}')
        
        return '\n'.join(dot_lines)
    
    def continuous_analysis(self):
        """持续分析系统拓扑"""
        while not rospy.is_shutdown():
            try:
                self.generate_topology_report()
                
                # 检测问题
                issues = self.detect_issues()
                if issues:
                    rospy.logwarn("检测到系统拓扑问题:")
                    for issue in issues:
                        rospy.logwarn("  - %s", issue)
                
                # 生成图形文件
                dot_content = self.generate_dot_graph()
                with open('/tmp/ros_topology.dot', 'w') as f:
                    f.write(dot_content)
                
                rospy.loginfo("拓扑分析完成，图形文件保存至 /tmp/ros_topology.dot")
                rospy.loginfo("使用命令生成图片: dot -Tpng /tmp/ros_topology.dot -o topology.png")
                
            except Exception as e:
                rospy.logerr("拓扑分析错误: %s", str(e))
            
            time.sleep(10)  # 每10秒分析一次
    
    def start_analysis(self):
        """开始持续分析"""
        self.analysis_thread.start()
        rospy.loginfo("ROS拓扑分析器已启动")

if __name__ == '__main__':
    analyzer = ROSTopologyAnalyzer()
    analyzer.start_analysis()
    rospy.spin()

3.4 图像处理与rqt_image_view实战

rqt_image_view是ROS中用于实时显示图像话题的强大工具，支持多种图像格式和基本的图像操作。

C++图像发布示例：

// 文件名：image_publisher_demo.cpp
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>
#include <vector>

class ImagePublisherDemo {
private:
    ros::NodeHandle nh;
    ros::Publisher image_pub;
    ros::Publisher compressed_pub;
    
    int image_width;
    int image_height;
    int frame_count;

public:
    ImagePublisherDemo() : image_width(640), image_height(480), frame_count(0) {
        image_pub = nh.advertise<sensor_msgs::Image>("camera/image_raw", 10);
        compressed_pub = nh.advertise<sensor_msgs::CompressedImage>("camera/image_compressed", 10);
    }

    void publishTestPattern() {
        // 创建测试图案
        cv::Mat image(image_height, image_width, CV_8UC3);
        
        // 生成动态测试图案
        for(int y = 0; y < image_height; y++) {
            for(int x = 0; x < image_width; x++) {
                cv::Vec3b& pixel = image.at<cv::Vec3b>(y, x);
                
                // 红色通道：水平渐变 + 时间变化
                pixel[2] = static_cast<unsigned char>(255 * x / image_width + 
                    50 * sin(frame_count * 0.1 + x * 0.05));
                
                // 绿色通道：垂直渐变 + 时间变化  
                pixel[1] = static_cast<unsigned char>(255 * y / image_height + 
                    50 * cos(frame_count * 0.1 + y * 0.05));
                
                // 蓝色通道：对角线渐变
                pixel[0] = static_cast<unsigned char>(128 + 
                    127 * sin((x + y) * 0.01 + frame_count * 0.05));
            }
        }
        
        // 添加移动的圆形
        int circle_radius = 50 + 20 * sin(frame_count * 0.2);
        cv::Point circle_center(
            image_width / 2 + 100 * cos(frame_count * 0.1),
            image_height / 2 + 100 * sin(frame_count * 0.1)
        );
        cv::circle(image, circle_center, circle_radius, cv::Scalar(255, 255, 255), 3);
        
        // 添加文本
        std::string text = "Frame: " + std::to_string(frame_count);
        cv::putText(image, text, cv::Point(10, 30), 
                   cv::FONT_HERSHEY_SIMPLEX, 1.0, cv::Scalar(255, 255, 255), 2);
        
        // 转换为ROS消息并发布
        sensor_msgs::ImagePtr msg = cv_bridge::CvImage(
            std_msgs::Header(), "bgr8", image).toImageMsg();
        msg->header.stamp = ros::Time::now();
        msg->header.frame_id = "camera_frame";
        
        image_pub.publish(msg);
        
        // 发布压缩图像
        sensor_msgs::CompressedImage compressed_msg;
        compressed_msg.header = msg->header;
        compressed_msg.format = "jpeg";
        
        // 压缩图像
        std::vector<int> compression_params;
        compression_params.push_back(cv::IMWRITE_JPEG_QUALITY);
        compression_params.push_back(80); // 质量参数
        
        cv::imencode(".jpg", image, compressed_msg.data, compression_params);
        compressed_pub.publish(compressed_msg);
        
        frame_count++;
    }

    void run() {
        ros::Rate rate(10); // 10Hz
        
        while(ros::ok()) {
            publishTestPattern();
            ros::spinOnce();
            rate.sleep();
        }
    }
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "image_publisher_demo");
    
    // 检查OpenCV可用性
    try {
        ImagePublisherDemo demo;
        demo.run();
    } catch(const cv::Exception& e) {
        ROS_ERROR("OpenCV错误: %s", e.what());
        return 1;
    }
    
    return 0;
}

Python图像处理管道：

#!/usr/bin/env python
# 文件名：image_processing_pipeline.py
import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image, CompressedImage
from cv_bridge import CvBridge, CvBridgeError

class ImageProcessingPipeline:
    def __init__(self):
        rospy.init_node('image_processing_pipeline')
        
        self.bridge = CvBridge()
        
        # 订阅原始图像
        self.image_sub = rospy.Subscriber("/camera/image_raw", Image, self.image_callback)
        
        # 发布处理后的图像
        self.gray_pub = rospy.Publisher("/image_processing/gray", Image, queue_size=10)
        self.edges_pub = rospy.Publisher("/image_processing/edges", Image, queue_size=10)
        self.blur_pub = rospy.Publisher("/image_processing/blur", Image, queue_size=10)
        self.hsv_pub = rospy.Publisher("/image_processing/hsv", Image, queue_size=10)
        
        self.frame_count = 0
        
    def image_callback(self, data):
        try:
            # 转换ROS图像消息为OpenCV格式
            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
            
            # 应用多种图像处理
            self.publish_grayscale(cv_image, data.header)
            self.publish_edge_detection(cv_image, data.header)
            self.publish_blurred(cv_image, data.header)
            self.publish_hsv(cv_image, data.header)
            
            self.frame_count += 1
            
        except CvBridgeError as e:
            rospy.logerr("CV桥接错误: %s", e)
    
    def publish_grayscale(self, cv_image, header):
        """发布灰度图像"""
        gray_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
        gray_msg = self.bridge.cv2_to_imgmsg(gray_image, "mono8")
        gray_msg.header = header
        self.gray_pub.publish(gray_msg)
    
    def publish_edge_detection(self, cv_image, header):
        """发布边缘检测结果"""
        gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
        edges = cv2.Canny(gray, 50, 150)
        edges_msg = self.bridge.cv2_to_imgmsg(edges, "mono8")
        edges_msg.header = header
        self.edges_pub.publish(edges_msg)
    
    def publish_blurred(self, cv_image, header):
        """发布模糊图像"""
        # 高斯模糊
        blurred = cv2.GaussianBlur(cv_image, (15, 15), 0)
        blurred_msg = self.bridge.cv2_to_imgmsg(blurred, "bgr8")
        blurred_msg.header = header
        self.blur_pub.publish(blurred_msg)
    
    def publish_hsv(self, cv_image, header):
        """发布HSV颜色空间图像"""
        hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
        hsv_msg = self.bridge.cv2_to_imgmsg(hsv, "bgr8")  # 注意：HSV在OpenCV中仍用BGR8编码显示
        hsv_msg.header = header
        self.hsv_pub.publish(hsv_msg)
    
    def run(self):
        rospy.loginfo("图像处理管道已启动")
        rospy.loginfo("使用 rqt_image_view 查看以下主题:")
        rospy.loginfo("  /image_processing/gray - 灰度图像")
        rospy.loginfo("  /image_processing/edges - 边缘检测")
        rospy.loginfo("  /image_processing/blur - 模糊图像") 
        rospy.loginfo("  /image_processing/hsv - HSV颜色空间")
        rospy.spin()

if __name__ == '__main__':
    try:
        pipeline = ImageProcessingPipeline()
        pipeline.run()
    except rospy.ROSInterruptException:
        pass

rqt_image_view配置脚本：

#!/bin/bash
# 文件名：rqt_image_view_setup.sh

echo "设置rqt_image_view多窗口图像监控..."

# 启动rqt_image_view查看不同处理阶段的图像
echo "1. 启动原始图像查看器..."
rqt_image_view /camera/image_raw &
sleep 1

echo "2. 启动灰度图像查看器..."
rqt_image_view /image_processing/gray &
sleep 1

echo "3. 启动边缘检测查看器..."
rqt_image_view /image_processing/edges &
sleep 1

echo "4. 启动模糊图像查看器..."
rqt_image_view /image_processing/blur &
sleep 1

echo "5. 启动HSV图像查看器..."
rqt_image_view /image_processing/hsv &

echo "所有图像查看器已启动"
echo ""
echo "使用说明:"
echo "- 在每个窗口中可以使用鼠标滚轮缩放"
echo "- 右键点击可保存当前帧"
echo "- 使用主题下拉菜单切换不同图像流"

3.5 高级数据可视化与PlotJuggler应用

PlotJuggler是ROS生态系统中功能强大的数据可视化工具，支持时间序列数据的深入分析和处理。

C++复杂数据发布示例：

// 文件名：plotjuggler_data_source.cpp
#include <ros/ros.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Int32.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Imu.h>
#include <nav_msgs/Odometry.h>
#include <tf2/LinearMath/Quaternion.h>
#include <cmath>
#include <vector>

class PlotJugglerDataSource {
private:
    ros::NodeHandle nh;
    
    // 多种数据发布者
    ros::Publisher sine_wave_pub;
    ros::Publisher square_wave_pub;
    ros::Publisher triangle_wave_pub;
    ros::Publisher random_walk_pub;
    ros::Publisher imu_pub;
    ros::Publisher odom_pub;
    ros::Publisher multi_signal_pub;
    
    double time_counter;
    double random_walk_value;

public:
    PlotJugglerDataSource() : time_counter(0), random_walk_value(0) {
        sine_wave_pub = nh.advertise<std_msgs::Float64>("signals/sine_wave", 10);
        square_wave_pub = nh.advertise<std_msgs::Float64>("signals/square_wave", 10);
        triangle_wave_pub = nh.advertise<std_msgs::Float64>("signals/triangle_wave", 10);
        random_walk_pub = nh.advertise<std_msgs::Float64>("signals/random_walk", 10);
        imu_pub = nh.advertise<sensor_msgs::Imu>("sensors/imu", 10);
        odom_pub = nh.advertise<nav_msgs::Odometry>("navigation/odometry", 10);
        multi_signal_pub = nh.advertise<geometry_msgs::Point>("signals/multi_component", 10);
    }

    void publishWaveforms() {
        // 正弦波
        std_msgs::Float64 sine_msg;
        sine_msg.data = sin(time_counter);
        sine_wave_pub.publish(sine_msg);
        
        // 方波
        std_msgs::Float64 square_msg;
        square_msg.data = (sin(time_counter) > 0) ? 1.0 : -1.0;
        square_wave_pub.publish(square_msg);
        
        // 三角波
        std_msgs::Float64 triangle_msg;
        triangle_msg.data = 2.0 * fabs(fmod(time_counter, 2.0) - 1.0) - 1.0;
        triangle_wave_pub.publish(triangle_msg);
        
        // 随机游走
        std_msgs::Float64 random_msg;
        random_walk_value += (rand() % 100 - 50) / 100.0;
        random_walk_value = fmax(-5.0, fmin(5.0, random_walk_value)); // 限制范围
        random_msg.data = random_walk_value;
        random_walk_pub.publish(random_msg);
    }

    void publishIMUData() {
        sensor_msgs::Imu imu_msg;
        imu_msg.header.stamp = ros::Time::now();
        imu_msg.header.frame_id = "imu_link";
        
        // 模拟陀螺仪数据
        imu_msg.angular_velocity.x = 0.1 * sin(time_counter);
        imu_msg.angular_velocity.y = 0.2 * cos(time_counter * 0.7);
        imu_msg.angular_velocity.z = 0.05 * sin(time_counter * 1.3);
        
        // 模拟加速度计数据
        imu_msg.linear_acceleration.x = 9.8 + 0.5 * sin(time_counter * 2.0);
        imu_msg.linear_acceleration.y = 0.3 * cos(time_counter * 1.5);
        imu_msg.linear_acceleration.z = 0.2 * sin(time_counter * 0.8);
        
        imu_pub.publish(imu_msg);
    }

    void publishOdometry() {
        nav_msgs::Odometry odom_msg;
        odom_msg.header.stamp = ros::Time::now();
        odom_msg.header.frame_id = "odom";
        odom_msg.child_frame_id = "base_link";
        
        // 模拟位置
        odom_msg.pose.pose.position.x = 2.0 * sin(time_counter * 0.2);
        odom_msg.pose.pose.position.y = 1.5 * cos(time_counter * 0.2);
        odom_msg.pose.pose.position.z = 0.1 * sin(time_counter);
        
        // 模拟方向
        tf2::Quaternion quat;
        quat.setRPY(0, 0, time_counter * 0.1);
        odom_msg.pose.pose.orientation.x = quat.x();
        odom_msg.pose.pose.orientation.y = quat.y();
        odom_msg.pose.pose.orientation.z = quat.z();
        odom_msg.pose.pose.orientation.w = quat.w();
        
        // 模拟速度
        odom_msg.twist.twist.linear.x = 0.4 * cos(time_counter * 0.2);
        odom_msg.twist.twist.linear.y = -0.3 * sin(time_counter * 0.2);
        odom_msg.twist.twist.angular.z = 0.1;
        
        odom_pub.publish(odom_msg);
    }

    void publishMultiComponentSignal() {
        geometry_msgs::Point multi_msg;
        multi_msg.x = sin(time_counter);          // 基础信号
        multi_msg.y = 0.5 * sin(2 * time_counter); // 二次谐波
        multi_msg.z = 0.2 * sin(5 * time_counter); // 五次谐波
        multi_signal_pub.publish(multi_msg);
    }

    void run() {
        ros::Rate rate(20); // 20Hz
        
        ROS_INFO("启动PlotJuggler数据源");
        ROS_INFO("发布以下数据流:");
        ROS_INFO("  /signals/sine_wave - 正弦波");
        ROS_INFO("  /signals/square_wave - 方波"); 
        ROS_INFO("  /signals/triangle_wave - 三角波");
        ROS_INFO("  /signals/random_walk - 随机游走");
        ROS_INFO("  /sensors/imu - 模拟IMU数据");
        ROS_INFO("  /navigation/odometry - 模拟里程计数据");
        ROS_INFO("  /signals/multi_component - 多分量信号");
        
        while(ros::ok()) {
            publishWaveforms();
            publishIMUData();
            publishOdometry();
            publishMultiComponentSignal();
            
            time_counter += 0.05; // 每次增加0.05秒
            ros::spinOnce();
            rate.sleep();
        }
    }
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "plotjuggler_data_source");
    
    // 初始化随机数生成器
    srand(time(nullptr));
    
    PlotJugglerDataSource data_source;
    data_source.run();
    
    return 0;
}

PlotJuggler配置文件生成：

#!/usrusr/bin/env python
# 文件名：plotjuggler_config_generator.py
import json
import rospy
import os

class PlotJugglerConfigGenerator:
    def __init__(self):
        self.config = {
            "version": "1.0",
            "plot_layouts": [],
            "data_streams": [],
            "transforms": []
        }
    
    def create_waveform_layout(self):
        """创建波形显示布局"""
        waveform_layout = {
            "name": "Waveforms",
            "type": "Horizontal",
            "children": [
                {
                    "name": "Sine_Wave",
                    "type": "Plot",
                    "curves": [
                        {
                            "topic": "/signals/sine_wave",
                            "field": "data",
                            "color": "#FF0000",
                            "style": "Lines"
                        }
                    ]
                },
                {
                    "name": "Square_Triangle",
                    "type": "Vertical",
                    "children": [
                        {
                            "name": "Square_Wave",
                            "type": "Plot",
                            "curves": [
                                {
                                    "topic": "/signals/square_wave",
                                    "field": "data",
                                    "color": "#00FF00",
                                    "style": "Lines"
                                }
                            ]
                        },
                        {
                            "name": "Triangle_Wave",
                            "type": "Plot", 
                            "curves": [
                                {
                                    "topic": "/signals/triangle_wave",
                                    "field": "data",
                                    "color": "#0000FF",
                                    "style": "Lines"
                                }
                            ]
                        }
                    ]
                }
            ]
        }
        return waveform_layout
    
    def create_imu_layout(self):
        """创建IMU数据显示布局"""
        imu_layout = {
            "name": "IMU_Data",
            "type": "Horizontal",
            "children": [
                {
                    "name": "Angular_Velocity",
                    "type": "Plot",
                    "curves": [
                        {
                            "topic": "/sensors/imu",
                            "field": "angular_velocity.x",
                            "color": "#FF0000",
                            "style": "Lines",
                            "label": "Gyro X"
                        },
                        {
                            "topic": "/sensors/imu", 
                            "field": "angular_velocity.y",
                            "color": "#00FF00", 
                            "style": "Lines",
                            "label": "Gyro Y"
                        },
                        {
                            "topic": "/sensors/imu",
                            "field": "angular_velocity.z", 
                            "color": "#0000FF",
                            "style": "Lines",
                            "label": "Gyro Z"
                        }
                    ]
                },
                {
                    "name": "Linear_Acceleration",
                    "type": "Plot",
                    "curves": [
                        {
                            "topic": "/sensors/imu",
                            "field": "linear_acceleration.x",
                            "color": "#FF0000", 
                            "style": "Lines",
                            "label": "Accel X"
                        },
                        {
                            "topic": "/sensors/imu",
                            "field": "linear_acceleration.y",
                            "color": "#00FF00",
                            "style": "Lines", 
                            "label": "Accel Y"
                        },
                        {
                            "topic": "/sensors/imu",
                            "field": "linear_acceleration.z",
                            "color": "#0000FF",
                            "style": "Lines",
                            "label": "Accel Z"
                        }
                    ]
                }
            ]
        }
        return imu_layout
    
    def create_odometry_layout(self):
        """创建里程计数据显示布局"""
        odom_layout = {
            "name": "Odometry",
            "type": "TabWidget",
            "children": [
                {
                    "name": "Position",
                    "type": "Plot",
                    "curves": [
                        {
                            "topic": "/navigation/odometry",
                            "field": "pose.pose.position.x",
                            "color": "#FF0000",
                            "style": "Lines",
                            "label": "Pos X"
                        },
                        {
                            "topic": "/navigation/odometry",
                            "field": "pose.pose.position.y", 
                            "color": "#00FF00",
                            "style": "Lines",
                            "label": "Pos Y"
                        },
                        {
                            "topic": "/navigation/odometry",
                            "field": "pose.pose.position.z",
                            "color": "#0000FF", 
                            "style": "Lines",
                            "label": "Pos Z"
                        }
                    ]
                },
                {
                    "name": "Velocity",
                    "type": "Plot",
                    "curves": [
                        {
                            "topic": "/navigation/odometry",
                            "field": "twist.twist.linear.x",
                            "color": "#FF0000",
                            "style": "Lines", 
                            "label": "Vel X"
                        },
                        {
                            "topic": "/navigation/odometry",
                            "field": "twist.twist.linear.y",
                            "color": "#00FF00",
                            "style": "Lines",
                            "label": "Vel Y"
                        },
                        {
                            "topic": "/navigation/odometry", 
                            "field": "twist.twist.angular.z",
                            "color": "#0000FF",
                            "style": "Lines",
                            "label": "Angular Vel Z"
                        }
                    ]
                }
            ]
        }
        return odom_layout
    
    def create_multi_signal_layout(self):
        """创建多信号分析布局"""
        multi_layout = {
            "name": "Multi_Signal_Analysis",
            "type": "Plot",
            "curves": [
                {
                    "topic": "/signals/multi_component",
                    "field": "x",
                    "color": "#FF0000",
                    "style": "Lines",
                    "label": "Fundamental"
                },
                {
                    "topic": "/signals/multi_component",
                    "field": "y", 
                    "color": "#00FF00",
                    "style": "Lines",
                    "label": "2nd Harmonic"
                },
                {
                    "topic": "/signals/multi_component",
                    "field": "z",
                    "color": "#0000FF",
                    "style": "Lines", 
                    "label": "5th Harmonic"
                },
                {
                    "topic": "/signals/random_walk",
                    "field": "data",
                    "color": "#FF00FF", 
                    "style": "Lines",
                    "label": "Random Walk"
                }
            ]
        }
        return multi_layout
    
    def generate_config(self, output_path):
        """生成PlotJuggler配置文件"""
        # 添加布局
        self.config["plot_layouts"].append(self.create_waveform_layout())
        self.config["plot_layouts"].append(self.create_imu_layout()) 
        self.config["plot_layouts"].append(self.create_odometry_layout())
        self.config["plot_layouts"].append(self.create_multi_signal_layout())
        
        # 保存配置文件
        with open(output_path, 'w') as f:
            json.dump(self.config, f, indent=2)
        
        print(f"PlotJuggler配置文件已生成: {output_path}")
        print("使用命令加载配置:")
        print(f"  plotjuggler --layout {output_path}")

def main():
    rospy.init_node('plotjuggler_config_generator')
    
    generator = PlotJugglerConfigGenerator()
    
    # 确定输出路径
    output_dir = os.path.expanduser("~/.plotjuggler")
    os.makedirs(output_dir, exist_ok=True)
    output_path = os.path.join(output_dir, "ros_data_analysis.xml")
    
    generator.generate_config(output_path)
    
    rospy.loginfo("PlotJuggler配置生成完成")
    rospy.loginfo("请先启动数据源节点，然后运行:")
    rospy.loginfo("  plotjuggler --layout %s", output_path)

if __name__ == '__main__':
    main()

本章后续内容（RViz、Gazebo、人机交互等）将保持简洁但完整。实际开发中，这些可视化工具的组合使用可以极大提高机器人系统的开发和调试效率。
+++++++++++++++++++++++++++++++++++++++++++++++++++++++

第3章 ROS可视化工具与系统调试技巧（续）

3.6 RViz三维可视化平台深度应用

RViz是ROS中最核心的三维可视化工具，能够显示机器人模型、传感器数据、导航信息等。掌握RViz的高级应用对于机器人开发至关重要。

RViz配置与插件开发

RViz显示配置管理：

// 文件名：rviz_config_manager.cpp
#include <ros/ros.h>
#include <rviz/visualization_manager.h>
#include <rviz/render_panel.h>
#include <rviz/display.h>
#include <rviz/tool_manager.h>
#include <rviz/view_manager.h>
#include <rviz/default_plugin/view_controllers/orbit_view_controller.h>
#include <QApplication>

class RvizConfigManager {
private:
    rviz::VisualizationManager* manager;
    rviz::RenderPanel* render_panel;
    
public:
    RvizConfigManager() {
        // 创建RViz组件
        render_panel = new rviz::RenderPanel();
        manager = new rviz::VisualizationManager(render_panel);
        render_panel->initialize(manager->getSceneManager(), manager);
        manager->initialize();
        manager->startUpdate();
    }
    
    void setupDefaultDisplays() {
        // 添加网格显示
        rviz::Display* grid = manager->createDisplay("rviz/Grid", "Grid", true);
        grid->subProp("Line Style")->setValue("Billboards");
        grid->subProp("Color")->setValue(QColor(128, 128, 128));
        grid->subProp("Cell Size")->setValue(1.0);
        
        // 添加机器人模型显示
        rviz::Display* robot_model = manager->createDisplay("rviz/RobotModel", "Robot Model", true);
        robot_model->subProp("Robot Description")->setValue("robot_description");
        
        // 添加TF显示
        rviz::Display* tf = manager->createDisplay("rviz/TF", "TF", true);
        tf->subProp("Frame Timeout")->setValue(15.0);
        
        // 设置视角控制器
        manager->getViewManager()->setCurrentViewControllerType("rviz/Orbit");
        rviz::OrbitViewController* view_controller = 
            dynamic_cast<rviz::OrbitViewController*>(manager->getViewManager()->getCurrent());
        if(view_controller) {
            view_controller->subProp("Distance")->setValue(10.0);
        }
    }
    
    void addLaserScanDisplay() {
        rviz::Display* laser = manager->createDisplay("rviz/LaserScan", "Laser Scan", true);
        laser->subProp("Topic")->setValue("/scan");
        laser->subProp("Size (m)")->setValue(0.1);
        laser->subProp("Color")->setValue(QColor(255, 0, 0));
    }
    
    void addPointCloudDisplay() {
        rviz::Display* pointcloud = manager->createDisplay("rviz/PointCloud2", "PointCloud", true);
        pointcloud->subProp("Topic")->setValue("/point_cloud");
        pointcloud->subProp("Size (Pixels)")->setValue(3);
        pointcloud->subProp("Style")->setValue("Points");
    }
    
    void addMapDisplay() {
        rviz::Display* map = manager->createDisplay("rviz/Map", "Map", true);
        map->subProp("Topic")->setValue("/map");
        map->subProp("Color Scheme")->setValue("map");
        map->subProp("Alpha")->setValue(0.7);
    }
    
    void addPathDisplay() {
        rviz::Display* path = manager->createDisplay("rviz/Path", "Global Path", true);
        path->subProp("Topic")->setValue("/global_plan");
        path->subProp("Color")->setValue(QColor(0, 255, 0));
        path->subProp("Buffer Length")->setValue(100);
    }
    
    void saveConfig(const std::string& filename) {
        rviz::Config config;
        manager->save(config);
        YAML::Emitter emitter;
        emitter << config;
        std::ofstream fout(filename);
        fout << emitter.c_str();
    }
    
    void loadConfig(const std::string& filename) {
        YAML::Node node = YAML::LoadFile(filename);
        rviz::Config config;
        config = node;
        manager->load(config);
    }
};

RViz标记发布示例：

// 文件名：rviz_marker_publisher.cpp
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Pose.h>
#include <tf2/LinearMath/Quaternion.h>
#include <cmath>
#include <vector>

class RvizMarkerPublisher {
private:
    ros::NodeHandle nh;
    ros::Publisher marker_pub;
    ros::Publisher marker_array_pub;
    
    int marker_id;
    double time_counter;

public:
    RvizMarkerPublisher() : marker_id(0), time_counter(0) {
        marker_pub = nh.advertise<visualization_msgs::Marker>("visualization_marker", 10);
        marker_array_pub = nh.advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 10);
    }

    void publishSphereMarker() {
        visualization_msgs::Marker marker;
        marker.header.frame_id = "base_link";
        marker.header.stamp = ros::Time::now();
        marker.ns = "basic_shapes";
        marker.id = marker_id++;
        marker.type = visualization_msgs::Marker::SPHERE;
        marker.action = visualization_msgs::Marker::ADD;
        
        // 位置和方向
        marker.pose.position.x = 2.0 * cos(time_counter);
        marker.pose.position.y = 2.0 * sin(time_counter);
        marker.pose.position.z = 1.0;
        marker.pose.orientation.w = 1.0;
        
        // 尺寸
        marker.scale.x = 0.5;
        marker.scale.y = 0.5;
        marker.scale.z = 0.5;
        
        // 颜色和透明度
        marker.color.r = 1.0f;
        marker.color.g = 0.0f;
        marker.color.b = 0.0f;
        marker.color.a = 0.8f;
        
        marker.lifetime = ros::Duration(0.5); // 只显示0.5秒
        
        marker_pub.publish(marker);
    }

    void publishCubeMarker() {
        visualization_msgs::Marker marker;
        marker.header.frame_id = "base_link";
        marker.header.stamp = ros::Time::now();
        marker.ns = "basic_shapes";
        marker.id = marker_id++;
        marker.type = visualization_msgs::Marker::CUBE;
        marker.action = visualization_msgs::Marker::ADD;
        
        marker.pose.position.x = 0.0;
        marker.pose.position.y = 0.0;
        marker.pose.position.z = 0.5;
        marker.pose.orientation.w = 1.0;
        
        marker.scale.x = 1.0;
        marker.scale.y = 1.0;
        marker.scale.z = 1.0;
        
        marker.color.r = 0.0f;
        marker.color.g = 1.0f;
        marker.color.b = 0.0f;
        marker.color.a = 0.6f;
        
        marker.lifetime = ros::Duration();
        
        marker_pub.publish(marker);
    }

    void publishLineStripMarker() {
        visualization_msgs::Marker marker;
        marker.header.frame_id = "base_link";
        marker.header.stamp = ros::Time::now();
        marker.ns = "lines";
        marker.id = marker_id++;
        marker.type = visualization_msgs::Marker::LINE_STRIP;
        marker.action = visualization_msgs::Marker::ADD;
        
        marker.pose.orientation.w = 1.0;
        
        // 线条尺寸
        marker.scale.x = 0.1;
        
        // 线条颜色
        marker.color.r = 0.0f;
        marker.color.g = 0.0f;
        marker.color.b = 1.0f;
        marker.color.a = 1.0f;
        
        // 创建圆形路径
        int num_points = 20;
        for(int i = 0; i <= num_points; ++i) {
            geometry_msgs::Point p;
            double angle = 2.0 * M_PI * i / num_points;
            p.x = 3.0 * cos(angle);
            p.y = 3.0 * sin(angle);
            p.z = 0.0;
            marker.points.push_back(p);
        }
        
        marker_pub.publish(marker);
    }

    void publishTextMarker() {
        visualization_msgs::Marker marker;
        marker.header.frame_id = "base_link";
        marker.header.stamp = ros::Time::now();
        marker.ns = "text";
        marker.id = marker_id++;
        marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
        marker.action = visualization_msgs::Marker::ADD;
        
        marker.pose.position.x = 0.0;
        marker.pose.position.y = 0.0;
        marker.pose.position.z = 2.0;
        marker.pose.orientation.w = 1.0;
        
        marker.scale.z = 0.3; // 文字高度
        
        marker.color.r = 1.0f;
        marker.color.g = 1.0f;
        marker.color.b = 1.0f;
        marker.color.a = 1.0f;
        
        marker.text = "ROS RViz Marker Demo\nTime: " + std::to_string(time_counter);
        
        marker_pub.publish(marker);
    }

    void publishMarkerArray() {
        visualization_msgs::MarkerArray marker_array;
        
        // 创建多个球体标记
        int num_markers = 10;
        for(int i = 0; i < num_markers; ++i) {
            visualization_msgs::Marker marker;
            marker.header.frame_id = "base_link";
            marker.header.stamp = ros::Time::now();
            marker.ns = "array_spheres";
            marker.id = i;
            marker.type = visualization_msgs::Marker::SPHERE;
            marker.action = visualization_msgs::Marker::ADD;
            
            double angle = 2.0 * M_PI * i / num_markers;
            marker.pose.position.x = 4.0 * cos(angle + time_counter);
            marker.pose.position.y = 4.0 * sin(angle + time_counter);
            marker.pose.position.z = 0.5;
            marker.pose.orientation.w = 1.0;
            
            marker.scale.x = 0.3;
            marker.scale.y = 0.3;
            marker.scale.z = 0.3;
            
            // 彩虹颜色
            marker.color.r = 0.5f + 0.5f * cos(angle);
            marker.color.g = 0.5f + 0.5f * cos(angle + 2.0 * M_PI / 3.0);
            marker.color.b = 0.5f + 0.5f * cos(angle + 4.0 * M_PI / 3.0);
            marker.color.a = 0.7f;
            
            marker_array.markers.push_back(marker);
        }
        
        marker_array_pub.publish(marker_array);
    }

    void run() {
        ros::Rate rate(10); // 10Hz
        
        ROS_INFO("RViz标记发布器已启动");
        ROS_INFO("在RViz中添加Marker和MarkerArray显示来查看效果");
        
        while(ros::ok()) {
            publishSphereMarker();
            publishCubeMarker();
            publishLineStripMarker();
            publishTextMarker();
            publishMarkerArray();
            
            time_counter += 0.1;
            ros::spinOnce();
            rate.sleep();
        }
    }
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "rviz_marker_publisher");
    
    RvizMarkerPublisher publisher;
    publisher.run();
    
    return 0;
}

RViz Python配置工具：

#!/usr/bin/env python
# 文件名：rviz_config_tool.py
import rospy
import yaml
import os
from geometry_msgs.msg import Point, Pose, Quaternion
from visualization_msgs.msg import Marker, MarkerArray
import tf
import math

class RvizConfigTool:
    def __init__(self):
        self.marker_pub = rospy.Publisher('/visualization_marker', Marker, queue_size=10)
        self.marker_array_pub = rospy.Publisher('/visualization_marker_array', MarkerArray, queue_size=10)
        
    def create_sphere_marker(self, position, radius=0.1, color=(1.0, 0.0, 0.0), alpha=1.0, frame_id="base_link", ns="sphere"):
        """创建球体标记"""
        marker = Marker()
        marker.header.frame_id = frame_id
        marker.header.stamp = rospy.Time.now()
        marker.ns = ns
        marker.id = hash(str(position) + ns) % 10000
        marker.type = Marker.SPHERE
        marker.action = Marker.ADD
        
        marker.pose.position = Point(*position)
        marker.pose.orientation = Quaternion(0, 0, 0, 1)
        
        marker.scale.x = radius * 2
        marker.scale.y = radius * 2
        marker.scale.z = radius * 2
        
        marker.color.r = color[0]
        marker.color.g = color[1]
        marker.color.b = color[2]
        marker.color.a = alpha
        
        return marker
    
    def create_arrow_marker(self, start_point, end_point, diameter=0.05, color=(0.0, 1.0, 0.0), alpha=1.0, frame_id="base_link", ns="arrow"):
        """创建箭头标记"""
        marker = Marker()
        marker.header.frame_id = frame_id
        marker.header.stamp = rospy.Time.now()
        marker.ns = ns
        marker.id = hash(str(start_point) + str(end_point) + ns) % 10000
        marker.type = Marker.ARROW
        marker.action = Marker.ADD
        
        # 计算箭头的中心位置和方向
        center_x = (start_point[0] + end_point[0]) / 2.0
        center_y = (start_point[1] + end_point[1]) / 2.0
        center_z = (start_point[2] + end_point[2]) / 2.0
        
        marker.pose.position = Point(center_x, center_y, center_z)
        
        # 计算方向四元数
        dx = end_point[0] - start_point[0]
        dy = end_point[1] - start_point[1]
        dz = end_point[2] - start_point[2]
        
        length = math.sqrt(dx*dx + dy*dy + dz*dz)
        if length > 0:
            # 使用TF计算四元数
            quat = tf.transformations.quaternion_from_euler(0, math.atan2(dz, math.sqrt(dx*dx+dy*dy)), math.atan2(dy, dx))
            marker.pose.orientation = Quaternion(*quat)
        
        marker.scale.x = length  # 长度
        marker.scale.y = diameter  # 箭头直径
        marker.scale.z = diameter  # 箭头直径
        
        marker.color.r = color[0]
        marker.color.g = color[1]
        marker.color.b = color[2]
        marker.color.a = alpha
        
        return marker
    
    def create_text_marker(self, text, position, height=0.2, color=(1.0, 1.0, 1.0), alpha=1.0, frame_id="base_link", ns="text"):
        """创建文本标记"""
        marker = Marker()
        marker.header.frame_id = frame_id
        marker.header.stamp = rospy.Time.now()
        marker.ns = ns
        marker.id = hash(text + str(position)) % 10000
        marker.type = Marker.TEXT_VIEW_FACING
        marker.action = Marker.ADD
        
        marker.pose.position = Point(*position)
        marker.pose.orientation = Quaternion(0, 0, 0, 1)
        
        marker.scale.z = height  # 文字高度
        
        marker.color.r = color[0]
        marker.color.g = color[1]
        marker.color.b = color[2]
        marker.color.a = alpha
        
        marker.text = text
        
        return marker
    
    def create_cylinder_marker(self, position, height=1.0, radius=0.1, color=(0.0, 0.0, 1.0), alpha=1.0, frame_id="base_link", ns="cylinder"):
        """创建圆柱体标记"""
        marker = Marker()
        marker.header.frame_id = frame_id
        marker.header.stamp = rospy.Time.now()
        marker.ns = ns
        marker.id = hash(str(position) + ns) % 10000
        marker.type = Marker.CYLINDER
        marker.action = Marker.ADD
        
        marker.pose.position = Point(*position)
        marker.pose.orientation = Quaternion(0, 0, 0, 1)
        
        marker.scale.x = radius * 2
        marker.scale.y = radius * 2
        marker.scale.z = height
        
        marker.color.r = color[0]
        marker.color.g = color[1]
        marker.color.b = color[2]
        marker.color.a = alpha
        
        return marker
    
    def publish_coordinate_frame(self, position, scale=1.0, frame_id="base_link"):
        """发布坐标系标记"""
        markers = MarkerArray()
        
        # X轴 - 红色
        x_end = (position[0] + scale, position[1], position[2])
        markers.markers.append(self.create_arrow_marker(position, x_end, 0.02, (1.0, 0.0, 0.0), 1.0, frame_id, "coord_x"))
        markers.markers.append(self.create_text_marker("X", x_end, 0.1, (1.0, 0.0, 0.0), 1.0, frame_id, "coord_text"))
        
        # Y轴 - 绿色
        y_end = (position[0], position[1] + scale, position[2])
        markers.markers.append(self.create_arrow_marker(position, y_end, 0.02, (0.0, 1.0, 0.0), 1.0, frame_id, "coord_y"))
        markers.markers.append(self.create_text_marker("Y", y_end, 0.1, (0.0, 1.0, 0.0), 1.0, frame_id, "coord_text"))
        
        # Z轴 - 蓝色
        z_end = (position[0], position[1], position[2] + scale)
        markers.markers.append(self.create_arrow_marker(position, z_end, 0.02, (0.0, 0.0, 1.0), 1.0, frame_id, "coord_z"))
        markers.markers.append(self.create_text_marker("Z", z_end, 0.1, (0.0, 0.0, 1.0), 1.0, frame_id, "coord_text"))
        
        self.marker_array_pub.publish(markers)
    
    def generate_rviz_config(self, config_path="~/.rviz/default.rviz"):
        """生成RViz配置文件"""
        config = {
            'Visualization Manager': {
                'Class': 'rviz::VisualizationManager',
                'Displays': {
                    'Grid': {
                        'Class': 'rviz::Grid',
                        'Enabled': True,
                        'Line Style': 'Billboards',
                        'Cell Size': 1.0,
                        'Color': '128; 128; 128',
                        'Plane Cell Count': 10
                    },
                    'RobotModel': {
                        'Class': 'rviz::RobotModel',
                        'Enabled': True,
                        'Robot Description': 'robot_description'
                    },
                    'TF': {
                        'Class': 'rviz::TF',
                        'Enabled': True,
                        'Frame Timeout': 15.0
                    },
                    'LaserScan': {
                        'Class': 'rviz::LaserScan',
                        'Enabled': True,
                        'Topic': '/scan',
                        'Size (m)': 0.1
                    }
                },
                'Global Options': {
                    'Background Color': '48; 48; 48',
                    'Fixed Frame': 'base_link',
                    'Frame Rate': 30.0
                },
                'Toolbars': {
                    'Tools': {
                        'Class': 'rviz::Tool',
                        'Current': {
                            'Class': 'rviz::Interact'
                        }
                    }
                },
                'Views': {
                    'Current': {
                        'Class': 'rviz::OrbitViewController',
                        'Distance': 10.0,
                        'Focal Point': '0; 0; 0',
                        'Focal Shape Size': 0.05,
                        'Name': 'Current View',
                        'Near Clip Distance': 0.01,
                        'Pitch': 0.785,
                        'Target Frame': '<Fixed Frame>',
                        'Value': 'Orbit (rviz)',
                        'Yaw': 0.785
                    }
                }
            }
        }
        
        full_path = os.path.expanduser(config_path)
        os.makedirs(os.path.dirname(full_path), exist_ok=True)
        
        with open(full_path, 'w') as f:
            yaml.dump(config, f, default_flow_style=False)
        
        rospy.loginfo("RViz配置文件已生成: %s", full_path)

def main():
    rospy.init_node('rviz_config_tool')
    
    tool = RvizConfigTool()
    
    # 生成默认配置文件
    tool.generate_rviz_config()
    
    # 发布示例标记
    rate = rospy.Rate(1)
    count = 0
    
    while not rospy.is_shutdown():
        # 发布坐标系
        tool.publish_coordinate_frame((0, 0, 0), 1.0)
        
        # 发布球体
        sphere_pos = (2 * math.cos(count * 0.1), 2 * math.sin(count * 0.1), 1.0)
        sphere_marker = tool.create_sphere_marker(sphere_pos, 0.2, (1.0, 0.5, 0.0), 0.8)
        tool.marker_pub.publish(sphere_marker)
        
        # 发布圆柱体
        cylinder_pos = (0, 0, 0.5)
        cylinder_marker = tool.create_cylinder_marker(cylinder_pos, 1.0, 0.3, (0.5, 0.0, 0.5), 0.6)
        tool.marker_pub.publish(cylinder_marker)
        
        # 发布文本
        text_pos = (0, 0, 2.0)
        text_marker = tool.create_text_marker(f"RViz Demo\nCount: {count}", text_pos, 0.2, (1.0, 1.0, 1.0), 1.0)
        tool.marker_pub.publish(text_marker)
        
        count += 1
        rate.sleep()

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

3.7 Gazebo三维物理仿真平台集成

Gazebo是ROS中最强大的物理仿真平台，能够模拟复杂的物理环境和机器人行为。

Gazebo模型定义示例：

<!-- 文件名：mobile_robot/model.sdf -->
<?xml version="1.0"?>
<sdf version="1.6">
  <model name="mobile_robot">
    <pose>0 0 0.1 0 0 0</pose>
    
    <!-- 机器人底座 -->
    <link name="base_link">
      <pose>0 0 0.1 0 0 0</pose>
      <collision name="base_collision">
        <geometry>
          <box>
            <size>0.4 0.2 0.1</size>
          </box>
        </geometry>
        <surface>
          <friction>
            <ode>
              <mu>1.0</mu>
              <mu2>1.0</mu2>
            </ode>
          </friction>
        </surface>
      </collision>
      
      <visual name="base_visual">
        <geometry>
          <box>
            <size>0.4 0.2 0.1</size>
          </box>
        </geometry>
        <material>
          <script>
            <name>Gazebo/Blue</name>
          </script>
        </material>
      </visual>
      
      <inertial>
        <mass>5.0</mass>
        <inertia>
          <ixx>0.1</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.1</iyy>
          <iyz>0</iyz>
          <izz>0.1</izz>
        </inertia>
      </inertial>
    </link>
    
    <!-- 左轮 -->
    <link name="left_wheel">
      <pose>0.0 0.13 0.0 1.5707 0 0</pose>
      <collision name="left_wheel_collision">
        <geometry>
          <cylinder>
            <radius>0.05</radius>
            <length>0.02</length>
          </cylinder>
        </geometry>
        <surface>
          <friction>
            <ode>
              <mu>1.0</mu>
              <mu2>1.0</mu2>
              <slip1>0.1</slip1>
              <slip2>0.1</slip2>
            </ode>
          </friction>
        </surface>
      </collision>
      
      <visual name="left_wheel_visual">
        <geometry>
          <cylinder>
            <radius>0.05</radius>
            <length>0.02</length>
          </cylinder>
        </geometry>
        <material>
          <script>
            <name>Gazebo/Black</name>
          </script>
        </material>
      </visual>
      
      <inertial>
        <mass>0.5</mass>
        <inertia>
          <ixx>0.001</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.001</iyy>
          <iyz>0</iyz>
          <izz>0.001</izz>
        </inertia>
      </inertial>
    </link>
    
    <!-- 右轮 -->
    <link name="right_wheel">
      <pose>0.0 -0.13 0.0 1.5707 0 0</pose>
      <collision name="right_wheel_collision">
        <geometry>
          <cylinder>
            <radius>0.05</radius>
            <length>0.02</length>
          </cylinder>
        </geometry>
        <surface>
          <friction>
            <ode>
              <mu>1.0</mu>
              <mu2>1.0</mu2>
              <slip1>0.1</slip1>
              <slip2>0.1</slip2>
            </ode>
          </friction>
        </surface>
      </collision>
      
      <visual name="right_wheel_visual">
        <geometry>
          <cylinder>
            <radius>0.05</radius>
            <length>0.02</length>
          </cylinder>
        </geometry>
        <material>
          <script>
            <name>Gazebo/Black</name>
          </script>
        </material>
      </visual>
      
      <inertial>
        <mass>0.5</mass>
        <inertia>
          <ixx>0.001</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.001</iyy>
          <iyz>0</iyz>
          <izz>0.001</izz>
        </inertia>
      </inertial>
    </link>
    
    <!-- 激光雷达 -->
    <link name="laser_link">
      <pose>0.2 0 0.1 0 0 0</pose>
      <collision name="laser_collision">
        <geometry>
          <box>
            <size>0.05 0.05 0.05</size>
          </box>
        </geometry>
      </collision>
      
      <visual name="laser_visual">
        <geometry>
          <box>
            <size>0.05 0.05 0.05</size>
          </box>
        </geometry>
        <material>
          <script>
            <name>Gazebo/Red</name>
          </script>
        </material>
      </visual>
      
      <sensor name="laser_sensor" type="ray">
        <pose>0 0 0 0 0 0</pose>
        <visualize>true</visualize>
        <update_rate>40</update_rate>
        <ray>
          <scan>
            <horizontal>
              <samples>360</samples>
              <resolution>1.0</resolution>
              <min_angle>-3.14159</min_angle>
              <max_angle>3.14159</max_angle>
            </horizontal>
          </scan>
          <range>
            <min>0.1</min>
            <max>10.0</max>
            <resolution>0.01</resolution>
          </range>
        </ray>
        <plugin name="laser_controller" filename="libgazebo_ros_laser.so">
          <topicName>/scan</topicName>
          <frameName>laser_link</frameName>
        </plugin>
      </sensor>
    </link>
    
    <!-- 关节定义 -->
    <joint name="left_wheel_joint" type="revolute">
      <parent>base_link</parent>
      <child>left_wheel</child>
      <axis>
        <xyz>0 1 0</xyz>
        <limit>
          <lower>-1e+16</lower>
          <upper>1e+16</upper>
        </limit>
        <dynamics>
          <damping>0.1</damping>
        </dynamics>
      </axis>
    </joint>
    
    <joint name="right_wheel_joint" type="revolute">
      <parent>base_link</parent>
      <child>right_wheel</child>
      <axis>
        <xyz>0 1 0</xyz>
        <limit>
          <lower>-1e+16</lower>
          <upper>1e+16</upper>
        </limit>
        <dynamics>
          <damping>0.1</damping>
        </dynamics>
      </axis>
    </joint>
    
    <joint name="laser_joint" type="fixed">
      <parent>base_link</parent>
      <child>laser_link</child>
    </joint>
    
    <!-- ROS控制插件 -->
    <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>
      <publishOdomTF>true</publishOdomTF>
      <wheelSeparation>0.26</wheelSeparation>
      <wheelDiameter>0.1</wheelDiameter>
      <torque>10</torque>
      <publishWheelTF>true</publishWheelTF>
      <publishWheelJointState>true</publishWheelJointState>
    </plugin>
  </model>
</sdf>

Gazebo世界文件定义：

<!-- 文件名：simulation_world.world -->
<?xml version="1.0"?>
<sdf version="1.6">
  <world name="simulation_world">
    
    <!-- 物理引擎设置 -->
    <physics name="default_physics" default="true" type="ode">
      <max_step_size>0.001</max_step_size>
      <real_time_factor>1.0</real_time_factor>
      <real_time_update_rate>1000</real_time_update_rate>
    </physics>
    
    <!-- 场景设置 -->
    <scene>
      <ambient>0.4 0.4 0.4 1.0</ambient>
      <background>0.7 0.7 0.7 1.0</background>
      <shadows>true</shadows>
    </scene>
    
    <!-- 光照 -->
    <light name="sun" type="directional">
      <cast_shadows>true</cast_shadows>
      <pose>0 0 10 0 0 0</pose>
      <diffuse>0.8 0.8 0.8 1</diffuse>
      <specular>0.2 0.2 0.2 1</specular>
      <attenuation>
        <range>1000</range>
        <constant>0.9</constant>
        <linear>0.01</linear>
        <quadratic>0.001</quadratic>
      </attenuation>
      <direction>-0.5 0.1 -0.9</direction>
    </light>
    
    <!-- 地面 -->
    <model name="ground_plane">
      <static>true</static>
      <link name="link">
        <collision name="collision">
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>20 20</size>
            </plane>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>100</mu>
                <mu2>50</mu2>
              </ode>
            </friction>
          </surface>
        </collision>
        <visual name="visual">
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>20 20</size>
            </plane>
          </geometry>
          <material>
            <script>
              <name>Gazebo/Grey</name>
            </script>
          </material>
        </visual>
      </link>
    </model>
    
    <!-- 墙壁和障碍物 -->
    <model name="wall_north">
      <static>true</static>
      <pose>0 5 0.5 0 0 0</pose>
      <link name="link">
        <collision name="collision">
          <geometry>
            <box>
              <size>10 0.1 1</size>
            </box>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <box>
              <size>10 0.1 1</size>
            </box>
          </geometry>
          <material>
            <script>
              <name>Gazebo/Red</name>
            </script>
          </material>
        </visual>
      </link>
    </model>
    
    <model name="wall_south">
      <static>true</static>
      <pose>0 -5 0.5 0 0 0</pose>
      <link name="link">
        <collision name="collision">
          <geometry>
            <box>
              <size>10 0.1 1</size>
            </box>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <box>
              <size>10 0.1 1</size>
            </box>
          </geometry>
          <material>
            <script>
              <name>Gazebo/Red</name>
            </script>
          </material>
        </visual>
      </link>
    </model>
    
    <model name="wall_east">
      <static>true</static>
      <pose>5 0 0.5 0 0 0</pose>
      <link name="link">
        <collision name="collision">
          <geometry>
            <box>
              <size>0.1 10 1</size>
            </box>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <box>
              <size>0.1 10 1</size>
            </box>
          </geometry>
          <material>
            <script>
              <name>Gazebo/Red</name>
            </script>
          </material>
        </visual>
      </link>
    </model>
    
    <model name="wall_west">
      <static>true</static>
      <pose>-5 0 0.5 0 0 0</pose>
      <link name="link">
        <collision name="collision">
          <geometry>
            <box>
              <size>0.1 10 1</size>
            </box>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <box>
              <size>0.1 10 1</size>
            </box>
          </geometry>
          <material>
            <script>
              <name>Gazebo/Red</name>
            </script>
          </material>
        </visual>
      </link>
    </model>
    
    <!-- 障碍物 -->
    <model name="obstacle_1">
      <static>true</static>
      <pose>2 2 0.5 0 0 0</pose>
      <link name="link">
        <collision name="collision">
          <geometry>
            <cylinder>
              <radius>0.5</radius>
              <length>1</length>
            </cylinder>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <cylinder>
              <radius>0.5</radius>
              <length>1</length>
            </cylinder>
          </geometry>
          <material>
            <script>
              <name>Gazebo/Green</name>
            </script>
          </material>
        </visual>
      </link>
    </model>
    
    <model name="obstacle_2">
      <static>true</static>
      <pose>-2 -2 0.5 0 0 0</pose>
      <link name="link">
        <collision name="collision">
          <geometry>
            <box>
              <size>0.8 0.8 1</size>
            </box>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <box>
              <size>0.8 0.8 1</size>
            </box>
          </geometry>
          <material>
            <script>
              <name>Gazebo/Yellow</name>
            </script>
          </material>
        </visual>
      </link>
    </model>
    
  </world>
</sdf>

Gazebo控制节点：

// 文件名：gazebo_control_node.cpp
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <gazebo_msgs/ModelState.h>
#include <gazebo_msgs/SetModelState.h>
#include <gazebo_msgs/GetModelState.h>
#include <gazebo_msgs/SpawnModel.h>
#include <gazebo_msgs/DeleteModel.h>
#include <tf2/LinearMath/Quaternion.h>
#include <fstream>
#include <sstream>

class GazeboControlNode {
private:
    ros::NodeHandle nh;
    ros::Publisher cmd_vel_pub;
    ros::ServiceClient set_state_client;
    ros::ServiceClient get_state_client;
    ros::ServiceClient spawn_model_client;
    ros::ServiceClient delete_model_client;
    
    std::string robot_name;

public:
    GazeboControlNode() : robot_name("mobile_robot") {
        cmd_vel_pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 10);
        set_state_client = nh.serviceClient<gazebo_msgs::SetModelState>("/gazebo/set_model_state");
        get_state_client = nh.serviceClient<gazebo_msgs::GetModelState>("/gazebo/get_model_state");
        spawn_model_client = nh.serviceClient<gazebo_msgs::SpawnModel>("/gazebo/spawn_sdf_model");
        delete_model_client = nh.serviceClient<gazebo_msgs::DeleteModel>("/gazebo/delete_model");
    }

    bool setRobotPose(double x, double y, double z, double yaw) {
        gazebo_msgs::SetModelState srv;
        srv.request.model_state.model_name = robot_name;
        srv.request.model_state.pose.position.x = x;
        srv.request.model_state.pose.position.y = y;
        srv.request.model_state.pose.position.z = z;
        
        tf2::Quaternion quat;
        quat.setRPY(0, 0, yaw);
        srv.request.model_state.pose.orientation.x = quat.x();
        srv.request.model_state.pose.orientation.y = quat.y();
        srv.request.model_state.pose.orientation.z = quat.z();
        srv.request.model_state.pose.orientation.w = quat.w();
        
        srv.request.model_state.twist.linear.x = 0;
        srv.request.model_state.twist.linear.y = 0;
        srv.request.model_state.twist.linear.z = 0;
        srv.request.model_state.twist.angular.x = 0;
        srv.request.model_state.twist.angular.y = 0;
        srv.request.model_state.twist.angular.z = 0;
        
        srv.request.model_state.reference_frame = "world";
        
        if(set_state_client.call(srv)) {
            ROS_INFO("成功设置机器人位置: (%.2f, %.2f, %.2f)", x, y, z);
            return true;
        } else {
            ROS_ERROR("设置机器人位置失败");
            return false;
        }
    }

    bool getRobotPose(double& x, double& y, double& z, double& yaw) {
        gazebo_msgs::GetModelState srv;
        srv.request.model_name = robot_name;
        srv.request.relative_entity_name = "world";
        
        if(get_state_client.call(srv)) {
            x = srv.response.pose.position.x;
            y = srv.response.pose.position.y;
            z = srv.response.pose.position.z;
            
            // 从四元数提取偏航角
            tf2::Quaternion quat(
                srv.response.pose.orientation.x,
                srv.response.pose.orientation.y,
                srv.response.pose.orientation.z,
                srv.response.pose.orientation.w
            );
            tf2::Matrix3x3 mat(quat);
            double roll, pitch;
            mat.getRPY(roll, pitch, yaw);
            
            ROS_INFO("机器人当前位置: (%.2f, %.2f, %.2f), 偏航角: %.2f", x, y, z, yaw);
            return true;
        } else {
            ROS_ERROR("获取机器人位置失败");
            return false;
        }
    }

    bool spawnAdditionalModel(const std::string& model_name, const std::string& model_file, 
                             double x, double y, double z, double yaw) {
        // 读取模型文件
        std::ifstream file(model_file);
        if(!file.is_open()) {
            ROS_ERROR("无法打开模型文件: %s", model_file.c_str());
            return false;
        }
        
        std::stringstream buffer;
        buffer << file.rdbuf();
        std::string model_xml = buffer.str();
        
        gazebo_msgs::SpawnModel srv;
        srv.request.model_name = model_name;
        srv.request.model_xml = model_xml;
        srv.request.robot_namespace = "/" + model_name;
        srv.request.initial_pose.position.x = x;
        srv.request.initial_pose.position.y = y;
        srv.request.initial_pose.position.z = z;
        
        tf2::Quaternion quat;
        quat.setRPY(0, 0, yaw);
        srv.request.initial_pose.orientation.x = quat.x();
        srv.request.initial_pose.orientation.y = quat.y();
        srv.request.initial_pose.orientation.z = quat.z();
        srv.request.initial_pose.orientation.w = quat.w();
        
        srv.request.reference_frame = "world";
        
        if(spawn_model_client.call(srv)) {
            ROS_INFO("成功生成模型: %s", model_name.c_str());
            return true;
        } else {
            ROS_ERROR("生成模型失败: %s", model_name.c_str());
            return false;
        }
    }

    bool deleteModel(const std::string& model_name) {
        gazebo_msgs::DeleteModel srv;
        srv.request.model_name = model_name;
        
        if(delete_model_client.call(srv)) {
            ROS_INFO("成功删除模型: %s", model_name.c_str());
            return true;
        } else {
            ROS_ERROR("删除模型失败: %s", model_name.c_str());
            return false;
        }
    }

    void sendVelocityCommand(double linear_x, double angular_z) {
        geometry_msgs::Twist cmd;
        cmd.linear.x = linear_x;
        cmd.angular.z = angular_z;
        cmd_vel_pub.publish(cmd);
        
        ROS_DEBUG("发送速度命令: 线速度=%.2f, 角速度=%.2f", linear_x, angular_z);
    }

    void runNavigationDemo() {
        ros::Rate rate(10);
        double time_counter = 0;
        
        // 重置机器人位置
        setRobotPose(0, 0, 0.1, 0);
        
        ROS_INFO("开始Gazebo导航演示");
        
        while(ros::ok()) {
            // 简单的导航模式
            if(time_counter < 10.0) {
                // 前进
                sendVelocityCommand(0.5, 0.0);
            } else if(time_counter < 15.0) {
                // 旋转
                sendVelocityCommand(0.0, 0.5);
            } else if(time_counter < 25.0) {
                // 前进
                sendVelocityCommand(0.5, 0.0);
            } else {
                // 停止
                sendVelocityCommand(0.0, 0.0);
                
                // 每30秒重置位置
                if(fmod(time_counter, 30.0) < 0.1) {
                    setRobotPose(0, 0, 0.1, 0);
                }
            }
            
            // 定期报告位置
            if(fmod(time_counter, 5.0) < 0.1) {
                double x, y, z, yaw;
                getRobotPose(x, y, z, yaw);
            }
            
            time_counter += 0.1;
            ros::spinOnce();
            rate.sleep();
        }
    }
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "gazebo_control_node");
    
    GazeboControlNode controller;
    
    // 等待Gazebo服务可用
    ROS_INFO("等待Gazebo服务...");
    ros::Duration(5.0).sleep();
    
    // 演示各种功能
    controller.setRobotPose(1.0, 1.0, 0.1, M_PI/4);
    
    // 运行导航演示
    controller.runNavigationDemo();
    
    return 0;
}

3.8 ROS人机交互软件开发

ROS人机交互（HMI）软件包括各种界面工具，从简单的命令行工具到复杂的图形界面。

基于Qt的ROS控制界面：

// 文件名：ros_control_panel.h
#ifndef ROS_CONTROL_PANEL_H
#define ROS_CONTROL_PANEL_H

#include <ros/ros.h>
#include <QMainWindow>
#include <QTimer>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/LaserScan.h>
#include <nav_msgs/Odometry.h>
#include <std_srvs/Empty.h>

namespace Ui {
class ROSControlPanel;
}

class ROSControlPanel : public QMainWindow {
    Q_OBJECT

public:
    explicit ROSControlPanel(QWidget *parent = 0);
    ~ROSControlPanel();

private slots:
    void on_forwardButton_clicked();
    void on_backwardButton_clicked();
    void on_leftButton_clicked();
    void on_rightButton_clicked();
    void on_stopButton_clicked();
    void on_linearSlider_valueChanged(int value);
    void on_angularSlider_valueChanged(int value);
    void updateROS();
    
private:
    Ui::ROSControlPanel *ui;
    
    ros::NodeHandlePtr nh;
    ros::Publisher cmd_vel_pub;
    ros::Subscriber laser_sub;
    ros::Subscriber odom_sub;
    ros::ServiceClient reset_simulation_client;
    
    QTimer* ros_timer;
    
    double linear_velocity;
    double angular_velocity;
    
    void laserCallback(const sensor_msgs::LaserScan::ConstPtr& msg);
    void odomCallback(const nav_msgs::Odometry::ConstPtr& msg);
};

#endif // ROS_CONTROL_PANEL_H

// 文件名：ros_control_panel.cpp
#include "ros_control_panel.h"
#include "ui_ros_control_panel.h"

#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QPushButton>
#include <QSlider>
#include <QLabel>
#include <QProgressBar>
#include <QTextEdit>

ROSControlPanel::ROSControlPanel(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::ROSControlPanel),
    linear_velocity(0.5),
    angular_velocity(0.5)
{
    ui->setupUi(this);
    
    // 初始化ROS
    int argc = 0;
    char** argv = nullptr;
    ros::init(argc, argv, "ros_control_panel");
    nh = boost::make_shared<ros::NodeHandle>();
    
    // 创建发布者和订阅者
    cmd_vel_pub = nh->advertise<geometry_msgs::Twist>("cmd_vel", 10);
    laser_sub = nh->subscribe("scan", 10, &ROSControlPanel::laserCallback, this);
    odom_sub = nh->subscribe("odom", 10, &ROSControlPanel::odomCallback, this);
    reset_simulation_client = nh->serviceClient<std_srvs::Empty>("/gazebo/reset_simulation");
    
    // 设置定时器处理ROS回调
    ros_timer = new QTimer(this);
    connect(ros_timer, &QTimer::timeout, this, &ROSControlPanel::updateROS);
    ros_timer->start(100); // 100ms
    
    // 初始化UI状态
    ui->linearSlider->setValue(50);
    ui->angularSlider->setValue(50);
    ui->linearValueLabel->setText("0.5 m/s");
    ui->angularValueLabel->setText("0.5 rad/s");
}

ROSControlPanel::~ROSControlPanel() {
    delete ui;
}

void ROSControlPanel::on_forwardButton_clicked() {
    geometry_msgs::Twist cmd;
    cmd.linear.x = linear_velocity;
    cmd.angular.z = 0;
    cmd_vel_pub.publish(cmd);
}

void ROSControlPanel::on_backwardButton_clicked() {
    geometry_msgs::Twist cmd;
    cmd.linear.x = -linear_velocity;
    cmd.angular.z = 0;
    cmd_vel_pub.publish(cmd);
}

void ROSControlPanel::on_leftButton_clicked() {
    geometry_msgs::Twist cmd;
    cmd.linear.x = 0;
    cmd.angular.z = angular_velocity;
    cmd_vel_pub.publish(cmd);
}

void ROSControlPanel::on_rightButton_clicked() {
    geometry_msgs::Twist cmd;
    cmd.linear.x = 0;
    cmd.angular.z = -angular_velocity;
    cmd_vel_pub.publish(cmd);
}

void ROSControlPanel::on_stopButton_clicked() {
    geometry_msgs::Twist cmd;
    cmd.linear.x = 0;
    cmd.angular.z = 0;
    cmd_vel_pub.publish(cmd);
}

void ROSControlPanel::on_linearSlider_valueChanged(int value) {
    linear_velocity = value / 100.0;
    ui->linearValueLabel->setText(QString::number(linear_velocity, 'f', 2) + " m/s");
}

void ROSControlPanel::on_angularSlider_valueChanged(int value) {
    angular_velocity = value / 100.0;
    ui->angularValueLabel->setText(QString::number(angular_velocity, 'f', 2) + " rad/s");
}

void ROSControlPanel::updateROS() {
    ros::spinOnce();
}

void ROSControlPanel::laserCallback(const sensor_msgs::LaserScan::ConstPtr& msg) {
    // 更新激光数据显示
    if(!msg->ranges.empty()) {
        double min_range = *std::min_element(msg->ranges.begin(), msg->ranges.end());
        ui->minRangeLabel->setText(QString::number(min_range, 'f', 2) + " m");
        
        // 更新进度条
        int progress = std::min(100, static_cast<int>((min_range / 10.0) * 100));
        ui->safetyProgressBar->setValue(progress);
    }
}

void ROSControlPanel::odomCallback(const nav_msgs::Odometry::ConstPtr& msg) {
    // 更新位置信息
    double x = msg->pose.pose.position.x;
    double y = msg->pose.pose.position.y;
    
    ui->positionLabel->setText(QString("X: %1, Y: %2").arg(x, 0, 'f', 2).arg(y, 0, 'f', 2));
    
    // 更新速度信息
    double linear_speed = msg->twist.twist.linear.x;
    double angular_speed = msg->twist.twist.angular.z;
    
    ui->speedLabel->setText(QString("Linear: %1, Angular: %2")
                           .arg(linear_speed, 0, 'f', 2)
                           .arg(angular_speed, 0, 'f', 2));
}

基于rqt的插件开发：

#!/usr/bin/env python
# 文件名：custom_rqt_plugin.py
import rospy
import python_qt_binding.QtWidgets as QtWidgets
import python_qt_binding.QtCore as QtCore
import rqt_gui.py
from rqt_gui_py.plugin import Plugin

from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
import math

class CustomRQTPlugin(Plugin):
    def __init__(self, context):
        super(CustomRQTPlugin, self).__init__(context)
        self.setObjectName('CustomRQTPlugin')
        
        # 创建QWidget
        self._widget = QtWidgets.QWidget()
        self._widget.setObjectName('CustomRQTPluginUI')
        
        # 创建布局
        layout = QtWidgets.QVBoxLayout(self._widget)
        
        # 速度控制部分
        speed_group = QtWidgets.QGroupBox("速度控制")
        speed_layout = QtWidgets.QHBoxLayout()
        
        self.linear_slider = QtWidgets.QSlider(QtCore.Qt.Horizontal)
        self.linear_slider.setRange(0, 100)
        self.linear_slider.setValue(50)
        self.linear_label = QtWidgets.QLabel("0.5 m/s")
        
        self.angular_slider = QtWidgets.QSlider(QtCore.Qt.Horizontal)
        self.angular_slider.setRange(0, 100)
        self.angular_slider.setValue(50)
        self.angular_label = QtWidgets.QLabel("0.5 rad/s")
        
        speed_layout.addWidget(QtWidgets.QLabel("线速度:"))
        speed_layout.addWidget(self.linear_slider)
        speed_layout.addWidget(self.linear_label)
        speed_layout.addWidget(QtWidgets.QLabel("角速度:"))
        speed_layout.addWidget(self.angular_slider)
        speed_layout.addWidget(self.angular_label)
        
        speed_group.setLayout(speed_layout)
        layout.addWidget(speed_group)
        
        # 方向控制部分
        control_group = QtWidgets.QGroupBox("方向控制")
        control_layout = QtWidgets.QGridLayout()
        
        self.forward_btn = QtWidgets.QPushButton("前进")
        self.backward_btn = QtWidgets.QPushButton("后退")
        self.left_btn = QtWidgets.QPushButton("左转")
        self.right_btn = QtWidgets.QPushButton("右转")
        self.stop_btn = QtWidgets.QPushButton("停止")
        
        control_layout.addWidget(self.forward_btn, 0, 1)
        control_layout.addWidget(self.left_btn, 1, 0)
        control_layout.addWidget(self.stop_btn, 1, 1)
        control_layout.addWidget(self.right_btn, 1, 2)
        control_layout.addWidget(self.backward_btn, 2, 1)
        
        control_group.setLayout(control_layout)
        layout.addWidget(control_group)
        
        # 传感器数据显示部分
        sensor_group = QtWidgets.QGroupBox("传感器数据")
        sensor_layout = QtWidgets.QFormLayout()
        
        self.laser_label = QtWidgets.QLabel("等待数据...")
        self.odom_label = QtWidgets.QLabel("等待数据...")
        
        sensor_layout.addRow("最近障碍物:", self.laser_label)
        sensor_layout.addRow("当前位置:", self.odom_label)
        
        sensor_group.setLayout(sensor_layout)
        layout.addWidget(sensor_group)
        
        # 连接信号和槽
        self.linear_slider.valueChanged.connect(self.update_linear_velocity)
        self.angular_slider.valueChanged.connect(self.update_angular_velocity)
        
        self.forward_btn.clicked.connect(self.move_forward)
        self.backward_btn.clicked.connect(self.move_backward)
        self.left_btn.clicked.connect(self.turn_left)
        self.right_btn.clicked.connect(self.turn_right)
        self.stop_btn.clicked.connect(self.stop)
        
        # ROS初始化
        self.cmd_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        self.laser_sub = rospy.Subscriber('/scan', LaserScan, self.laser_callback)
        
        self.linear_vel = 0.5
        self.angular_vel = 0.5
        
        context.add_widget(self._widget)
    
    def update_linear_velocity(self, value):
        self.linear_vel = value / 100.0
        self.linear_label.setText(f"{self.linear_vel:.2f} m/s")
    
    def update_angular_velocity(self, value):
        self.angular_vel = value / 100.0
        self.angular_label.setText(f"{self.angular_vel:.2f} rad/s")
    
    def move_forward(self):
        twist = Twist()
        twist.linear.x = self.linear_vel
        self.cmd_pub.publish(twist)
    
    def move_backward(self):
        twist = Twist()
        twist.linear.x = -self.linear_vel
        self.cmd_pub.publish(twist)
    
    def turn_left(self):
        twist = Twist()
        twist.angular.z = self.angular_vel
        self.cmd_pub.publish(twist)
    
    def turn_right(self):
        twist = Twist()
        twist.angular.z = -self.angular_vel
        self.cmd_pub.publish(twist)
    
    def stop(self):
        twist = Twist()
        self.cmd_pub.publish(twist)
    
    def laser_callback(self, msg):
        if msg.ranges:
            min_range = min(msg.ranges)
            self.laser_label.setText(f"{min_range:.2f} m")
    
    def shutdown_plugin(self):
        self.laser_sub.unregister()
        self.cmd_pub.unregister()

if __name__ == '__main__':
    pass

3.9 ROS包管理与系统优化

包选择与依赖管理：

#!/usr/bin/env python
# 文件名：package_manager.py
import rospy
import os
import subprocess
import yaml
from catkin_pkg.packages import find_packages

class ROSPackageManager:
    def __init__(self):
        self.workspace_path = os.path.expanduser("~/catkin_ws")
        self.src_path = os.path.join(self.workspace_path, "src")
        
    def find_all_packages(self):
        """查找工作空间中的所有包"""
        packages = find_packages(self.src_path)
        return packages
    
    def analyze_package_dependencies(self, package_name):
        """分析包的依赖关系"""
        try:
            # 使用rosdep检查依赖
            result = subprocess.check_output(
                ['rosdep', 'check', package_name], 
                stderr=subprocess.STDOUT, 
                text=True
            )
            return result
        except subprocess.CalledProcessError as e:
            return f"依赖检查失败: {e.output}"
    
    def get_package_size(self, package_path):
        """获取包的大小"""
        total_size = 0
        for dirpath, dirnames, filenames in os.walk(package_path):
            for filename in filenames:
                filepath = os.path.join(dirpath, filename)
                total_size += os.path.getsize(filepath)
        return total_size
    
    def generate_dependency_graph(self):
        """生成依赖关系图"""
        packages = self.find_all_packages()
        
        dot_content = ['digraph ROS_Packages {', '  rankdir=TB;', '  node [shape=box];', '']
        
        for pkg_path, pkg in packages.items():
            dot_content.append(f'  "{pkg.name}"')
            
            # 添加依赖关系
            for dep in pkg.build_depends + pkg.run_depends:
                dot_content.append(f'  "{pkg.name}" -> "{dep.name}"')
        
        dot_content.append('}')
        
        dot_file = "/tmp/ros_packages.dot"
        with open(dot_file, 'w') as f:
            f.write('\n'.join(dot_content))
        
        print(f"依赖关系图已生成: {dot_file}")
        print("使用以下命令生成图片:")
        print(f"  dot -Tpng {dot_file} -o packages.png")
    
    def optimize_build(self, package_names=None):
        """优化构建过程"""
        cmd = ['catkin_make']
        
        if package_names:
            cmd.extend(['--only-pkg-with-deps'] + package_names)
        
        # 添加优化参数
        cmd.extend([
            '-DCMAKE_BUILD_TYPE=Release',
            '-DCMAKE_CXX_FLAGS="-O2 -march=native"',
            '--jobs', str(os.cpu_count())
        ])
        
        print(f"执行优化构建命令: {' '.join(cmd)}")
        
        try:
            subprocess.run(cmd, cwd=self.workspace_path, check=True)
            print("构建完成")
        except subprocess.CalledProcessError as e:
            print(f"构建失败: {e}")
    
    def clean_unused_packages(self):
        """清理未使用的包"""
        packages = self.find_all_packages()
        
        for pkg_path, pkg in packages.items():
            # 检查包是否最近被修改
            stat = os.stat(pkg_path)
            days_since_modification = (time.time() - stat.st_mtime) / (24 * 3600)
            
            if days_since_modification > 180:  # 超过6个月未修改
                print(f"建议清理包: {pkg.name} (最后修改: {days_since_modification:.0f} 天前)")
                
                # 检查是否有其他包依赖此包
                is_dependency = False
                for other_path, other_pkg in packages.items():
                    if pkg_path == other_path:
                        continue
                    
                    all_deps = other_pkg.build_depends + other_pkg.run_depends
                    if any(dep.name == pkg.name for dep in all_deps):
                        is_dependency = True
                        break
                
                if not is_dependency:
                    print(f"  可以安全删除: {pkg_path}")

def main():
    rospy.init_node('package_manager')
    
    manager = ROSPackageManager()
    
    print("=== ROS包管理工具 ===")
    
    # 显示所有包
    packages = manager.find_all_packages()
    print(f"\n找到 {len(packages)} 个包:")
    
    for pkg_path, pkg in packages.items():
        size_mb = manager.get_package_size(pkg_path) / (1024 * 1024)
        print(f"  {pkg.name:20} {size_mb:6.1f} MB")
    
    # 生成依赖图
    print("\n生成依赖关系图...")
    manager.generate_dependency_graph()
    
    # 优化建议
    print("\n优化建议:")
    manager.clean_unused_packages()

if __name__ == '__main__':
    main()

3.10 常用GUI工具快速参考手册

rqt工具集使用指南：

#!/usr/bin/env python
# 文件名：rqt_quick_reference.py
import rospy
import subprocess
import threading
import time

class RQTQuickReference:
    def __init__(self):
        self.rqt_processes = []
        
    def start_rqt_console(self):
        """启动rqt_console日志查看器"""
        proc = subprocess.Popen(['rqt_console'])
        self.rqt_processes.append(proc)
        print("rqt_console已启动 - 用于查看和过滤ROS日志")
        
    def start_rqt_graph(self):
        """启动rqt_graph节点关系图"""
        proc = subprocess.Popen(['rqt_graph'])
        self.rqt_processes.append(proc)
        print("rqt_graph已启动 - 用于可视化节点和主题通信")
        
    def start_rqt_plot(self):
        """启动rqt_plot数据绘图工具"""
        proc = subprocess.Popen(['rqt_plot'])
        self.rqt_processes.append(proc)
        print("rqt_plot已启动 - 用于绘制数值数据趋势图")
        
    def start_rqt_image_view(self):
        """启动rqt_image_view图像查看器"""
        proc = subprocess.Popen(['rqt_image_view'])
        self.rqt_processes.append(proc)
        print("rqt_image_view已启动 - 用于查看图像主题")
        
    def start_rqt_bag(self):
        """启动rqt_bag数据记录工具"""
        proc = subprocess.Popen(['rqt_bag'])
        self.rqt_processes.append(proc)
        print("rqt_bag已启动 - 用于记录和回放ROS数据")
        
    def start_rqt_reconfigure(self):
        """启动rqt_reconfigure动态参数配置"""
        proc = subprocess.Popen(['rqt_reconfigure'])
        self.rqt_processes.append(proc)
        print("rqt_reconfigure已启动 - 用于动态调整节点参数")
        
    def start_rviz(self):
        """启动RViz三维可视化工具"""
        proc = subprocess.Popen(['rviz'])
        self.rqt_processes.append(proc)
        print("RViz已启动 - 用于三维机器人可视化")
        
    def start_gazebo(self):
        """启动Gazebo仿真环境"""
        proc = subprocess.Popen(['gazebo'])
        self.rqt_processes.append(proc)
        print("Gazebo已启动 - 用于物理仿真")
        
    def start_plotjuggler(self):
        """启动PlotJuggler高级数据可视化"""
        try:
            proc = subprocess.Popen(['plotjuggler'])
            self.rqt_processes.append(proc)
            print("PlotJuggler已启动 - 用于高级时间序列数据分析")
        except FileNotFoundError:
            print("PlotJuggler未安装，请使用: sudo apt install plotjuggler")
            
    def show_usage_guide(self):
        """显示使用指南"""
        guide = """
        ROS GUI工具快速参考:
        
        1. rqt_console    - 日志查看和过滤
           * 快捷键: Ctrl+F 搜索, Ctrl+R 清除
           
        2. rqt_graph      - 系统拓扑可视化  
           * 显示节点、主题、服务的关系
           * 绿色: 节点, 蓝色: 主题, 红色: 服务
           
        3. rqt_plot       - 数据绘图
           * 添加主题: 点击 '+' 按钮
           * 格式: /topic/field (如: /cmd_vel/linear/x)
           
        4. rqt_image_view - 图像查看
           * 下拉菜单选择图像主题
           * 右键保存当前帧
           
        5. rqt_bag        - 数据记录
           * 记录: 点击红色录制按钮
           * 回放: 加载bag文件并使用时间轴
           
        6. rqt_reconfigure - 动态参数
           * 实时调整节点参数
           * 支持整数、浮点数、布尔值、字符串
           
        7. RViz           - 三维可视化
           * 添加显示: 左下角 'Add' 按钮
           * 常用显示: Grid, RobotModel, LaserScan, TF
           
        8. Gazebo         - 物理仿真
           * 插入模型: 左上角 'Insert' 标签
           * 运行仿真: 底部播放按钮
           
        9. PlotJuggler    - 高级绘图
           * 拖放主题到绘图区域
           * 支持数据变换和数学运算
        """
        print(guide)
        
    def stop_all_tools(self):
        """停止所有GUI工具"""
        for proc in self.rqt_processes:
            proc.terminate()
        self.rqt_processes.clear()
        print("所有GUI工具已停止")

def main():
    rospy.init_node('rqt_quick_reference')
    
    reference = RQTQuickReference()
    
    print("ROS GUI工具快速启动器")
    print("=" * 50)
    
    # 显示使用指南
    reference.show_usage_guide()
    
    # 交互式启动工具
    while True:
        print("\n选择要启动的工具:")
        print("1. rqt_console     2. rqt_graph     3. rqt_plot")
        print("4. rqt_image_view  5. rqt_bag       6. rqt_reconfigure") 
        print("7. RViz            8. Gazebo        9. PlotJuggler")
        print("a. 启动所有工具    s. 显示指南      q. 退出")
        
        choice = input("请输入选择: ").strip().lower()
        
        if choice == '1':
            reference.start_rqt_console()
        elif choice == '2':
            reference.start_rqt_graph()
        elif choice == '3':
            reference.start_rqt_plot()
        elif choice == '4':
            reference.start_rqt_image_view()
        elif choice == '5':
            reference.start_rqt_bag()
        elif choice == '6':
            reference.start_rqt_reconfigure()
        elif choice == '7':
            reference.start_rviz()
        elif choice == '8':
            reference.start_gazebo()
        elif choice == '9':
            reference.start_plotjuggler()
        elif choice == 'a':
            reference.start_rqt_console()
            time.sleep(1)
            reference.start_rqt_graph()
            time.sleep(1)
            reference.start_rqt_plot()
            time.sleep(1)
            reference.start_rqt_image_view()
            print("所有工具启动完成")
        elif choice == 's':
            reference.show_usage_guide()
        elif choice == 'q':
            reference.stop_all_tools()
            break
        else:
            print("无效选择，请重新输入")

if __name__ == '__main__':
    try:
        main()
    except KeyboardInterrupt:
        print("\n程序已退出")

通过本章的深入学习，读者将掌握ROS可视化工具链的完整使用方法。从基础的日志查看和数据绘图，到复杂的RViz三维可视化和Gazebo物理仿真，这些工具为机器人系统的开发、调试和部署提供了强大的支持。合理运用这些可视化工具，可以显著提高机器人开发的效率和质量。