文章目录
引言
在Gazebo仿真中，模型插件是实现物理交互与智能控制的核心组件。本文将以SU7 Ultra 四轮车辆模型为例，详细讲解如何基于ROS 2创建Gazebo模型插件，实现对车辆的速度与转向控制，并通过自定义消息完成数据交互。
一、插件开发环境与工程结构
-
开发环境
本文的开发依赖以下工具
- ROS 2 foxy
- Gazebo 11+
- CMake 3.5+
- C++17 编译器
-
工程结构
本文的工程文件采用模块化设计，将功能拆分为两个核心包，既保证了模块独立性，又便于维护扩展：
gazebo_four_wheeled_vehicle/ ├── vehicle_msgs/ # 自定义消息包 │ ├── msg/ │ │ ├── VehicleCmd.msg # 控制指令消息 │ │ └── VehicleStatus.msg # 车辆状态消息 │ ├── CMakeLists.txt │ └── package.xml └── four_wheeled_vehicle_plugin/ # 模型插件包 ├── include/ │ └── four_wheeled_vehicle_plugin.h # 插件头文件 ├── src/ │ └── four_wheeled_vehicle_plugin.cpp # 插件实现 ├── models/ # SU7 Ultra 模型文件 │ └── four_wheeled_vehicle/ │ ├── model.config │ └── model.sdf ├── launch/ # 启动文件 │ └── vehicle_gazebo.launch.py ├── CMakeLists.txt └── package.xml- vehicle_msgs：独立消息包便于其他模块复用；
- four_wheeled_vehicle_plugin：包含插件核心逻辑、模型定义和启动脚本。
二、自定义消息定义
为实现ROS 2与Gazebo插件的通信和提高消息的可读性，定义专用消息类型传递控制指令与车辆状态。
2.1 消息文件设计
-
VehicleCmd.msg：用于发送速度与转向指令float64 speed # 目标速度（m/s） float64 steering_angle # 目标转向角（rad）speed：目标线速度（m/s），支持正负值（前进 / 后退）
steering_angle：目标转向角（rad），支持正负值（左 / 右转向）
-
VehicleStatus.msg：用于反馈车辆状态geometry_msgs/Point position # 位置坐标 float64 yaw # 偏航角（rad） float64 speed # 当前速度（m/s）position：基于geometry_msgs/Point复用坐标信息（x,y,z）yaw：偏航角（rad），反映车辆航向speed：当前实际速度（m/s），用于控制闭环反馈
2.2 消息包配置
-
package.xml：声明依赖与构建工具
<?xml version="1.0"?> <package format="3"> <name>vehicle_msgs</name> <version>0.1.0</version> <description>Custom vehicle control messages</description> <maintainer email="user@example.com">Your Name</maintainer> <license>Apache-2.0</license> <buildtool_depend>ament_cmake</buildtool_depend> <buildtool_depend>rosidl_default_generators</buildtool_depend> <depend>geometry_msgs</depend> <exec_depend>rosidl_default_runtime</exec_depend> <member_of_group>rosidl_interface_packages</member_of_group> <export> <build_type>ament_cmake</build_type> </export> </package>rosidl_default_generators：ROS 2 消息生成工具，自动将.msg 文件转换为 C++/Python 接口geometry_msgs：依赖该包的 Point 类型，避免重复定义基础坐标结构rosidl_default_runtime：运行时依赖，确保消息序列化 / 反序列化正常工作
-
CMakeLists.txt：配置消息生成
cmake_minimum_required(VERSION 3.5) project(vehicle_msgs) find_package(ament_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(geometry_msgs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/VehicleCmd.msg" "msg/VehicleStatus.msg" DEPENDENCIES geometry_msgs ) ament_export_dependencies(rosidl_default_runtime) ament_package()rosidl_generate_interfaces：核心指令，指定消息文件及依赖的其他消息包DEPENDENCIES geometry_msgs：明确消息依赖关系，确保编译顺序正确ament_export_dependencies：导出依赖，便于其他包使用该消息包
三、Gazebo模型插件实现
Gazebo 模型插件通过继承ModelPlugin类，实现对仿真模型的底层控制。插件需完成三大核心功能：解析 ROS 2 控制指令、驱动模型物理运动、反馈车辆状态。
3.1 插件类设计（.h 文件）
four_wheeled_vehicle_plugin.h 定义了插件核心类，继承自Gazebo的ModelPlugin：
#ifndef FOUR_WHEELED_VEHICLE_PLUGIN_H
#define FOUR_WHEELED_VEHICLE_PLUGIN_H
#include "vehicle_msgs/msg/vehicle_cmd.hpp"
#include "vehicle_msgs/msg/vehicle_status.hpp"
#include <gazebo/common/PID.hh> // 用于PID控制器
#include <gazebo/common/Plugin.hh>
#include <gazebo/physics/Joint.hh>
#include <gazebo/physics/JointController.hh>
#include <gazebo/physics/Link.hh>
#include <gazebo/physics/Model.hh>
#include <geometry_msgs/msg/twist.hpp>
#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.h>
namespace gazebo {
class FourWheeledVehiclePlugin : public ModelPlugin {
public:
FourWheeledVehiclePlugin();
~FourWheeledVehiclePlugin() override;
void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) override;
private:
void OnUpdate(const common::UpdateInfo &_info);
void CmdCallback(const vehicle_msgs::msg::VehicleCmd::SharedPtr msg);
void UpdateSteering(double dt);
void UpdateSpeed(double dt);
physics::ModelPtr model_;
physics::JointPtr fl_steer_joint_;
physics::JointPtr fr_steer_joint_;
physics::JointPtr rl_wheel_joint_;
physics::JointPtr rr_wheel_joint_;
event::ConnectionPtr update_connection_;
common::Time last_update_time;
common::PID left_steering_pid;
common::PID right_steering_pid;
common::PID rear_left_pid;
common::PID rear_right_pid;
rclcpp::Node::SharedPtr ros_node_;
rclcpp::Subscription<vehicle_msgs::msg::VehicleCmd>::SharedPtr cmd_sub_;
rclcpp::Publisher<vehicle_msgs::msg::VehicleStatus>::SharedPtr status_pub_;
double target_speed;
double target_steering_angle;
double wheelbase_;
double track_width_;
double wheel_radius_;
double max_speed_;
double max_steering_angle_;
};
} // namespace gazebo
#endif // FOUR_WHEELED_VEHICLE_PLUGIN_H
这里分离转向与速度控制为独立函数（UpdateSteering/UpdateSpeed），便于单独调试，同时使用 PID 控制器处理物理模型的动态响应延迟，提高控制稳定性。
3.2 插件逻辑实现（.cpp 文件）
four_wheeled_vehicle_plugin.cpp 实现了插件的具体逻辑，主要包括初始化、控制指令处理、状态更新等。
#include "four_wheeled_vehicle_plugin.h"
#include <gazebo/physics/Joint.hh>
#include <gazebo/physics/Link.hh>
#include <rclcpp/logging.hpp>
#include <tf2/LinearMath/Matrix3x3.h>
namespace gazebo {
FourWheeledVehiclePlugin::FourWheeledVehiclePlugin() {
left_steering_pid = common::PID(2000.0, 0.0, 300.0);
right_steering_pid = common::PID(2000.0, 0.0, 300.0);
left_steering_pid.SetCmdMin(-5000.0);
left_steering_pid.SetCmdMax(5000.0);
right_steering_pid.SetCmdMin(-5000.0);
right_steering_pid.SetCmdMax(5000.0);
rear_left_pid = common::PID(1000.0, 0.0, 1.0);
rear_right_pid = common::PID(1000.0, 0.0, 1.0);
rear_left_pid.SetCmdMin(-5000.0);
rear_left_pid.SetCmdMax(5000.0);
rear_right_pid.SetCmdMin(-5000.0);
rear_right_pid.SetCmdMax(5000.0);
target_speed = 0.0;
target_steering_angle = 0.0;
last_update_time = common::Time(0);
}
FourWheeledVehiclePlugin::~FourWheeledVehiclePlugin() {
update_connection_.reset();
ros_node_.reset();
}
void FourWheeledVehiclePlugin::Load(physics::ModelPtr _model,
sdf::ElementPtr _sdf) {
model_ = _model;
if (!rclcpp::ok()) {
RCLCPP_FATAL_STREAM(rclcpp::get_logger("vehicle_plugin"), "ROS 2未初始化");
return;
}
ros_node_ = rclcpp::Node::make_shared("vehicle_controller_node");
RCLCPP_INFO(ros_node_->get_logger(), "车辆控制器插件加载成功");
fl_steer_joint_ = model_->GetJoint("front_left_steering_joint");
fr_steer_joint_ = model_->GetJoint("front_right_steering_joint");
rl_wheel_joint_ = model_->GetJoint("rear_left_wheel_joint");
rr_wheel_joint_ = model_->GetJoint("rear_right_wheel_joint");
wheelbase_ = _sdf->Get<double>("wheelbase", 3.0).first;
track_width_ = _sdf->Get<double>("track_width", 1.666).first;
wheel_radius_ = _sdf->Get<double>("wheel_radius", 0.3).first;
max_speed_ = _sdf->Get<double>("max_speed", 20.0).first;
max_steering_angle_ = _sdf->Get<double>("max_steering_angle", 0.6).first;
if (!fl_steer_joint_ || !fr_steer_joint_ || !rl_wheel_joint_ ||
!rr_wheel_joint_) {
RCLCPP_FATAL(ros_node_->get_logger(), "关节未找到,请检查SDF关节名");
return;
}
cmd_sub_ = ros_node_->create_subscription<vehicle_msgs::msg::VehicleCmd>(
"vehicle_cmd", 10,
std::bind(&FourWheeledVehiclePlugin::CmdCallback, this,
std::placeholders::_1));
status_pub_ = ros_node_->create_publisher<vehicle_msgs::msg::VehicleStatus>(
"vehicle_status", 10);
update_connection_ = event::Events::ConnectWorldUpdateBegin(std::bind(
&FourWheeledVehiclePlugin::OnUpdate, this, std::placeholders::_1));
}
void FourWheeledVehiclePlugin::CmdCallback(
const vehicle_msgs::msg::VehicleCmd::SharedPtr msg) {
target_speed = msg->speed;
target_steering_angle = msg->steering_angle;
target_speed = std::clamp(target_speed, -max_speed_, max_speed_);
target_steering_angle = std::clamp(target_steering_angle,
-max_steering_angle_, max_steering_angle_);
}
void FourWheeledVehiclePlugin::OnUpdate(const common::UpdateInfo &_info) {
rclcpp::spin_some(ros_node_);
if (last_update_time == common::Time(0)) {
last_update_time = _info.simTime;
return;
}
double dt = (_info.simTime - last_update_time).Double();
last_update_time = _info.simTime;
UpdateSteering(dt);
UpdateSpeed(dt);
auto status_msg = vehicle_msgs::msg::VehicleStatus();
auto base_link = model_->GetLink("base_link");
auto pose = base_link->WorldPose();
status_msg.position.x = pose.Pos().X();
status_msg.position.y = pose.Pos().Y();
status_msg.position.z = pose.Pos().Z();
status_msg.yaw = pose.Rot().Yaw();
auto linear_vel = base_link->WorldLinearVel();
status_msg.speed = std::hypot(linear_vel.X(), linear_vel.Y());
status_pub_->publish(status_msg);
}
void FourWheeledVehiclePlugin::UpdateSteering(double dt) {
double tan_alph = tan(target_steering_angle);
double target_fl_angle = atan(wheelbase_ * tan_alph /
(wheelbase_ - 0.5 * track_width_ * tan_alph));
double target_fr_angle = atan(wheelbase_ * tan_alph /
(wheelbase_ + 0.5 * track_width_ * tan_alph));
double current_fl_angle = fl_steer_joint_->Position(0);
double current_fr_angle = fr_steer_joint_->Position(0);
double fl_error = current_fl_angle - target_fl_angle;
double fr_error = current_fr_angle - target_fr_angle;
double fl_force = left_steering_pid.Update(fl_error, dt);
double fr_force = right_steering_pid.Update(fr_error, dt);
fl_steer_joint_->SetForce(0, fl_force);
fr_steer_joint_->SetForce(0, fr_force);
}
void FourWheeledVehiclePlugin::UpdateSpeed(double dt) {
double target_angular_vel = target_speed / wheel_radius_;
double current_rl_vel = rl_wheel_joint_->GetVelocity(0);
double current_rr_vel = rr_wheel_joint_->GetVelocity(0);
double rl_error = current_rl_vel - target_angular_vel;
double rr_error = current_rr_vel - target_angular_vel;
double rl_force = rear_left_pid.Update(rl_error, dt);
double rr_force = rear_right_pid.Update(rr_error, dt);
rl_wheel_joint_->SetForce(0, rl_force);
rr_wheel_joint_->SetForce(0, rr_force);
}
GZ_REGISTER_MODEL_PLUGIN(FourWheeledVehiclePlugin)
} // namespace gazebo
四、编译配置
配置插件编译选项，链接依赖库，插件对应的CMakeLists.txt 为
cmake_minimum_required(VERSION 3.5)
project(four_wheeled_vehicle)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# 查找依赖
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(gazebo_ros REQUIRED)
find_package(gazebo_dev REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(vehicle_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(tf2 REQUIRED)
# 编译插件
add_library(four_wheeled_vehicle_plugin SHARED
src/four_wheeled_vehicle_plugin.cpp
)
target_include_directories(four_wheeled_vehicle_plugin PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
${GAZEBO_INCLUDE_DIRS}
)
target_link_libraries(four_wheeled_vehicle_plugin
${GAZEBO_LIBRARIES}
)
ament_target_dependencies(four_wheeled_vehicle_plugin
gazebo_ros
rclcpp
geometry_msgs
std_msgs
tf2
vehicle_msgs
)
include_directories(
${vehicle_msgs_INCLUDE_DIRS}
${rosidl_generator_c_INCLUDE_DIRS}
)
# 安装插件
install(TARGETS four_wheeled_vehicle_plugin
DESTINATION lib/${PROJECT_NAME}
)
# 安装模型、启动文件、世界文件
install(DIRECTORY models launch/ worlds/
DESTINATION share/${PROJECT_NAME}/
)
ament_package()
声明包依赖与插件类型，对应的package.xml为
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>four_wheeled_vehicle</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="w@todo.todo">w</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>gazebo_dev</depend>
<depend>geometry_msgs</depend>
<depend>vehicle_msgs</depend>
<depend>std_msgs</depend>
<depend>tf2</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<exec_depend>gazebo_ros</exec_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
五、模型与插件集成
5.1 SDF模型配置
在SU7 Ultra 模型的sdf文件中，添加了插件的引用，以及指定插件库路径与参数：
<sdf version='1.7'>
<model name='four_wheeled_vehicle'>
<pose>0 0 1.0 0 0 0</pose>
<link name='base_link'>
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>2000</mass>
<inertia>
<ixx>1200</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>2500</iyy>
<iyz>0</iyz>
<izz>3000</izz>
</inertia>
</inertial>
<collision name='base_link_collision'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<box>
<size>5.24 1.5 0.47</size>
</box>
</geometry>
</collision>
<visual name='base_link_visual'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<mesh>
<uri>model://four_wheeled_vehicle/meshes/car.dae</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
</visual>
</link>
<joint name='front_left_steering_joint' type='revolute'>
<pose relative_to='base_link'>1.445 0.833 -0.024 0 0 0</pose>
<parent>base_link</parent>
<child>front_left_steering_link</child>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-0.6</lower>
<upper>0.6</upper>
<effort>1e+06</effort>
<velocity>10000</velocity>
</limit>
<dynamics>
<damping>10</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
</axis>
</joint>
<link name='front_left_steering_link'>
<pose relative_to='front_left_steering_joint'>0 0 0 0 0 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>5</mass>
<inertia>
<ixx>0.012</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.025</iyy>
<iyz>0</iyz>
<izz>0.012</izz>
</inertia>
</inertial>
<visual name='front_left_steering_link_visual'>
<pose>0 0 0 1.5708 0 0</pose>
<geometry>
<cylinder>
<length>0.01</length>
<radius>0.1</radius>
</cylinder>
</geometry>
</visual>
</link>
<joint name='front_left_wheel_joint' type='revolute'>
<pose relative_to='front_left_steering_link'>0 0 0 0 0 0</pose>
<parent>front_left_steering_link</parent>
<child>front_left_wheel_link</child>
<axis>
<xyz>0 1 0</xyz>
<limit>
<effort>0</effort>
<velocity>0</velocity>
<lower>-1e+16</lower>
<upper>1e+16</upper>
</limit>
<dynamics>
<damping>10.0</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
</axis>
</joint>
<link name='front_left_wheel_link'>
<pose relative_to='front_left_wheel_joint'>0 0 0 0 0 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>40</mass>
<inertia>
<ixx>1.4</ixx>
<iyy>2.5</iyy>
<izz>1.4</izz>
</inertia>
</inertial>
<collision name='front_left_wheel_link_collision'>
<pose>0 0 0 1.5708 0 0</pose>
<geometry>
<cylinder>
<length>0.35</length>
<radius>0.3</radius>
</cylinder>
</geometry>
<surface>
<friction>
<ode>
<mu>2.0</mu>
<mu2>1.5</mu2>
<slip1>0.0</slip1>
<slip2>0.0</slip2>
</ode>
</friction>
</surface>
</collision>
<visual name='front_left_wheel_link_visual'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<mesh>
<uri>model://four_wheeled_vehicle/meshes/wheel_l.dae</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
</visual>
</link>
<joint name='front_right_steering_joint' type='revolute'>
<pose relative_to='base_link'>1.445 -0.833 -0.024 0 0 0</pose>
<parent>base_link</parent>
<child>front_right_steering_link</child>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-0.6</lower>
<upper>0.6</upper>
<effort>1e+06</effort>
<velocity>10000</velocity>
</limit>
<dynamics>
<damping>10</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
</axis>
</joint>
<link name='front_right_steering_link'>
<pose relative_to='front_right_steering_joint'>0 0 0 0 0 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>5</mass>
<inertia>
<ixx>0.012</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.025</iyy>
<iyz>0</iyz>
<izz>0.012</izz>
</inertia>
</inertial>
<visual name='front_right_steering_link_visual'>
<pose>0 0 0 1.5708 0 0</pose>
<geometry>
<cylinder>
<length>0.01</length>
<radius>0.1</radius>
</cylinder>
</geometry>
</visual>
</link>
<joint name='front_right_wheel_joint' type='revolute'>
<pose relative_to='front_right_steering_link'>0 0 0 0 0 0</pose>
<parent>front_right_steering_link</parent>
<child>front_right_wheel_link</child>
<axis>
<xyz>0 1 0</xyz>
<limit>
<effort>0</effort>
<velocity>0</velocity>
<lower>-1e+16</lower>
<upper>1e+16</upper>
</limit>
<dynamics>
<damping>10.0</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
</axis>
</joint>
<link name='front_right_wheel_link'>
<pose relative_to='front_right_wheel_joint'>0 0 0 0 0 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>40</mass>
<inertia>
<ixx>1.4</ixx>
<iyy>2.5</iyy>
<izz>1.4</izz>
</inertia>
</inertial>
<collision name='front_right_wheel_link_collision'>
<pose>0 0 0 1.5708 0 0</pose>
<geometry>
<cylinder>
<length>0.35</length>
<radius>0.3</radius>
</cylinder>
</geometry>
<surface>
<friction>
<ode>
<mu>2.0</mu>
<mu2>1.5</mu2>
<slip1>0.0</slip1>
<slip2>0.0</slip2>
</ode>
</friction>
</surface>
</collision>
<visual name='front_right_wheel_link_visual'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<mesh>
<uri>model://four_wheeled_vehicle/meshes/wheel_r.dae</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
</visual>
</link>
<joint name='rear_left_wheel_joint' type='revolute'>
<pose relative_to='base_link'>-1.545 0.833 -0.024 0 0 0</pose>
<parent>base_link</parent>
<child>rear_left_wheel_link</child>
<axis>
<xyz>0 1 0</xyz>
<limit>
<effort>5000</effort>
<velocity>10000</velocity>
<lower>-1e+16</lower>
<upper>1e+16</upper>
</limit>
<dynamics>
<damping>10.0</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
</axis>
</joint>
<link name='rear_left_wheel_link'>
<pose relative_to='rear_left_wheel_joint'>0 0 0 0 0 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>40</mass>
<inertia>
<ixx>1.4</ixx>
<iyy>2.5</iyy>
<izz>1.4</izz>
</inertia>
</inertial>
<collision name='rear_left_wheel_link_collision'>
<pose>0 0 0 1.5708 0 0</pose>
<geometry>
<cylinder>
<length>0.35</length>
<radius>0.3</radius>
</cylinder>
</geometry>
<surface>
<friction>
<ode>
<mu>2.0</mu>
<mu2>1.5</mu2>
<slip1>0.0</slip1>
<slip2>0.0</slip2>
</ode>
</friction>
</surface>
</collision>
<visual name='rear_left_wheel_link_visual'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<mesh>
<uri>model://four_wheeled_vehicle/meshes/wheel_l.dae</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
</visual>
</link>
<joint name='rear_right_wheel_joint' type='revolute'>
<pose relative_to='base_link'>-1.545 -0.833 -0.024 0 0 0</pose>
<parent>base_link</parent>
<child>rear_right_wheel_link</child>
<axis>
<xyz>0 1 0</xyz>
<limit>
<effort>5000</effort>
<velocity>10000</velocity>
<lower>-1e+16</lower>
<upper>1e+16</upper>
</limit>
<dynamics>
<damping>10.0</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
</axis>
</joint>
<link name='rear_right_wheel_link'>
<pose relative_to='rear_right_wheel_joint'>0 0 0 0 0 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>40</mass>
<inertia>
<ixx>1.4</ixx>
<iyy>2.5</iyy>
<izz>1.4</izz>
</inertia>
</inertial>
<collision name='rear_right_wheel_link_collision'>
<pose>0 0 0 1.5708 0 0</pose>
<geometry>
<cylinder>
<length>0.35</length>
<radius>0.3</radius>
</cylinder>
</geometry>
<surface>
<friction>
<ode>
<mu>2.0</mu>
<mu2>1.5</mu2>
<slip1>0.0</slip1>
<slip2>0.0</slip2>
</ode>
</friction>
</surface>
</collision>
<visual name='rear_right_wheel_link_visual'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<mesh>
<uri>model://four_wheeled_vehicle/meshes/wheel_r.dae</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
</visual>
</link>
<frame name='inertial_joint' attached_to='base_link'>
<pose>0 0 0 0 0 0</pose>
</frame>
<frame name='main_mass' attached_to='inertial_joint' />
<plugin name="vehicle_controller" filename="libfour_wheeled_vehicle_plugin.so">
<ros>
<namespace>/vehicle</namespace>
<command_topic>vehicle_cmd</command_topic>
<status_topic>vehicle_status</status_topic>
<wheelbase>3.0</wheelbase>
<track_width>1.666</track_width>
<wheel_radius>0.3</wheel_radius>
<max_speed>20.0</max_speed>
<max_steering_angle>0.6</max_steering_angle>
</ros>
</plugin>
</model>
</sdf>
这里我们加载了车辆的外观模型car.dae、wheel_l.dae和wheel_r.dae，他们为SU7 Ultra的车身和左右轮的3D模型，碰撞还是简单3D几何模型。
注：如果觉得模型加载慢的话，可以考虑不使用dae文件模型，使用简单的3D几何代替，替换方法参考前面的文章《Gazebo仿真环境系列教程（六）：创建一个四轮小车模型》。
5.2 启动文件配置
vehicle_gazebo.launch.py 负责协调启动 Gazebo、加载模型和设置环境变量：
import os
from launch import LaunchDescription
from launch.actions import ExecuteProcess
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
# 设置模型路径
pkg_path = FindPackageShare('four_wheeled_vehicle').find('four_wheeled_vehicle')
model_path = os.path.join(pkg_path,'models')
model_sdf = os.path.join(model_path, 'four_wheeled_vehicle', 'model.sdf')
world_file = os.path.join(pkg_path, 'empty.world')
plugin_path = os.path.join(pkg_path, '../..', 'lib', 'four_wheeled_vehicle')
os.environ['GAZEBO_MODEL_PATH'] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{model_path}"
os.environ['GAZEBO_PLUGIN_PATH'] = f"{os.environ.get('GAZEBO_PLUGIN_PATH', '')}:{plugin_path}"
print("plugin_path: ", plugin_path)
print("world_file: ", world_file)
print("model_path: ", model_path)
return LaunchDescription([
# 启动 Gazebo 空世界
ExecuteProcess(
cmd=['gazebo', '--verbose', world_file, '-s', 'libgazebo_ros_factory.so'],
output='screen'
),
# 加载模型
ExecuteProcess(
cmd=[
'ros2', 'run', 'gazebo_ros', 'spawn_entity.py',
'-entity', 'four_wheeled_car',
'-file', model_sdf,
'-x', '0.0', '-y', '0.0', '-z', '1.0'
],
output='screen'
)
])
六、运行与测试
-
编译工程
colcon build --packages-select vehicle_msgs four_wheeled_vehicle source install/setup.bash -
启动仿真
ros2 launch four_wheeled_vehicle vehicle_gazebo.launch.py -
控制测试
另其一个终端，使用提供的
test.py发送控制指令并记录轨迹：source install/setup.bash python3 scripts/test.py脚本功能说明：
-
发布固定速度（2m/s）与转向角（0.3rad）指令
-
订阅
vehicle_status记录位置、速度等数据 -
运行50秒后自动停止，并生成轨迹、偏航角、速度曲线图表
-
运行效果如下：
Gazebo仿真环境系列教程（七）：构建小米 SU7 Ult
注：完整代码，公众号原文后台私信：”20250901“获取
扫一扫
197
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
