【ROS1从入门到精通】第11篇：Gazebo仿真（打造虚拟机器人世界）
🎯 本文目标：深入学习Gazebo物理仿真环境，掌握机器人模型导入、传感器仿真、环境建模、插件开发等技术，能够构建完整的机器人仿真系统，实现在虚拟环境中测试和验证机器人算法。
📑 目录
- Gazebo仿真概述
- Gazebo环境搭建
- URDF到SDF转换
- 传感器仿真
- 物理引擎配置
- 环境建模
- Gazebo插件开发
- ros_control集成
- 仿真调试与优化
- 实战案例：完整仿真系统
- 总结与最佳实践
1. Gazebo仿真概述
1.1 什么是Gazebo？
Gazebo是一个开源的3D机器人仿真器，提供了：
- 物理仿真：基于ODE、Bullet等物理引擎
- 传感器仿真：相机、激光、IMU、GPS等
- 环境建模：室内外场景构建
- 机器人控制：与ROS深度集成
- 可视化界面：3D渲染和交互
Gazebo架构 Gazebo Server Physics Engine Sensor System Rendering Engine ODE Bullet DART Simbody Camera LiDAR IMU GPS OGRE OptiX ROS Interface Topics Services Plugins
1.2 Gazebo与ROS集成
python
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Gazebo与ROS集成基础
"""
import rospy
from gazebo_msgs.msg import ModelStates, LinkStates
from gazebo_msgs.srv import (
SpawnModel, DeleteModel, GetModelState,
SetModelState, GetWorldProperties,
ApplyBodyWrench, ApplyJointEffort
)
from geometry_msgs.msg import Pose, Twist, Wrench
import tf
class GazeboROSInterface:
"""Gazebo ROS接口"""
def __init__(self):
rospy.init_node('gazebo_ros_interface')
# 订阅Gazebo状态
self.model_states_sub = rospy.Subscriber(
'/gazebo/model_states', ModelStates, self.model_states_callback)
self.link_states_sub = rospy.Subscriber(
'/gazebo/link_states', LinkStates, self.link_states_callback)
# Gazebo服务
self.spawn_model_srv = rospy.ServiceProxy(
'/gazebo/spawn_urdf_model', SpawnModel)
self.delete_model_srv = rospy.ServiceProxy(
'/gazebo/delete_model', DeleteModel)
self.get_model_state_srv = rospy.ServiceProxy(
'/gazebo/get_model_state', GetModelState)
self.set_model_state_srv = rospy.ServiceProxy(
'/gazebo/set_model_state', SetModelState)
# 等待服务可用
rospy.wait_for_service('/gazebo/spawn_urdf_model')
# 存储状态
self.models = {}
self.links = {}
rospy.loginfo("Gazebo ROS interface initialized")
def model_states_callback(self, msg):
"""模型状态回调"""
for i, name in enumerate(msg.name):
self.models[name] = {
'pose': msg.pose[i],
'twist': msg.twist[i]
}
def link_states_callback(self, msg):
"""连杆状态回调"""
for i, name in enumerate(msg.name):
self.links[name] = {
'pose': msg.pose[i],
'twist': msg.twist[i]
}
def spawn_model(self, model_name, model_xml, initial_pose=None,
reference_frame="world"):
"""生成模型"""
if initial_pose is None:
initial_pose = Pose()
initial_pose.orientation.w = 1.0
try:
resp = self.spawn_model_srv(
model_name=model_name,
model_xml=model_xml,
robot_namespace="",
initial_pose=initial_pose,
reference_frame=reference_frame
)
if resp.success:
rospy.loginfo(f"Successfully spawned model: {model_name}")
else:
rospy.logerr(f"Failed to spawn model: {resp.status_message}")
return resp.success
except rospy.ServiceException as e:
rospy.logerr(f"Service call failed: {e}")
return False
def delete_model(self, model_name):
"""删除模型"""
try:
resp = self.delete_model_srv(model_name=model_name)
if resp.success:
rospy.loginfo(f"Successfully deleted model: {model_name}")
else:
rospy.logerr(f"Failed to delete model: {resp.status_message}")
return resp.success
except rospy.ServiceException as e:
rospy.logerr(f"Service call failed: {e}")
return False
def get_model_pose(self, model_name, reference_frame="world"):
"""获取模型位姿"""
try:
resp = self.get_model_state_srv(
model_name=model_name,
relative_entity_name=reference_frame
)
if resp.success:
return resp.pose
else:
rospy.logwarn(f"Failed to get model state: {resp.status_message}")
return None
except rospy.ServiceException as e:
rospy.logerr(f"Service call failed: {e}")
return None
def set_model_pose(self, model_name, pose, twist=None,
reference_frame="world"):
"""设置模型位姿"""
from gazebo_msgs.msg import ModelState
model_state = ModelState()
model_state.model_name = model_name
model_state.pose = pose
model_state.twist = twist if twist else Twist()
model_state.reference_frame = reference_frame
try:
resp = self.set_model_state_srv(model_state=model_state)
if resp.success:
rospy.loginfo(f"Successfully set model state: {model_name}")
else:
rospy.logerr(f"Failed to set model state: {resp.status_message}")
return resp.success
except rospy.ServiceException as e:
rospy.logerr(f"Service call failed: {e}")
return False
def apply_force(self, body_name, force, torque=None, duration=None):
"""施加力和力矩"""
wrench = Wrench()
wrench.force.x = force[0]
wrench.force.y = force[1]
wrench.force.z = force[2]
if torque:
wrench.torque.x = torque[0]
wrench.torque.y = torque[1]
wrench.torque.z = torque[2]
try:
apply_wrench_srv = rospy.ServiceProxy(
'/gazebo/apply_body_wrench', ApplyBodyWrench)
resp = apply_wrench_srv(
body_name=body_name,
reference_frame="world",
reference_point=Point(),
wrench=wrench,
start_time=rospy.Time(),
duration=rospy.Duration(duration) if duration else rospy.Duration(-1)
)
return resp.success
except rospy.ServiceException as e:
rospy.logerr(f"Service call failed: {e}")
return False2. Gazebo环境搭建
2.1 安装和配置
bash
# 安装Gazebo（ROS Noetic默认Gazebo 11）
sudo apt-get install ros-noetic-gazebo-ros-pkgs ros-noetic-gazebo-ros-control
# 设置Gazebo模型路径
echo "export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:~/gazebo_models" >> ~/.bashrc
echo "export GAZEBO_RESOURCE_PATH=$GAZEBO_RESOURCE_PATH:~/gazebo_worlds" >> ~/.bashrc
source ~/.bashrc
# 下载模型库
cd ~
git clone https://github.com/osrf/gazebo_models.git
# 测试Gazebo
roslaunch gazebo_ros empty_world.launch2.2 Launch文件配置
xml
<!-- launch/gazebo_simulation.launch -->
<launch>
<!-- Gazebo参数 -->
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<arg name="world_name" default="$(find my_robot_gazebo)/worlds/my_world.world"/>
<!-- 启动Gazebo -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(arg world_name)"/>
<arg name="debug" value="$(arg debug)"/>
<arg name="gui" value="$(arg gui)"/>
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="headless" value="$(arg headless)"/>
</include>
<!-- 加载机器人描述 -->
<param name="robot_description"
command="$(find xacro)/xacro $(find my_robot_description)/urdf/robot.urdf.xacro"/>
<!-- 生成机器人 -->
<node name="spawn_robot" pkg="gazebo_ros" type="spawn_model"
args="-urdf -param robot_description -model my_robot -x 0 -y 0 -z 0.5"
respawn="false" output="screen"/>
<!-- Robot State Publisher -->
<node name="robot_state_publisher" pkg="robot_state_publisher"
type="robot_state_publisher">
<param name="publish_frequency" value="50"/>
</node>
<!-- Joint State Publisher -->
<node name="joint_state_publisher" pkg="joint_state_publisher"
type="joint_state_publisher">
<param name="use_gui" value="false"/>
</node>
</launch>2.3 Gazebo配置管理
python
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Gazebo配置管理器
"""
import os
import yaml
import xml.etree.ElementTree as ET
class GazeboConfig:
"""Gazebo配置管理"""
def __init__(self):
self.physics_engines = {
'ode': {
'type': 'ode',
'max_step_size': 0.001,
'real_time_factor': 1.0,
'real_time_update_rate': 1000,
'gravity': [0, 0, -9.8]
},
'bullet': {
'type': 'bullet',
'max_step_size': 0.001,
'real_time_factor': 1.0,
'real_time_update_rate': 1000,
'gravity': [0, 0, -9.8],
'bullet': {
'solver': {
'type': 'sequential_impulse',
'iters': 50,
'sor': 1.3
}
}
}
}
def create_world_file(self, world_name, config):
"""创建世界文件"""
world = ET.Element('sdf', version='1.6')
world_elem = ET.SubElement(world, 'world', name=world_name)
# 添加物理引擎配置
physics = ET.SubElement(world_elem, 'physics',
name='default_physics',
type=config.get('physics_engine', 'ode'))
ET.SubElement(physics, 'max_step_size').text = str(
config.get('max_step_size', 0.001))
ET.SubElement(physics, 'real_time_factor').text = str(
config.get('real_time_factor', 1.0))
ET.SubElement(physics, 'real_time_update_rate').text = str(
config.get('real_time_update_rate', 1000))
gravity = ET.SubElement(physics, 'gravity')
g = config.get('gravity', [0, 0, -9.8])
gravity.text = f"{g[0]} {g[1]} {g[2]}"
# 添加场景
scene = ET.SubElement(world_elem, 'scene')
ET.SubElement(scene, 'ambient').text = '0.4 0.4 0.4 1'
ET.SubElement(scene, 'background').text = '0.7 0.7 0.7 1'
ET.SubElement(scene, 'shadows').text = '1'
# 添加光源
self.add_light(world_elem, 'sun', 'directional',
[0, 0, 10], [0.8, 0.8, 0.8, 1])
# 添加地面
self.add_ground(world_elem)
return world
def add_light(self, parent, name, light_type, pose, diffuse):
"""添加光源"""
light = ET.SubElement(parent, 'light', name=name, type=light_type)
pose_elem = ET.SubElement(light, 'pose')
pose_elem.text = f"{pose[0]} {pose[1]} {pose[2]} 0 0 0"
diffuse_elem = ET.SubElement(light, 'diffuse')
diffuse_elem.text = f"{diffuse[0]} {diffuse[1]} {diffuse[2]} {diffuse[3]}"
ET.SubElement(light, 'cast_shadows').text = '1'
def add_ground(self, parent):
"""添加地面"""
model = ET.SubElement(parent, 'model', name='ground_plane')
ET.SubElement(model, 'static').text = 'true'
link = ET.SubElement(model, 'link', name='link')
# 碰撞
collision = ET.SubElement(link, 'collision', name='collision')
geom = ET.SubElement(collision, 'geometry')
plane = ET.SubElement(geom, 'plane')
ET.SubElement(plane, 'normal').text = '0 0 1'
ET.SubElement(plane, 'size').text = '100 100'
# 视觉
visual = ET.SubElement(link, 'visual', name='visual')
geom = ET.SubElement(visual, 'geometry')
plane = ET.SubElement(geom, 'plane')
ET.SubElement(plane, 'normal').text = '0 0 1'
ET.SubElement(plane, 'size').text = '100 100'
material = ET.SubElement(visual, 'material')
script = ET.SubElement(material, 'script')
ET.SubElement(script, 'uri').text = 'file://media/materials/scripts/gazebo.material'
ET.SubElement(script, 'name').text = 'Gazebo/Grey'
def save_world(self, world, filename):
"""保存世界文件"""
tree = ET.ElementTree(world)
tree.write(filename, encoding='utf-8', xml_declaration=True)
print(f"World file saved to: {filename}")3. URDF到SDF转换
3.1 Gazebo标签扩展
xml
<!-- URDF with Gazebo tags -->
<robot name="gazebo_robot">
<!-- Link Gazebo properties -->
<gazebo reference="base_link">
<material>Gazebo/Blue</material>
<gravity>true</gravity>
<self_collide>false</self_collide>
<mu1>1.0</mu1>
<mu2>1.0</mu2>
<kp>1e6</kp>
<kd>1.0</kd>
</gazebo>
<!-- Joint Gazebo properties -->
<gazebo reference="joint1">
<provideFeedback>true</provideFeedback>
<implicitSpringDamper>true</implicitSpringDamper>
<springStiffness>0.0</springStiffness>
<springReference>0.0</springReference>
<stopCfm>0.0</stopCfm>
<stopErp>0.2</stopErp>
</gazebo>
<!-- Transmission for ros_control -->
<transmission name="joint1_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint1">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="joint1_motor">
<mechanicalReduction>1</mechanicalReduction>
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</actuator>
</transmission>
</robot>3.2 Gazebo属性生成器
python
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Gazebo属性生成器
"""
import xml.etree.ElementTree as ET
class GazeboTags:
"""Gazebo标签生成器"""
# Gazebo材质列表
GAZEBO_MATERIALS = [
'Gazebo/White', 'Gazebo/Grey', 'Gazebo/Black',
'Gazebo/Red', 'Gazebo/Green', 'Gazebo/Blue',
'Gazebo/Yellow', 'Gazebo/Orange', 'Gazebo/Purple',
'Gazebo/Turquoise', 'Gazebo/Wood', 'Gazebo/WoodFloor',
'Gazebo/Bricks', 'Gazebo/Grass', 'Gazebo/Rocky'
]
def __init__(self):
self.gazebo_tags = []
def add_link_properties(self, link_name, properties):
"""添加连杆属性"""
gazebo = ET.Element('gazebo', reference=link_name)
# 材质
if 'material' in properties:
ET.SubElement(gazebo, 'material').text = properties['material']
# 重力
if 'gravity' in properties:
ET.SubElement(gazebo, 'gravity').text = str(properties['gravity']).lower()
# 摩擦系数
if 'mu1' in properties or 'mu2' in properties:
surface = ET.SubElement(gazebo, 'surface')
friction = ET.SubElement(surface, 'friction')
ode = ET.SubElement(friction, 'ode')
if 'mu1' in properties:
ET.SubElement(ode, 'mu').text = str(properties['mu1'])
if 'mu2' in properties:
ET.SubElement(ode, 'mu2').text = str(properties['mu2'])
# 接触系数
if 'kp' in properties or 'kd' in properties:
if 'surface' not in [child.tag for child in gazebo]:
surface = ET.SubElement(gazebo, 'surface')
else:
surface = gazebo.find('surface')
contact = ET.SubElement(surface, 'contact')
ode_contact = ET.SubElement(contact, 'ode')
if 'kp' in properties:
ET.SubElement(ode_contact, 'kp').text = str(properties['kp'])
if 'kd' in properties:
ET.SubElement(ode_contact, 'kd').text = str(properties['kd'])
# 自碰撞
if 'self_collide' in properties:
ET.SubElement(gazebo, 'self_collide').text = str(
properties['self_collide']).lower()
self.gazebo_tags.append(gazebo)
return gazebo
def add_joint_properties(self, joint_name, properties):
"""添加关节属性"""
gazebo = ET.Element('gazebo', reference=joint_name)
# 反馈
if 'provide_feedback' in properties:
ET.SubElement(gazebo, 'provideFeedback').text = str(
properties['provide_feedback']).lower()
# 弹簧阻尼
if 'implicit_spring_damper' in properties:
ET.SubElement(gazebo, 'implicitSpringDamper').text = str(
properties['implicit_spring_damper']).lower()
# 弹簧参数
if 'spring_stiffness' in properties:
ET.SubElement(gazebo, 'springStiffness').text = str(
properties['spring_stiffness'])
if 'spring_reference' in properties:
ET.SubElement(gazebo, 'springReference').text = str(
properties['spring_reference'])
# 停止参数
if 'stop_cfm' in properties:
ET.SubElement(gazebo, 'stopCfm').text = str(properties['stop_cfm'])
if 'stop_erp' in properties:
ET.SubElement(gazebo, 'stopErp').text = str(properties['stop_erp'])
self.gazebo_tags.append(gazebo)
return gazebo
def add_transmission(self, joint_name, actuator_name=None,
transmission_type='SimpleTransmission',
hardware_interface='EffortJointInterface',
mechanical_reduction=1.0):
"""添加传动装置（用于ros_control）"""
transmission = ET.Element('transmission', name=f"{joint_name}_trans")
# 传动类型
ET.SubElement(transmission, 'type').text = f"transmission_interface/{transmission_type}"
# 关节
joint = ET.SubElement(transmission, 'joint', name=joint_name)
ET.SubElement(joint, 'hardwareInterface').text = f"hardware_interface/{hardware_interface}"
# 执行器
actuator_name = actuator_name or f"{joint_name}_motor"
actuator = ET.SubElement(transmission, 'actuator', name=actuator_name)
ET.SubElement(actuator, 'mechanicalReduction').text = str(mechanical_reduction)
ET.SubElement(actuator, 'hardwareInterface').text = f"hardware_interface/{hardware_interface}"
return transmission
def generate_gazebo_urdf(self, urdf_file, output_file):
"""生成带Gazebo标签的URDF"""
tree = ET.parse(urdf_file)
root = tree.getroot()
# 添加Gazebo标签
for tag in self.gazebo_tags:
root.append(tag)
# 保存文件
tree.write(output_file, encoding='utf-8', xml_declaration=True)
print(f"Gazebo URDF saved to: {output_file}")4. 传感器仿真
4.1 相机传感器
xml
<!-- Camera sensor in URDF -->
<gazebo reference="camera_link">
<sensor type="camera" name="camera_sensor">
<update_rate>30.0</update_rate>
<camera name="camera">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>640</width>
<height>480</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>30.0</updateRate>
<cameraName>robot/camera</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>camera_link</frameName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</gazebo>4.2 激光传感器
xml
<!-- Laser sensor in URDF -->
<gazebo reference="laser_link">
<sensor type="ray" name="laser_sensor">
<pose>0 0 0 0 0 0</pose>
<visualize>true</visualize>
<update_rate>10</update_rate>
<ray>
<scan>
<horizontal>
<samples>720</samples>
<resolution>0.5</resolution>
<min_angle>-3.14159</min_angle>
<max_angle>3.14159</max_angle>
</horizontal>
</scan>
<range>
<min>0.10</min>
<max>30.0</max>
<resolution>0.01</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="laser_controller" filename="libgazebo_ros_laser.so">
<topicName>/scan</topicName>
<frameName>laser_link</frameName>
</plugin>
</sensor>
</gazebo>4.3 传感器管理器
python
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Gazebo传感器管理器
"""
class SensorManager:
"""传感器管理器"""
def __init__(self):
self.sensors = {}
def create_camera_sensor(self, name, link_name, config):
"""创建相机传感器"""
sensor = {
'type': 'camera',
'name': name,
'link': link_name,
'update_rate': config.get('update_rate', 30.0),
'horizontal_fov': config.get('horizontal_fov', 1.3962634),
'image_width': config.get('width', 640),
'image_height': config.get('height', 480),
'image_format': config.get('format', 'R8G8B8'),
'clip_near': config.get('near', 0.02),
'clip_far': config.get('far', 300),
'noise_type': config.get('noise_type', 'gaussian'),
'noise_mean': config.get('noise_mean', 0.0),
'noise_stddev': config.get('noise_stddev', 0.007)
}
self.sensors[name] = sensor
return self.generate_camera_xml(sensor)
def create_laser_sensor(self, name, link_name, config):
"""创建激光传感器"""
sensor = {
'type': 'ray',
'name': name,
'link': link_name,
'update_rate': config.get('update_rate', 10.0),
'samples': config.get('samples', 720),
'resolution': config.get('resolution', 0.5),
'min_angle': config.get('min_angle', -3.14159),
'max_angle': config.get('max_angle', 3.14159),
'min_range': config.get('min_range', 0.10),
'max_range': config.get('max_range', 30.0),
'range_resolution': config.get('range_resolution', 0.01),
'noise_type': config.get('noise_type', 'gaussian'),
'noise_mean': config.get('noise_mean', 0.0),
'noise_stddev': config.get('noise_stddev', 0.01)
}
self.sensors[name] = sensor
return self.generate_laser_xml(sensor)
def create_imu_sensor(self, name, link_name, config):
"""创建IMU传感器"""
sensor = {
'type': 'imu',
'name': name,
'link': link_name,
'update_rate': config.get('update_rate', 100.0),
'angular_velocity_noise': config.get('angular_velocity_noise', 0.0001),
'linear_acceleration_noise': config.get('linear_acceleration_noise', 0.0001),
'angular_velocity_bias': config.get('angular_velocity_bias', 0.0),
'linear_acceleration_bias': config.get('linear_acceleration_bias', 0.0)
}
self.sensors[name] = sensor
return self.generate_imu_xml(sensor)
def create_gps_sensor(self, name, link_name, config):
"""创建GPS传感器"""
sensor = {
'type': 'gps',
'name': name,
'link': link_name,
'update_rate': config.get('update_rate', 10.0),
'position_noise': config.get('position_noise', 0.001),
'velocity_noise': config.get('velocity_noise', 0.0001),
'reference_latitude': config.get('reference_latitude', 49.9),
'reference_longitude': config.get('reference_longitude', 8.9),
'reference_altitude': config.get('reference_altitude', 0.0),
'reference_heading': config.get('reference_heading', 0.0)
}
self.sensors[name] = sensor
return self.generate_gps_xml(sensor)
def generate_camera_xml(self, sensor):
"""生成相机XML"""
gazebo = ET.Element('gazebo', reference=sensor['link'])
sensor_elem = ET.SubElement(gazebo, 'sensor',
type='camera',
name=sensor['name'])
ET.SubElement(sensor_elem, 'update_rate').text = str(sensor['update_rate'])
camera = ET.SubElement(sensor_elem, 'camera', name='camera')
ET.SubElement(camera, 'horizontal_fov').text = str(sensor['horizontal_fov'])
image = ET.SubElement(camera, 'image')
ET.SubElement(image, 'width').text = str(sensor['image_width'])
ET.SubElement(image, 'height').text = str(sensor['image_height'])
ET.SubElement(image, 'format').text = sensor['image_format']
clip = ET.SubElement(camera, 'clip')
ET.SubElement(clip, 'near').text = str(sensor['clip_near'])
ET.SubElement(clip, 'far').text = str(sensor['clip_far'])
noise = ET.SubElement(camera, 'noise')
ET.SubElement(noise, 'type').text = sensor['noise_type']
ET.SubElement(noise, 'mean').text = str(sensor['noise_mean'])
ET.SubElement(noise, 'stddev').text = str(sensor['noise_stddev'])
# 添加ROS插件
plugin = ET.SubElement(sensor_elem, 'plugin',
name='camera_controller',
filename='libgazebo_ros_camera.so')
ET.SubElement(plugin, 'alwaysOn').text = 'true'
ET.SubElement(plugin, 'updateRate').text = str(sensor['update_rate'])
ET.SubElement(plugin, 'cameraName').text = sensor['name']
ET.SubElement(plugin, 'imageTopicName').text = 'image_raw'
ET.SubElement(plugin, 'cameraInfoTopicName').text = 'camera_info'
ET.SubElement(plugin, 'frameName').text = sensor['link']
return gazebo
def generate_laser_xml(self, sensor):
"""生成激光XML"""
gazebo = ET.Element('gazebo', reference=sensor['link'])
sensor_elem = ET.SubElement(gazebo, 'sensor',
type='ray',
name=sensor['name'])
ET.SubElement(sensor_elem, 'pose').text = '0 0 0 0 0 0'
ET.SubElement(sensor_elem, 'visualize').text = 'true'
ET.SubElement(sensor_elem, 'update_rate').text = str(sensor['update_rate'])
ray = ET.SubElement(sensor_elem, 'ray')
scan = ET.SubElement(ray, 'scan')
horizontal = ET.SubElement(scan, 'horizontal')
ET.SubElement(horizontal, 'samples').text = str(sensor['samples'])
ET.SubElement(horizontal, 'resolution').text = str(sensor['resolution'])
ET.SubElement(horizontal, 'min_angle').text = str(sensor['min_angle'])
ET.SubElement(horizontal, 'max_angle').text = str(sensor['max_angle'])
range_elem = ET.SubElement(ray, 'range')
ET.SubElement(range_elem, 'min').text = str(sensor['min_range'])
ET.SubElement(range_elem, 'max').text = str(sensor['max_range'])
ET.SubElement(range_elem, 'resolution').text = str(sensor['range_resolution'])
noise = ET.SubElement(ray, 'noise')
ET.SubElement(noise, 'type').text = sensor['noise_type']
ET.SubElement(noise, 'mean').text = str(sensor['noise_mean'])
ET.SubElement(noise, 'stddev').text = str(sensor['noise_stddev'])
# 添加ROS插件
plugin = ET.SubElement(sensor_elem, 'plugin',
name='laser_controller',
filename='libgazebo_ros_laser.so')
ET.SubElement(plugin, 'topicName').text = f"/{sensor['name']}/scan"
ET.SubElement(plugin, 'frameName').text = sensor['link']
return gazebo5. 物理引擎配置
5.1 物理参数优化
python
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Gazebo物理引擎配置优化
"""
import rospy
from gazebo_msgs.srv import SetPhysicsProperties, GetPhysicsProperties
from geometry_msgs.msg import Vector3
class PhysicsOptimizer:
"""物理引擎优化器"""
def __init__(self):
rospy.init_node('physics_optimizer')
# 物理引擎服务
self.get_physics_srv = rospy.ServiceProxy(
'/gazebo/get_physics_properties', GetPhysicsProperties)
self.set_physics_srv = rospy.ServiceProxy(
'/gazebo/set_physics_properties', SetPhysicsProperties)
rospy.wait_for_service('/gazebo/get_physics_properties')
def get_physics_properties(self):
"""获取物理属性"""
try:
resp = self.get_physics_srv()
print("Current Physics Properties:")
print(f" Time step: {resp.time_step}")
print(f" Max update rate: {resp.max_update_rate}")
print(f" Gravity: {resp.gravity}")
print(f" Auto disable bodies: {resp.ode_config.auto_disable_bodies}")
print(f" SOR PGS iters: {resp.ode_config.sor_pgs_iters}")
print(f" Contact surface layer: {resp.ode_config.contact_surface_layer}")
print(f" Contact max correcting vel: {resp.ode_config.contact_max_correcting_vel}")
print(f" CFM: {resp.ode_config.cfm}")
print(f" ERP: {resp.ode_config.erp}")
return resp
except rospy.ServiceException as e:
rospy.logerr(f"Service call failed: {e}")
return None
def set_physics_properties(self, config):
"""设置物理属性"""
try:
# 获取当前属性
current = self.get_physics_srv()
# 更新属性
time_step = config.get('time_step', current.time_step)
max_update_rate = config.get('max_update_rate', current.max_update_rate)
gravity = config.get('gravity',
[current.gravity.x, current.gravity.y, current.gravity.z])
# ODE配置
ode_config = current.ode_config
ode_config.auto_disable_bodies = config.get('auto_disable_bodies',
ode_config.auto_disable_bodies)
ode_config.sor_pgs_iters = config.get('sor_pgs_iters',
ode_config.sor_pgs_iters)
ode_config.contact_surface_layer = config.get('contact_surface_layer',
ode_config.contact_surface_layer)
ode_config.contact_max_correcting_vel = config.get('contact_max_correcting_vel',
ode_config.contact_max_correcting_vel)
ode_config.cfm = config.get('cfm', ode_config.cfm)
ode_config.erp = config.get('erp', ode_config.erp)
# 调用服务
resp = self.set_physics_srv(
time_step=time_step,
max_update_rate=max_update_rate,
gravity=Vector3(x=gravity[0], y=gravity[1], z=gravity[2]),
ode_config=ode_config
)
if resp.success:
rospy.loginfo("Physics properties updated successfully")
else:
rospy.logerr(f"Failed to update physics: {resp.status_message}")
return resp.success
except rospy.ServiceException as e:
rospy.logerr(f"Service call failed: {e}")
return False
def optimize_for_realtime(self):
"""优化实时性能"""
config = {
'time_step': 0.001, # 1ms时间步
'max_update_rate': 1000, # 1000Hz更新率
'sor_pgs_iters': 20, # 减少迭代次数
'contact_surface_layer': 0.001,
'contact_max_correcting_vel': 100.0,
'cfm': 0.0,
'erp': 0.2
}
return self.set_physics_properties(config)
def optimize_for_accuracy(self):
"""优化精度"""
config = {
'time_step': 0.0001, # 0.1ms时间步
'max_update_rate': 10000, # 10kHz更新率
'sor_pgs_iters': 100, # 增加迭代次数
'contact_surface_layer': 0.0001,
'contact_max_correcting_vel': 10.0,
'cfm': 0.0,
'erp': 0.8
}
return self.set_physics_properties(config)
def set_gravity(self, gravity_vector):
"""设置重力"""
config = {'gravity': gravity_vector}
return self.set_physics_properties(config)
def enable_zero_gravity(self):
"""启用零重力"""
return self.set_gravity([0, 0, 0])
def set_moon_gravity(self):
"""设置月球重力"""
return self.set_gravity([0, 0, -1.62])
def set_mars_gravity(self):
"""设置火星重力"""
return self.set_gravity([0, 0, -3.71])6. 环境建模
6.1 世界文件创建
xml
<!-- worlds/my_world.world -->
<?xml version="1.0"?>
<sdf version="1.6">
<world name="my_world">
<!-- 物理引擎 -->
<physics type="ode">
<real_time_update_rate>1000</real_time_update_rate>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<ode>
<solver>
<type>quick</type>
<iters>50</iters>
<precon_iters>0</precon_iters>
<sor>1.400000</sor>
</solver>
</ode>
</physics>
<!-- 场景 -->
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</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>100 100</size>
</plane>
</geometry>
<surface>
<friction>
<ode>
<mu>100</mu>
<mu2>50</mu2>
</ode>
</friction>
</surface>
</collision>
<visual name="visual">
<cast_shadows>false</cast_shadows>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
</link>
</model>
</world>
</sdf>6.2 环境构建器
python
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Gazebo环境构建器
"""
import random
import math
class WorldBuilder:
"""世界构建器"""
def __init__(self):
self.models = []
self.lights = []
self.physics_config = {}
def create_office_environment(self):
"""创建办公室环境"""
# 地面
self.add_ground()
# 墙壁
self.add_walls(10, 10, 3)
# 办公桌
desks = [
(2, 2, 0, 0),
(2, -2, 0, 0),
(-2, 2, 0, 3.14),
(-2, -2, 0, 3.14)
]
for x, y, z, yaw in desks:
self.add_desk(x, y, z, yaw)
# 椅子
chairs = [
(2.5, 2, 0, 3.14),
(2.5, -2, 0, 3.14),
(-2.5, 2, 0, 0),
(-2.5, -2, 0, 0)
]
for x, y, z, yaw in chairs:
self.add_chair(x, y, z, yaw)
# 障碍物
self.add_random_obstacles(10, 10, 10)
def create_outdoor_environment(self):
"""创建室外环境"""
# 地形
self.add_terrain()
# 树木
self.add_trees(20, 20, 20)
# 建筑物
self.add_buildings(5)
# 道路**作者**：机器人开发工程师
**最后更新**：2024年12月
self.add_roads()
def add_ground(self, size=100):
"""添加地面"""
ground = {
'name': 'ground_plane',
'static': True,
'geometry': {
'type': 'plane',
'normal': [0, 0, 1],
'size': [size, size]
},
'material': 'Gazebo/Grey',
'friction': {'mu': 100, 'mu2': 50}
}
self.models.append(ground)
def add_walls(self, width, length, height):
"""添加墙壁"""
walls = [
# 前墙
{'pos': [width/2, 0, height/2], 'size': [0.1, length, height]},
# 后墙
{'pos': [-width/2, 0, height/2], 'size': [0.1, length, height]},
# 左墙
{'pos': [0, length/2, height/2], 'size': [width, 0.1, height]},
# 右墙
{'pos': [0, -length/2, height/2], 'size': [width, 0.1, height]}
]
for i, wall in enumerate(walls):
model = {
'name': f'wall_{i}',
'static': True,
'pose': wall['pos'] + [0, 0, 0],
'geometry': {
'type': 'box',
'size': wall['size']
},
'material': 'Gazebo/Wood'
}
self.models.append(model)
def add_desk(self, x, y, z, yaw):
"""添加办公桌"""
desk = {
'name': f'desk_{len(self.models)}',
'static': False,
'pose': [x, y, z + 0.4, 0, 0, yaw],
'links': [
# 桌面
{
'name': 'top',
'geometry': {'type': 'box', 'size': [1.2, 0.6, 0.02]},
'mass': 10.0,
'material': 'Gazebo/Wood'
},
# 桌腿
{
'name': 'leg1',
'pose': [0.5, 0.25, -0.4, 0, 0, 0],
'geometry': {'type': 'cylinder', 'radius': 0.02, 'length': 0.8},
'mass': 1.0,
'material': 'Gazebo/Grey'
},
{
'name': 'leg2',
'pose': [0.5, -0.25, -0.4, 0, 0, 0],
'geometry': {'type': 'cylinder', 'radius': 0.02, 'length': 0.8},
'mass': 1.0,
'material': 'Gazebo/Grey'
},
{
'name': 'leg3',
'pose': [-0.5, 0.25, -0.4, 0, 0, 0],
'geometry': {'type': 'cylinder', 'radius': 0.02, 'length': 0.8},
'mass': 1.0,
'material': 'Gazebo/Grey'
},
{
'name': 'leg4',
'pose': [-0.5, -0.25, -0.4, 0, 0, 0],
'geometry': {'type': 'cylinder', 'radius': 0.02, 'length': 0.8},
'mass': 1.0,
'material': 'Gazebo/Grey'
}
]
}
self.models.append(desk)
def add_chair(self, x, y, z, yaw):
"""添加椅子"""
chair = {
'name': f'chair_{len(self.models)}',
'static': False,
'pose': [x, y, z + 0.25, 0, 0, yaw],
'links': [
# 座位
{
'name': 'seat',
'geometry': {'type': 'box', 'size': [0.4, 0.4, 0.04]},
'mass': 2.0,
'material': 'Gazebo/Blue'
},
# 靠背
{
'name': 'back',
'pose': [-0.18, 0, 0.25, 0, 0, 0],
'geometry': {'type': 'box', 'size': [0.04, 0.4, 0.5]},
'mass': 1.0,
'material': 'Gazebo/Blue'
}
]
}
self.models.append(chair)
def add_random_obstacles(self, area_width, area_length, count):
"""添加随机障碍物"""
for i in range(count):
# 随机位置
x = random.uniform(-area_width/2, area_width/2)
y = random.uniform(-area_length/2, area_length/2)
z = 0
# 随机大小
size = [
random.uniform(0.1, 0.5),
random.uniform(0.1, 0.5),
random.uniform(0.5, 1.5)
]
# 随机形状
shape_type = random.choice(['box', 'cylinder', 'sphere'])
obstacle = {
'name': f'obstacle_{i}',
'static': False,
'pose': [x, y, z + size[2]/2, 0, 0, random.uniform(0, 2*math.pi)],
'geometry': {'type': shape_type},
'mass': random.uniform(1, 10),
'material': random.choice(['Gazebo/Red', 'Gazebo/Green',
'Gazebo/Blue', 'Gazebo/Yellow'])
}
if shape_type == 'box':
obstacle['geometry']['size'] = size
elif shape_type == 'cylinder':
obstacle['geometry']['radius'] = size[0]/2
obstacle['geometry']['length'] = size[2]
else: # sphere
obstacle['geometry']['radius'] = size[0]/2
self.models.append(obstacle)
def add_trees(self, area_width, area_length, count):
"""添加树木"""
for i in range(count):
x = random.uniform(-area_width/2, area_width/2)
y = random.uniform(-area_length/2, area_length/2)
tree = {
'name': f'tree_{i}',
'static': True,
'pose': [x, y, 0, 0, 0, 0],
'links': [
# 树干
{
'name': 'trunk',
'pose': [0, 0, 1, 0, 0, 0],
'geometry': {'type': 'cylinder', 'radius': 0.1, 'length': 2},
'material': 'Gazebo/Wood'
},
# 树冠
{
'name': 'crown',
'pose': [0, 0, 3, 0, 0, 0],
'geometry': {'type': 'sphere', 'radius': 1},
'material': 'Gazebo/Green'
}
]
}
self.models.append(tree)7. Gazebo插件开发
7.1 模型插件
cpp
// src/model_plugin.cpp
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <ros/ros.h>
#include <std_msgs/Float64.h>
#include <geometry_msgs/Twist.h>
namespace gazebo {
class ModelPlugin : public ModelPlugin {
public:
ModelPlugin() : ModelPlugin() {}
virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
// 存储模型指针
this->model = _model;
// 解析SDF参数
if (_sdf->HasElement("updateRate")) {
this->update_rate = _sdf->Get<double>("updateRate");
} else {
this->update_rate = 100.0;
}
// 初始化ROS
if (!ros::isInitialized()) {
int argc = 0;
char **argv = NULL;
ros::init(argc, argv, "gazebo_model_plugin",
ros::init_options::NoSigintHandler);
}
// 创建ROS节点句柄
this->rosNode.reset(new ros::NodeHandle("gazebo"));
// 创建订阅者
ros::SubscribeOptions so = ros::SubscribeOptions::create<geometry_msgs::Twist>(
"/" + this->model->GetName() + "/cmd_vel",
1,
boost::bind(&ModelPlugin::OnCmdVel, this, _1),
ros::VoidPtr(),
&this->rosQueue
);
this->cmdVelSub = this->rosNode->subscribe(so);
// 创建发布者
this->posePub = this->rosNode->advertise<geometry_msgs::Pose>(
"/" + this->model->GetName() + "/pose", 1);
// 启动ROS异步线程
this->rosQueueThread = std::thread(
std::bind(&ModelPlugin::QueueThread, this));
// 连接更新事件
this->updateConnection = event::Events::ConnectWorldUpdateBegin(
std::bind(&ModelPlugin::OnUpdate, this));
ROS_INFO("Model plugin loaded for: %s", this->model->GetName().c_str());
}
void OnUpdate() {
// 获取模型位姿
ignition::math::Pose3d pose = this->model->WorldPose();
// 发布位姿
geometry_msgs::Pose msg;
msg.position.x = pose.Pos().X();
msg.position.y = pose.Pos().Y();
msg.position.z = pose.Pos().Z();
msg.orientation.x = pose.Rot().X();
msg.orientation.y = pose.Rot().Y();
msg.orientation.z = pose.Rot().Z();
msg.orientation.w = pose.Rot().W();
this->posePub.publish(msg);
// 应用速度命令
if (this->cmd_vel_received) {
this->model->SetLinearVel(
ignition::math::Vector3d(
this->cmd_linear_x,
this->cmd_linear_y,
this->cmd_linear_z
)
);
this->model->SetAngularVel(
ignition::math::Vector3d(
this->cmd_angular_x,
this->cmd_angular_y,
this->cmd_angular_z
)
);
}
}
void OnCmdVel(const geometry_msgs::Twist::ConstPtr& msg) {
this->cmd_linear_x = msg->linear.x;
this->cmd_linear_y = msg->linear.y;
this->cmd_linear_z = msg->linear.z;
this->cmd_angular_x = msg->angular.x;
this->cmd_angular_y = msg->angular.y;
this->cmd_angular_z = msg->angular.z;
this->cmd_vel_received = true;
}
void QueueThread() {
static const double timeout = 0.01;
while (this->rosNode->ok()) {
this->rosQueue.callAvailable(ros::WallDuration(timeout));
}
}
private:
physics::ModelPtr model;
event::ConnectionPtr updateConnection;
std::unique_ptr<ros::NodeHandle> rosNode;
ros::Subscriber cmdVelSub;
ros::Publisher posePub;
ros::CallbackQueue rosQueue;
std::thread rosQueueThread;
double update_rate;
bool cmd_vel_received = false;
double cmd_linear_x = 0, cmd_linear_y = 0, cmd_linear_z = 0;
double cmd_angular_x = 0, cmd_angular_y = 0, cmd_angular_z = 0;
};
GZ_REGISTER_MODEL_PLUGIN(ModelPlugin)
}7.2 Python插件接口
python
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Gazebo插件Python接口
"""
import rospy
from geometry_msgs.msg import Twist, Pose
from std_msgs.msg import Float64
import threading
class GazeboPluginInterface:
"""Gazebo插件接口"""
def __init__(self, model_name):
self.model_name = model_name
# 发布者
self.cmd_vel_pub = rospy.Publisher(
f'/{model_name}/cmd_vel', Twist, queue_size=1)
# 订阅者
self.pose_sub = rospy.Subscriber(
f'/{model_name}/pose', Pose, self.pose_callback)
# 状态
self.current_pose = None
self.lock = threading.Lock()
def pose_callback(self, msg):
"""位姿回调"""
with self.lock:
self.current_pose = msg
def set_velocity(self, linear, angular):
"""设置速度"""
cmd = Twist()
cmd.linear.x = linear[0]
cmd.linear.y = linear[1]
cmd.linear.z = linear[2]
cmd.angular.x = angular[0]
cmd.angular.y = angular[1]
cmd.angular.z = angular[2]
self.cmd_vel_pub.publish(cmd)
def get_pose(self):
"""获取位姿"""
with self.lock:
return self.current_pose
def move_forward(self, speed=1.0):
"""前进"""
self.set_velocity([speed, 0, 0], [0, 0, 0])
def move_backward(self, speed=1.0):
"""后退"""
self.set_velocity([-speed, 0, 0], [0, 0, 0])
def turn_left(self, speed=1.0):
"""左转"""
self.set_velocity([0, 0, 0], [0, 0, speed])
def turn_right(self, speed=1.0):
"""右转"""
self.set_velocity([0, 0, 0], [0, 0, -speed])
def stop(self):
"""停止"""
self.set_velocity([0, 0, 0], [0, 0, 0])8. ros_control集成
8.1 控制器配置
yaml
# config/controllers.yaml
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
position_controllers:
joint1_position_controller:
type: effort_controllers/JointPositionController
joint: joint1
pid: {p: 100.0, i: 0.01, d: 10.0}
joint2_position_controller:
type: effort_controllers/JointPositionController
joint: joint2
pid: {p: 100.0, i: 0.01, d: 10.0}
velocity_controllers:
left_wheel_velocity_controller:
type: effort_controllers/JointVelocityController
joint: left_wheel_joint
pid: {p: 100.0, i: 0.01, d: 10.0}
right_wheel_velocity_controller:
type: effort_controllers/JointVelocityController
joint: right_wheel_joint
pid: {p: 100.0, i: 0.01, d: 10.0}
trajectory_controller:
type: position_controllers/JointTrajectoryController
joints:
- joint1
- joint2
- joint3
gains:
joint1: {p: 100, d: 1, i: 1, i_clamp: 1}
joint2: {p: 100, d: 1, i: 1, i_clamp: 1}
joint3: {p: 100, d: 1, i: 1, i_clamp: 1}8.2 控制器管理
python
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
ros_control控制器管理
"""
import rospy
from controller_manager_msgs.srv import (
LoadController, UnloadController,
SwitchController, ListControllers
)
from std_msgs.msg import Float64
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
class ControllerManager:
"""控制器管理器"""
def __init__(self):
rospy.init_node('controller_manager_interface')
# 控制器管理服务
self.load_controller_srv = rospy.ServiceProxy(
'/controller_manager/load_controller', LoadController)
self.unload_controller_srv = rospy.ServiceProxy(
'/controller_manager/unload_controller', UnloadController)
self.switch_controller_srv = rospy.ServiceProxy(
'/controller_manager/switch_controller', SwitchController)
self.list_controllers_srv = rospy.ServiceProxy(
'/controller_manager/list_controllers', ListControllers)
# 等待服务
rospy.wait_for_service('/controller_manager/load_controller')
# 控制器命令发布者
self.command_publishers = {}
def load_controller(self, controller_name):
"""加载控制器"""
try:
resp = self.load_controller_srv(name=controller_name)
if resp.ok:
rospy.loginfo(f"Controller '{controller_name}' loaded successfully")
else:
rospy.logerr(f"Failed to load controller '{controller_name}'")
return resp.ok
except rospy.ServiceException as e:
rospy.logerr(f"Service call failed: {e}")
return False
def unload_controller(self, controller_name):
"""卸载控制器"""
try:
resp = self.unload_controller_srv(name=controller_name)
if resp.ok:
rospy.loginfo(f"Controller '{controller_name}' unloaded successfully")
else:
rospy.logerr(f"Failed to unload controller '{controller_name}'")
return resp.ok
except rospy.ServiceException as e:
rospy.logerr(f"Service call failed: {e}")
return False
def start_controller(self, controller_name):
"""启动控制器"""
try:
resp = self.switch_controller_srv(
start_controllers=[controller_name],
stop_controllers=[],
strictness=1 # BEST_EFFORT
)
if resp.ok:
rospy.loginfo(f"Controller '{controller_name}' started")
else:
rospy.logerr(f"Failed to start controller '{controller_name}'")
return resp.ok
except rospy.ServiceException as e:
rospy.logerr(f"Service call failed: {e}")
return False
def stop_controller(self, controller_name):
"""停止控制器"""
try:
resp = self.switch_controller_srv(
start_controllers=[],
stop_controllers=[controller_name],
strictness=1 # BEST_EFFORT
)
if resp.ok:
rospy.loginfo(f"Controller '{controller_name}' stopped")
else:
rospy.logerr(f"Failed to stop controller '{controller_name}'")
return resp.ok
except rospy.ServiceException as e:
rospy.logerr(f"Service call failed: {e}")
return False
def list_controllers(self):
"""列出控制器"""
try:
resp = self.list_controllers_srv()
print("Controllers:")
for controller in resp.controller:
status = "running" if controller.state == "running" else "stopped"
print(f" - {controller.name}: {controller.type} [{status}]")
return resp.controller
except rospy.ServiceException as e:
rospy.logerr(f"Service call failed: {e}")
return []
def send_joint_command(self, controller_name, command):
"""发送关节命令"""
topic = f"/{controller_name}/command"
if topic not in self.command_publishers:
self.command_publishers[topic] = rospy.Publisher(
topic, Float64, queue_size=1)
msg = Float64()
msg.data = command
self.command_publishers[topic].publish(msg)
def send_trajectory(self, controller_name, joint_names, points):
"""发送轨迹"""
topic = f"/{controller_name}/command"
if topic not in self.command_publishers:
self.command_publishers[topic] = rospy.Publisher(
topic, JointTrajectory, queue_size=1)
trajectory = JointTrajectory()
trajectory.header.stamp = rospy.Time.now()
trajectory.joint_names = joint_names
for point_data in points:
point = JointTrajectoryPoint()
point.positions = point_data['positions']
point.velocities = point_data.get('velocities', [])
point.accelerations = point_data.get('accelerations', [])
point.time_from_start = rospy.Duration(point_data['time'])
trajectory.points.append(point)
self.command_publishers[topic].publish(trajectory)9. 仿真调试与优化
9.1 仿真性能分析
python
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Gazebo仿真性能分析
"""
import rospy
from rosgraph_msgs.msg import Clock
from gazebo_msgs.msg import PerformanceMetrics
import time
import psutil
import matplotlib.pyplot as plt
class SimulationPerformanceAnalyzer:
"""仿真性能分析器"""
def __init__(self):
rospy.init_node('simulation_performance_analyzer')
# 订阅时钟
self.clock_sub = rospy.Subscriber('/clock', Clock, self.clock_callback)
# 性能数据
self.real_times = []
self.sim_times = []
self.real_time_factors = []
self.cpu_usage = []
self.memory_usage = []
# 开始时间
self.start_real_time = time.time()
self.start_sim_time = None
def clock_callback(self, msg):
"""时钟回调"""
current_sim_time = msg.clock.to_sec()
current_real_time = time.time()
if self.start_sim_time is None:
self.start_sim_time = current_sim_time
return
# 计算经过的时间
elapsed_sim = current_sim_time - self.start_sim_time
elapsed_real = current_real_time - self.start_real_time
# 计算实时因子
if elapsed_real > 0:
rtf = elapsed_sim / elapsed_real
else:
rtf = 0
# 记录数据
self.real_times.append(elapsed_real)
self.sim_times.append(elapsed_sim)
self.real_time_factors.append(rtf)
# 系统资源使用
self.cpu_usage.append(psutil.cpu_percent())
self.memory_usage.append(psutil.virtual_memory().percent)
# 定期输出
if len(self.real_times) % 100 == 0:
self.print_statistics()
def print_statistics(self):
"""打印统计信息"""
if not self.real_time_factors:
return
current_rtf = self.real_time_factors[-1]
avg_rtf = sum(self.real_time_factors) / len(self.real_time_factors)
min_rtf = min(self.real_time_factors)
max_rtf = max(self.real_time_factors)
avg_cpu = sum(self.cpu_usage) / len(self.cpu_usage)
avg_mem = sum(self.memory_usage) / len(self.memory_usage)
print("\n" + "="*50)
print("SIMULATION PERFORMANCE")
print("="*50)
print(f"Current RTF: {current_rtf:.3f}")
print(f"Average RTF: {avg_rtf:.3f}")
print(f"Min RTF: {min_rtf:.3f}")
print(f"Max RTF: {max_rtf:.3f}")
print(f"Average CPU: {avg_cpu:.1f}%")
print(f"Average Memory: {avg_mem:.1f}%")
print("="*50)
def plot_performance(self):
"""绘制性能图表"""
fig, axes = plt.subplots(3, 1, figsize=(10, 8))
# 实时因子
axes[0].plot(self.real_times, self.real_time_factors)
axes[0].set_ylabel('Real Time Factor')
axes[0].set_title('Simulation Performance')
axes[0].grid(True)
axes[0].axhline(y=1.0, color='r', linestyle='--', label='Real-time')
axes[0].legend()
# CPU使用率
axes[1].plot(self.real_times, self.cpu_usage)
axes[1].set_ylabel('CPU Usage (%)')
axes[1].grid(True)
# 内存使用率
axes[2].plot(self.real_times, self.memory_usage)
axes[2].set_xlabel('Time (s)')
axes[2].set_ylabel('Memory Usage (%)')
axes[2].grid(True)
plt.tight_layout()
plt.show()
def save_data(self, filename):
"""保存数据"""
data = {
'real_times': self.real_times,
'sim_times': self.sim_times,
'real_time_factors': self.real_time_factors,
'cpu_usage': self.cpu_usage,
'memory_usage': self.memory_usage
}
import pickle
with open(filename, 'wb') as f:
pickle.dump(data, f)
print(f"Performance data saved to: {filename}")10. 实战案例：完整仿真系统
10.1 移动机器人仿真系统
python
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
完整的移动机器人仿真系统
"""
import rospy
import tf2_ros
from geometry_msgs.msg import Twist, PoseStamped
from sensor_msgs.msg import LaserScan, Image
from nav_msgs.msg import Odometry, Path
import numpy as np
class MobileRobotSimulation:
"""移动机器人仿真系统"""
def __init__(self, robot_name="mobile_robot"):
rospy.init_node('mobile_robot_simulation')
self.robot_name = robot_name
# Gazebo接口
self.gazebo_interface = GazeboROSInterface()
# 控制器管理
self.controller_manager = ControllerManager()
# 传感器管理
self.sensor_manager = SensorManager()
# 初始化机器人
self.spawn_robot()
self.setup_controllers()
self.setup_sensors()
# 导航系统
self.setup_navigation()
# 控制循环
self.control_timer = rospy.Timer(
rospy.Duration(0.1), self.control_callback)
def spawn_robot(self):
"""生成机器人"""
# 读取URDF
with open('mobile_robot.urdf', 'r') as f:
robot_xml = f.read()
# 初始位置
initial_pose = Pose()
initial_pose.position.x = 0
initial_pose.position.y = 0
initial_pose.position.z = 0.5
initial_pose.orientation.w = 1.0
# 生成机器人
success = self.gazebo_interface.spawn_model(
self.robot_name, robot_xml, initial_pose)
if success:
rospy.loginfo("Robot spawned successfully")
else:
rospy.logerr("Failed to spawn robot")
def setup_controllers(self):
"""设置控制器"""
# 加载控制器
controllers = [
'joint_state_controller',
'left_wheel_velocity_controller',
'right_wheel_velocity_controller'
]
for controller in controllers:
self.controller_manager.load_controller(controller)
self.controller_manager.start_controller(controller)
def setup_sensors(self):
"""设置传感器"""
# 激光传感器
laser_config = {
'update_rate': 10,
'samples': 720,
'min_angle': -3.14159,
'max_angle': 3.14159,
'min_range': 0.1,
'max_range': 10.0
}
self.sensor_manager.create_laser_sensor(
'laser', 'laser_link', laser_config)
# 相机传感器
camera_config = {
'update_rate': 30,
'width': 640,
'height': 480,
'horizontal_fov': 1.3962634
}
self.sensor_manager.create_camera_sensor(
'camera', 'camera_link', camera_config)
# IMU传感器
imu_config = {
'update_rate': 100,
'angular_velocity_noise': 0.0001,
'linear_acceleration_noise': 0.0001
}
self.sensor_manager.create_imu_sensor(
'imu', 'imu_link', imu_config)
def setup_navigation(self):
"""设置导航"""
# 速度发布者
self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
# 传感器订阅
self.scan_sub = rospy.Subscriber('/scan', LaserScan, self.scan_callback)
self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_callback)
# 目标订阅
self.goal_sub = rospy.Subscriber('/move_base_simple/goal',
PoseStamped, self.goal_callback)
# 路径发布
self.path_pub = rospy.Publisher('/path', Path, queue_size=1)
# 状态
self.current_pose = None
self.current_scan = None
self.goal_pose = None
self.path = Path()
def scan_callback(self, msg):
"""激光扫描回调"""
self.current_scan = msg
# 简单避障
if self.current_scan:
self.obstacle_avoidance()
def odom_callback(self, msg):
"""里程计回调"""
self.current_pose = msg.pose.pose
# 更新路径
pose_stamped = PoseStamped()
pose_stamped.header = msg.header
pose_stamped.pose = msg.pose.pose
self.path.poses.append(pose_stamped)
# 发布路径
self.path.header = msg.header
self.path_pub.publish(self.path)
def goal_callback(self, msg):
"""目标回调"""
self.goal_pose = msg.pose
rospy.loginfo(f"New goal: ({msg.pose.position.x:.2f}, "
f"{msg.pose.position.y:.2f})")
def obstacle_avoidance(self):
"""简单避障"""
if not self.current_scan:
return
# 分析激光数据
ranges = np.array(self.current_scan.ranges)
# 前方扇区（-30度到+30度）
front_sector = ranges[330:390]
# 检测障碍物
min_distance = np.min(front_sector[np.isfinite(front_sector)])
cmd = Twist()
if min_distance < 0.5: # 障碍物太近
# 停止并转向
cmd.linear.x = 0.0
cmd.angular.z = 0.5
rospy.logwarn(f"Obstacle detected at {min_distance:.2f}m, turning...")
else:
# 正常前进
cmd.linear.x = 0.5
cmd.angular.z = 0.0
self.cmd_vel_pub.publish(cmd)
def control_callback(self, event):
"""控制回调"""
if self.goal_pose and self.current_pose:
# 简单的点对点导航
dx = self.goal_pose.position.x - self.current_pose.position.x
dy = self.goal_pose.position.y - self.current_pose.position.y
distance = np.sqrt(dx**2 + dy**2)
angle = np.arctan2(dy, dx)
# 获取当前朝向
q = self.current_pose.orientation
_, _, yaw = tf.transformations.euler_from_quaternion(
[q.x, q.y, q.z, q.w])
angle_error = angle - yaw
# 归一化角度
while angle_error > np.pi:
angle_error -= 2*np.pi
while angle_error < -np.pi:
angle_error += 2*np.pi
cmd = Twist()
if distance < 0.1: # 到达目标
cmd.linear.x = 0.0
cmd.angular.z = 0.0
self.goal_pose = None
rospy.loginfo("Goal reached!")
elif abs(angle_error) > 0.2: # 需要转向
cmd.linear.x = 0.0
cmd.angular.z = 0.5 * np.sign(angle_error)
else: # 前进
cmd.linear.x = min(0.5, distance)
cmd.angular.z = 0.5 * angle_error
self.cmd_vel_pub.publish(cmd)
def main():
"""主函数"""
simulation = MobileRobotSimulation()
try:
rospy.spin()
except KeyboardInterrupt:
rospy.loginfo("Shutting down simulation...")
if __name__ == '__main__':
main()11. 总结与最佳实践
11.1 本文总结
通过本文的学习，你已经掌握了：
- ✅ Gazebo基础：理解Gazebo架构和ROS集成
- ✅ 环境搭建：配置Gazebo仿真环境
- ✅ URDF转换：添加Gazebo特定标签
- ✅ 传感器仿真：配置各类传感器
- ✅ 物理引擎：优化物理仿真参数
- ✅ 环境建模：创建仿真世界
- ✅ 插件开发：自定义Gazebo插件
- ✅ ros_control：集成控制系统
- ✅ 调试优化：性能分析和优化
- ✅ 完整系统：构建完整仿真系统
11.2 最佳实践
| 方面 | 建议 | 原因 |
|---|---|---|
| 模型简化 | 使用简单碰撞模型 | 提高仿真性能 |
| 更新频率 | 传感器10-30Hz | 平衡精度和性能 |
| 物理步长 | 0.001秒或更小 | 确保稳定性 |
| 实时因子 | 监控RTF | 评估性能 |
| 插件使用 | 优先使用现有插件 | 减少开发时间 |
| 资源管理 | 及时删除不用的模型 | 节省内存 |
11.3 常见问题解决
-
仿真卡顿
- 降低更新频率
- 简化碰撞模型
- 减少传感器数量
-
物体穿透
- 减小物理步长
- 增加迭代次数
- 调整接触参数
-
传感器噪声
- 配置噪声模型
- 使用滤波器
- 调整采样率
-
控制不稳定
- 调整PID参数
- 检查惯性参数
- 验证关节限制
11.4 下一步学习
在下一篇文章中，我们将学习：
- 导航与路径规划：move_base框架
- SLAM建图：gmapping和cartographer
- 定位算法：AMCL粒子滤波
- 避障策略：动态窗口法
版权声明：本文为原创文章，转载请注明出处
💡 学习建议：Gazebo仿真是机器人开发的重要工具，可以在安全的虚拟环境中测试算法。建议先从简单的模型开始，逐步增加复杂度。重点掌握传感器配置和物理参数调优，这对仿真真实性至关重要。
下一篇预告：《【ROS1从入门到精通】第12篇：导航与路径规划（让机器人自主导航）》
敬请期待！