Python自动化控制ROS命令与rosbag播放实践

1. 项目背景与需求解析

在机器人开发领域，Ubuntu 18.04 + ROS Melodic是经典的开发环境组合。实际开发中经常遇到这样的需求：需要自动执行一系列ROS相关命令，包括启动launch文件和播放bag数据包。传统做法是手动逐个执行命令，但存在以下痛点：

• 多终端窗口管理繁琐
• 命令执行顺序容易出错
• 参数调整后需要重复操作
• 难以集成到自动化测试流程

这正是我们需要通过Python脚本实现自动化操作的核心原因。通过脚本控制，可以实现：

1. 精确控制命令执行顺序
2. 方便调整参数和配置
3. 可集成到CI/CD流程
4. 一键式操作提升效率

2. 环境准备与依赖安装

2.1 基础环境配置

确保已安装以下组件：

• Ubuntu 18.04 LTS
• ROS Melodic（完整桌面版）
• Python 2.7（ROS Melodic默认版本）

验证ROS环境：

bash复制source /opt/ros/melodic/setup.bash
roscore

2.2 Python依赖库安装

需要以下Python库支持：

bash复制pip install rospkg
pip install subprocess32  # Python2环境下更稳定的subprocess实现

注意：如果使用Python3环境，需要确保ROS环境兼容性，可能需要额外配置

3. 核心代码实现

3.1 命令执行模块设计

创建command_executor.py作为基础模块：

python复制import subprocess
import time
import rospkg

class ROSCommandExecutor:
    def __init__(self):
        self.processes = []
        
    def execute(self, cmd, shell=True):
        """执行shell命令并返回进程对象"""
        p = subprocess.Popen(cmd, shell=shell)
        self.processes.append(p)
        return p
    
    def terminate_all(self):
        """终止所有启动的进程"""
        for p in self.processes:
            p.terminate()

3.2 launch文件启动实现

扩展实现launch文件启动功能：

python复制def launch_ros_package(package_name, launch_file, args=""):
    """
    启动ROS launch文件
    :param package_name: ROS包名
    :param launch_file: launch文件名（不带.launch后缀）
    :param args: 附加参数
    """
    rospack = rospkg.RosPack()
    try:
        pkg_path = rospack.get_path(package_name)
        launch_path = f"{pkg_path}/launch/{launch_file}.launch"
        cmd = f"roslaunch {package_name} {launch_file}.launch {args}"
        return execute_command(cmd)
    except rospkg.common.ResourceNotFound:
        print(f"Package [{package_name}] not found")
        return None

3.3 rosbag播放控制

实现bag文件播放控制：

python复制def play_rosbag(bag_path, rate=1.0, loop=False, topics=[]):
    """
    播放ROS bag文件
    :param bag_path: bag文件路径
    :param rate: 播放速率
    :param loop: 是否循环播放
    :param topics: 指定播放的topic列表
    """
    cmd = f"rosbag play {bag_path} --rate={rate}"
    if loop:
        cmd += " -l"
    if topics:
        cmd += " " + " ".join([f"-t {t}" for t in topics])
    return execute_command(cmd)

4. 完整流程集成

4.1 主控脚本实现

创建main.py作为主入口：

python复制#!/usr/bin/env python
import os
import time
from command_executor import ROSCommandExecutor

def main():
    executor = ROSCommandExecutor()
    
    try:
        # 启动roscore
        executor.execute("roscore")
        time.sleep(3)  # 等待roscore完全启动
        
        # 启动launch文件
        launch_proc = executor.launch_ros_package("my_pkg", "my_launch", "arg1:=value1")
        
        # 播放bag文件
        bag_proc = executor.play_rosbag("/path/to/bagfile.bag", rate=1.5)
        
        # 主循环
        while True:
            time.sleep(1)
            
    except KeyboardInterrupt:
        print("\nShutting down...")
        executor.terminate_all()

if __name__ == "__main__":
    main()

4.2 执行权限与启动方式

设置脚本可执行权限：

bash复制chmod +x main.py

推荐通过shell脚本封装环境初始化：

bash复制#!/bin/bash
source /opt/ros/melodic/setup.bash
source ~/catkin_ws/devel/setup.bash
python main.py

5. 高级功能扩展

5.1 进程状态监控

增强进程管理功能：

python复制def check_process_alive(process):
    """检查进程是否仍在运行"""
    return process.poll() is None

def monitor_processes(processes, interval=1.0):
    """监控进程状态"""
    while True:
        all_alive = all(check_process_alive(p) for p in processes)
        if not all_alive:
            print("Some processes have terminated")
            break
        time.sleep(interval)

5.2 参数化配置

支持YAML配置文件：

python复制import yaml

def load_config(config_path):
    with open(config_path) as f:
        return yaml.safe_load(f)

