ROS实践（一）虚拟仿真平台Stage/Gazebo（虚实结合）

原创

已于 2025-03-13 19:27:36 修改 · 1.4k 阅读

8 ·

CC 4.0 BY-SA版权

文章标签：

#ROS #stage仿真 #虚拟地图 #Gazebo

于 2025-03-07 17:48:04 首次发布

（3）功能3：在rviz中显示机器人运动轨迹。

4. 使用launch文件启动gazebo，加载世界文件。

5. 建立简单机器人模型

6. 将简单机器人模型导入我们的world中

一、两者区别

Stage 和 Gazebo 都是 ROS 生态系统中的仿真工具，但它们的功能侧重点不同。

Stage：轻量级的 2D 仿真工具，适用于大规模机器人群体仿真。

Gazebo：功能更强大的 3D 物理仿真环境，支持复杂物理交互、传感器建模和机器人动力学仿真。

二、Stage仿真

1. 安装stage仿真软件

使用命令：sudo apt-get install ros-noetic-stage-ros来安装。这个平台中有一些自带的.world地图文件，我们可以加载它自带的地图，也可以自己来编写地图文件。

2. 编写控制代码

我们通过编写的代码，可以控制小车在 Stage 仿真平台或真实小车上的运动。代码通过发布速度指令（Twist 消息）到 /cmd_vel 话题，来驱动小车的运动。小车的地盘运动节点会订阅这个话题，并解析收到的速度数据，然后控制电机转动，从而使小车按照指令移动。

在 ROS 中，机器人控制的基本原理是：通过发布速度指令到 /cmd_vel 话题，机器人控制器（如 Stage 仿真平台或真实的小车）接收并解析这些指令，进而控制小车的运动。这使得我们既可以在仿真环境中测试小车，也可以在实际的小车上执行相同的操作，达到预期的控制效果。

（1）功能1：控制机器人走圆形路线。

#!/usr/bin/python
import rospy
from geometry_msgs.msg import Twist

def cmd_velmodule():
    # 初始化ROS节点，节点名称为 'circle_drawer'，anonymous=False 使其名称唯一
    rospy.init_node('circle_drawer', anonymous=False)  
    # 创建一个Publisher，发布消息到 /cmd_vel 话题，消息类型为 Twist，队列大小为10
    cmd_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)  
    # 设置发布频率为 10Hz
    rate = rospy.Rate(10)  
    # 创建一个 Twist 消息对象，用于存储速度指令
    twist = Twist()
    rospy.loginfo("Start controlling robot to draw a circle")  # 记录日志信息
    # 进入主循环，持续发布速度指令，直到节点被终止
    while not rospy.is_shutdown():
        twist.linear.x = 0.2  # 设置线速度，单位：m/s
        twist.angular.z = 0.4  # 设置角速度，单位：rad/s（正值表示逆时针旋转）
        cmd_pub.publish(twist)  # 发布速度指令到 /cmd_vel 话题
        rate.sleep()  # 休眠以保持 10Hz 的循环频率
if __name__ == "__main__":
    try:
        cmd_velmodule()  # 运行控制函数
    except rospy.ROSInterruptException:
        pass  # 异常处理，当节点被中断时，安全退出

启动主节点，使用命令运行虚拟地图：rosrun stage_ros stageros $(rospack find stage_ros)/world/willow-erratic.world ，地图文件可以自己来写。

地图运行完毕后，运行上面我们自己编写的代码文件，就可以看到小车在按照我们编程程序的轨迹在虚拟地图上运行（按照圆形轨迹运动）。

（2）功能2：小车前进1m，后退1m。

#!/usr/bin/env python3
import rospy
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
import math

# 全局变量
current_position = None  # 机器人当前位置信息
target_distance = 1.0  # 目标前进距离（单位：米）
min_velocity = 0.1  # 机器人最小速度
max_velocity = 0.5  # 机器人最大速度
forward_flag = True  # 控制前进/后退方向

def get_odom(data):
    """
    订阅里程计数据回调函数
    获取机器人当前位置并更新全局变量 current_position
    """
    global current_position
    current_position = data.pose.pose.position

def cmd_vel_pub():
    """
    机器人沿直线路径前进一定距离，然后反向移动，实现往返运动
    """
    global target_distance, forward_flag, current_position
    rospy.init_node('line_loop', anonymous=False)    # 初始化 ROS 节点
    cmd_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)    # 创建 cmd_vel 话题的发布者，控制机器人速度
    rospy.Subscriber('/odom', Odometry, get_odom)    # 订阅 odom 话题，获取里程计数据
    rate = rospy.Rate(10)    # 设定循环频率 10Hz
    twist = Twist()    # 速度控制消息
    rospy.loginfo('Start controlling the robot to move in a line loop')
    while not rospy.is_shutdown():
        # 等待里程计数据更新
        if current_position is None:
            rospy.logwarn("Waiting for odometry data...")
            rate.sleep()
            continue
        start_position = current_position   # 记录初始位置
        while True:
            gap= target_distance - math.sqrt(     # 计算当前距离误差
                (current_position.x - start_position.x) ** 2 + 
                (current_position.y - start_position.y) ** 2
            )
            if gap > 0:
                twist.linear.x = math.sin(gap * math.pi)  # 计算前进速度（使用正弦函数调整平滑加速）
                # 限制速度范围，确保在 min_velocity ~ ma