# 配置示例config.yaml
"""
launch:
  package: my_pkg
  file: my_launch
  args: "arg1:=value1 arg2:=value2"
rosbag:
  path: "/path/to/bagfile.bag"
  rate: 1.5
  loop: false
"""

6. 常见问题与解决方案

6.1 环境变量问题

问题现象：

code复制[roslaunch] ROS_MASTER_URI is not defined

解决方案：

• 确保脚本中source了ROS环境
• 或者在Python中显式设置：

python复制os.environ['ROS_MASTER_URI'] = 'http://localhost:11311'

6.2 进程终止不完全

问题现象：

• Ctrl+C后仍有ROS节点在后台运行

解决方案：

• 增强终止逻辑：

python复制def terminate_all(self):
    for p in self.processes:
        try:
            p.terminate()
            p.wait(timeout=5)
        except subprocess.TimeoutExpired:
            p.kill()
    os.system("pkill -f roslaunch")
    os.system("pkill -f rosbag")

6.3 时间同步问题

问题现象：

• bag播放与系统时间不同步

解决方案：

• 使用--clock参数：

python复制cmd = f"rosbag play {bag_path} --clock --rate={rate}"

7. 性能优化建议

1. 并行启动优化：

python复制from threading import Thread

def parallel_launch():
    t1 = Thread(target=launch_ros_package, args=("pkg1", "file1"))
    t2 = Thread(target=play_rosbag, args=("/path/to/bag"))
    t1.start()
    t2.start()

2. 启动顺序控制：

python复制def wait_for_topic(topic, timeout=10):
    """等待指定topic出现"""
    start = time.time()
    while time.time() - start < timeout:
        if topic in get_active_topics():
            return True
        time.sleep(0.1)
    return False

3. 资源监控：

python复制def monitor_system():
    """监控系统资源使用情况"""
    while True:
        cpu = psutil.cpu_percent()
        mem = psutil.virtual_memory().percent
        print(f"CPU: {cpu}%, MEM: {mem}%")
        if cpu > 90 or mem > 90:
            print("Warning: high resource usage")
        time.sleep(1)

8. 实际应用案例

8.1 自动化测试场景

python复制def run_test_sequence():
    # 1. 启动被测系统
    launch_proc = launch_ros_package("sut", "test_launch")
    
    # 2. 播放测试数据
    bag_proc = play_rosbag("test_cases/tc1.bag")
    
    # 3. 启动测试监控
    monitor_proc = execute_command("rostest monitor_test.test")
    
    # 4. 等待测试完成
    monitor_proc.wait()
    
    # 5. 收集测试结果
    collect_results()

8.2 数据回放分析场景

python复制def analyze_bag_data():
    # 1. 启动分析工具
    launch_proc = launch_ros_package("analysis", "visualizer")
    
    # 2. 按不同速率播放数据
    for rate in [0.5, 1.0, 2.0]:
        bag_proc = play_rosbag("data/experiment1.bag", rate=rate)
        bag_proc.wait()
        
    # 3. 生成分析报告
    generate_report()

9. 工程化建议

1. 日志记录增强：

python复制import logging

logging.basicConfig(
    level=logging.INFO,
    format='%(asctime)s - %(name)s - %(levelname)s - %(message)s',
    filename='ros_automation.log'
)

def execute_with_logging(cmd):
    logger = logging.getLogger('CMD_EXEC')
    logger.info(f"Executing: {cmd}")
    p = subprocess.Popen(
        cmd,
        shell=True,
        stdout=subprocess.PIPE,
        stderr=subprocess.PIPE
    )
    stdout, stderr = p.communicate()
    if stdout: logger.info(stdout.decode())
    if stderr: logger.error(stderr.decode())
    return p

2. 异常处理框架：

python复制class ROSAutomationError(Exception):
    pass

def safe_execute(cmd):
    try:
        p = execute_command(cmd)
        if p.returncode != 0:
            raise ROSAutomationError(f"Command failed: {cmd}")
        return p
    except subprocess.SubprocessError as e:
        raise ROSAutomationError(f"Execution error: {str(e)}")

3. 配置管理：

python复制class ConfigManager:
    def __init__(self):
        self.config = {
            'ros': {
                'master_uri': 'http://localhost:11311',
                'default_pkg': 'my_pkg'
            },
            'paths': {
                'bags': './bags',
                'launches': './launches'
            }
        }
    
    def get_bag_path(self, name):
        return os.path.join(self.config['paths']['bags'], name)

10. 容器化部署方案

对于需要可移植性的场景，可以考虑Docker容器化：

Dockerfile示例：

dockerfile复制FROM ros:melodic

# 安装依赖
RUN apt-get update && \
    apt-get install -y python-pip ros-melodic-rosbag && \
    pip install rospkg subprocess32

# 拷贝代码
WORKDIR /app
COPY . .

# 设置入口点
CMD ["python", "main.py"]

构建和运行：

bash复制docker build -t ros-automation .
docker run -it --rm ros-automation