templates/lms-nav2-gazebo-simulator
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix(nav2): improve mkmini gazebo steering simulation

Browse Source
This commit is contained in:
chyoo
2026-08-10 09:02:16 +09:00
parent 68e0087bcb
commit 22cdb0aff9
7 changed files with 155 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/mkmini_gazebo_steering_plugin/CMakeLists.txt
+15
View File
@@ -0,0 +1,15 @@
cmake_minimum_required(VERSION 3.8)
project(mkmini_gazebo_steering_plugin)
find_package(ament_cmake REQUIRED)
find_package(gazebo_ros REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
add_library(${PROJECT_NAME} SHARED src/steering_command_plugin.cpp)
target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17)
ament_target_dependencies(${PROJECT_NAME} gazebo_ros geometry_msgs rclcpp std_msgs)
install(TARGETS ${PROJECT_NAME} LIBRARY DESTINATION lib)
ament_package()
src/mkmini_gazebo_steering_plugin/package.xml
+13
View File
@@ -0,0 +1,13 @@
<?xml version="1.0"?>
<package format="3">
<name>mkmini_gazebo_steering_plugin</name>
<version>1.0.0</version>
<description>Independent visual steering for the Gazebo MK-mini front wheels.</description>
<maintainer email="platform@autonomystudio.net">Autonomy Studio</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>gazebo_ros</depend>
<depend>geometry_msgs</depend>
<depend>rclcpp</depend>
<depend>std_msgs</depend>
</package>
src/mkmini_gazebo_steering_plugin/src/steering_command_plugin.cpp
+86
View File
@@ -0,0 +1,86 @@
#include <algorithm>
#include <cmath>
#include <functional>
#include <memory>
#include <mutex>
#include <string>
#include <utility>
#include <gazebo/common/Events.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo_ros/node.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/float64.hpp>
namespace mkmini_gazebo_steering_plugin
{
class SteeringCommandPlugin : public gazebo::ModelPlugin
{
public:
void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) override
{
left_joint_ = model->GetJoint(sdf->Get<std::string>("left_joint"));
right_joint_ = model->GetJoint(sdf->Get<std::string>("right_joint"));
if (!left_joint_ || !right_joint_) {
gzerr << "MK-mini visual steering joints were not found.\n";
return;
}
wheelbase_ = sdf->Get<double>("wheelbase", 0.60).first;
track_width_ = sdf->Get<double>("track_width", 0.72).first;
node_ = gazebo_ros::Node::Get(sdf);
const auto topic = sdf->Get<std::string>("topic", "/mkmini/steering_command").first;
subscription_ = node_->create_subscription<std_msgs::msg::Float64>(
topic, rclcpp::QoS(1),
[this](const std_msgs::msg::Float64::SharedPtr message) {
std::lock_guard<std::mutex> lock(mutex_);
target_angle_ = std::clamp(message->data, -0.5, 0.5);
has_target_ = true;
});
cmd_vel_subscription_ = node_->create_subscription<geometry_msgs::msg::Twist>(
"/cmd_vel", rclcpp::QoS(10),
[this](const geometry_msgs::msg::Twist::SharedPtr message) {
std::lock_guard<std::mutex> lock(mutex_);
target_angle_ = std::clamp(message->angular.z, -0.5, 0.5);
has_target_ = true;
});
update_connection_ = gazebo::event::Events::ConnectWorldUpdateBegin(
std::bind(&SteeringCommandPlugin::OnUpdate, this));
}
private:
void OnUpdate()
{
std::lock_guard<std::mutex> lock(mutex_);
if (!has_target_ || !left_joint_ || !right_joint_) return;
const auto [left_angle, right_angle] = AckermannAngles(target_angle_);
left_joint_->SetPosition(0, left_angle);
right_joint_->SetPosition(0, right_angle);
}
std::pair<double, double> AckermannAngles(double steering_angle) const
{
if (std::abs(steering_angle) < 1e-6) return {0.0, 0.0};
const auto direction = steering_angle > 0.0 ? 1.0 : -1.0;
const auto radius = wheelbase_ / std::tan(std::abs(steering_angle));
const auto inner = std::atan(wheelbase_ / std::max(0.01, radius - track_width_ * 0.5));
const auto outer = std::atan(wheelbase_ / (radius + track_width_ * 0.5));
return direction > 0.0 ? std::make_pair(inner, outer) : std::make_pair(-outer, -inner);
}
gazebo::physics::JointPtr left_joint_;
gazebo::physics::JointPtr right_joint_;
gazebo::event::ConnectionPtr update_connection_;
gazebo_ros::Node::SharedPtr node_;
rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr subscription_;
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_subscription_;
std::mutex mutex_;
double wheelbase_{0.60};
double track_width_{0.72};
double target_angle_{0.0};
bool has_target_{false};
};
GZ_REGISTER_MODEL_PLUGIN(SteeringCommandPlugin)
} // namespace mkmini_gazebo_steering_plugin
src/nav2_turtlebot_simulator/CMakeLists.txt
+1
View File
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.8)
project(nav2_turtlebot_simulator)
find_package(ament_cmake REQUIRED)
find_package(mkmini_gazebo_steering_plugin REQUIRED)
install(DIRECTORY launch worlds models DESTINATION share/${PROJECT_NAME})
src/nav2_turtlebot_simulator/launch/simulator.launch.py
+20 -8
View File
@@ -220,21 +220,33 @@ def create_mkmini_world(gazebo_dir):
except ValueError:
continue
# The MKMini lidar is centered at 0.52 m. Raise the original 0.5 m white
# pillars so they remain observable obstacles instead of falling below the
# scan plane.
for element in (*model.findall('.//collision'), *model.findall('.//visual')):
# Keep only the centre post from TurtleBot3 World's 3x3 white-pillar grid.
# This is applied to the generated MKMini world only; Burger keeps the
# original obstacle layout. Remove both visual and collision shapes so the
# Gazebo scene and Nav2 costmap describe the same space.
for link in model.findall('.//link'):
for element in list(link):
if element.tag not in {'collision', 'visual'}:
continue
cylinder = element.find('./geometry/cylinder')
if cylinder is None:
continue
if cylinder.findtext('radius') != '0.15' or cylinder.findtext('length') != '0.5':
continue
cylinder.find('length').text = '1.2'
pose = element.find('pose')
if pose is None:
values = pose.text.split() if pose is not None and pose.text else []
if len(values) != 6:
continue
values = pose.text.split()
if len(values) == 6:
try:
x, y = float(values[0]), float(values[1])
except ValueError:
continue
if abs(x) > 1e-6 or abs(y) > 1e-6:
link.remove(element)
continue
# The MKMini lidar is centered at 0.52 m. Raise the remaining
# centre post so it stays observable instead of below the scan.
cylinder.find('length').text = '1.2'
values[2] = '0.6'
pose.text = ' '.join(values)
src/nav2_turtlebot_simulator/models/mkmini_gazebo/model.sdf
+12 -24
View File
@@ -1,8 +1,8 @@
<?xml version="1.0"?>
<sdf version="1.6">
<!-- Lightweight MK-mini approximation. The body dimensions and wheelbase
match the platform profile; Gazebo's tricycle plugin supplies stable
non-holonomic Ackermann-like motion from the standard /cmd_vel topic. -->
<!-- MK-mini's proven tricycle drive remains the collision and odometry model.
Its visible front wheels are separate steering links so the Gazebo view
follows real left/right Ackermann angles instead of a rotating axle. -->
<model name="mkmini_gazebo">
<pose>0 0 0.16 0 0 0</pose>
@@ -13,37 +13,25 @@
<visual name="front_marker"><pose>0.56 0 0.68 0 0 0</pose><geometry><box><size>0.06 0.42 0.10</size></box></geometry><material><ambient>0.95 0.65 0.08 1</ambient><diffuse>0.95 0.65 0.08 1</diffuse></material></visual>
</link>
<link name="front_fork"><pose>0.30 0 0 0 0 0</pose><inertial><mass>1.0</mass><inertia><ixx>0.01</ixx><iyy>0.01</iyy><izz>0.01</izz></inertia></inertial>
<!-- Visual front-wheel pair. The tricycle plugin retains its hidden
centre drive wheel for stable steering and odometry. -->
<visual name="front_left_wheel_visual"><pose>0 0.36 0 1.5708 0 0</pose><geometry><cylinder><radius>0.14</radius><length>0.10</length></cylinder></geometry><material><ambient>0.04 0.04 0.04 1</ambient><diffuse>0.04 0.04 0.04 1</diffuse></material></visual>
<visual name="front_right_wheel_visual"><pose>0 -0.36 0 1.5708 0 0</pose><geometry><cylinder><radius>0.14</radius><length>0.10</length></cylinder></geometry><material><ambient>0.04 0.04 0.04 1</ambient><diffuse>0.04 0.04 0.04 1</diffuse></material></visual>
</link>
<link name="front_fork"><pose>0.30 0 0 0 0 0</pose><inertial><mass>1.0</mass><inertia><ixx>0.01</ixx><iyy>0.01</iyy><izz>0.01</izz></inertia></inertial></link>
<joint name="front_steering_joint" type="revolute"><parent>base_link</parent><child>front_fork</child><axis><xyz>0 0 1</xyz><limit><lower>-0.5</lower><upper>0.5</upper></limit></axis></joint>
<link name="front_drive_wheel"><pose>0.30 0 0 0 0 0</pose><inertial><mass>1.2</mass><inertia><ixx>0.02</ixx><iyy>0.01</iyy><izz>0.02</izz></inertia></inertial><collision name="collision"><pose>0 0 0 1.5708 0 0</pose><geometry><cylinder><radius>0.14</radius><length>0.18</length></cylinder></geometry></collision></link>
<joint name="front_drive_joint" type="revolute"><parent>front_fork</parent><child>front_drive_wheel</child><axis><xyz>0 1 0</xyz></axis></joint>
<link name="front_left_visual_wheel"><pose>0.30 0.36 0 0 0 0</pose><inertial><mass>0.05</mass><inertia><ixx>0.0001</ixx><iyy>0.0001</iyy><izz>0.0001</izz></inertia></inertial><visual name="visual"><pose>0 0 0 1.5708 0 0</pose><geometry><cylinder><radius>0.14</radius><length>0.10</length></cylinder></geometry><material><ambient>0.04 0.04 0.04 1</ambient><diffuse>0.04 0.04 0.04 1</diffuse></material></visual></link>
<joint name="front_left_visual_steering_joint" type="revolute"><parent>base_link</parent><child>front_left_visual_wheel</child><axis><xyz>0 0 1</xyz><limit><lower>-0.6</lower><upper>0.6</upper></limit></axis></joint>
<link name="front_right_visual_wheel"><pose>0.30 -0.36 0 0 0 0</pose><inertial><mass>0.05</mass><inertia><ixx>0.0001</ixx><iyy>0.0001</iyy><izz>0.0001</izz></inertia></inertial><visual name="visual"><pose>0 0 0 1.5708 0 0</pose><geometry><cylinder><radius>0.14</radius><length>0.10</length></cylinder></geometry><material><ambient>0.04 0.04 0.04 1</ambient><diffuse>0.04 0.04 0.04 1</diffuse></material></visual></link>
<joint name="front_right_visual_steering_joint" type="revolute"><parent>base_link</parent><child>front_right_visual_wheel</child><axis><xyz>0 0 1</xyz><limit><lower>-0.6</lower><upper>0.6</upper></limit></axis></joint>
<link name="rear_left_wheel"><pose>-0.30 0.36 0 0 0 0</pose><inertial><mass>1.0</mass><inertia><ixx>0.02</ixx><iyy>0.01</iyy><izz>0.02</izz></inertia></inertial><collision name="collision"><pose>0 0 0 1.5708 0 0</pose><geometry><cylinder><radius>0.14</radius><length>0.10</length></cylinder></geometry></collision><visual name="visual"><pose>0 0 0 1.5708 0 0</pose><geometry><cylinder><radius>0.14</radius><length>0.10</length></cylinder></geometry><material><ambient>0.04 0.04 0.04 1</ambient><diffuse>0.04 0.04 0.04 1</diffuse></material></visual></link>
<joint name="rear_left_joint" type="revolute"><parent>base_link</parent><child>rear_left_wheel</child><axis><xyz>0 1 0</xyz></axis></joint>
<link name="rear_right_wheel"><pose>-0.30 -0.36 0 0 0 0</pose><inertial><mass>1.0</mass><inertia><ixx>0.02</ixx><iyy>0.01</iyy><izz>0.02</izz></inertia></inertial><collision name="collision"><pose>0 0 0 1.5708 0 0</pose><geometry><cylinder><radius>0.14</radius><length>0.10</length></cylinder></geometry></collision><visual name="visual"><pose>0 0 0 1.5708 0 0</pose><geometry><cylinder><radius>0.14</radius><length>0.10</length></cylinder></geometry><material><ambient>0.04 0.04 0.04 1</ambient><diffuse>0.04 0.04 0.04 1</diffuse></material></visual></link>
<joint name="rear_right_joint" type="revolute"><parent>base_link</parent><child>rear_right_wheel</child><axis><xyz>0 1 0</xyz></axis></joint>
<link name="os_lidar"><pose>0 0 0.9 0 0 0</pose><inertial><mass>0.2</mass><inertia><ixx>0.001</ixx><iyy>0.001</iyy><izz>0.001</izz></inertia></inertial>
<visual name="visual"><geometry><cylinder><radius>0.075</radius><length>0.10</length></cylinder></geometry><material><ambient>0.85 0.12 0.10 1</ambient><diffuse>0.85 0.12 0.10 1</diffuse></material></visual>
<sensor name="lidar_2d" type="ray"><always_on>true</always_on><update_rate>10</update_rate><visualize>true</visualize><ray><scan><horizontal><samples>360</samples><resolution>1</resolution><min_angle>-3.14159</min_angle><max_angle>3.14159</max_angle></horizontal><vertical><samples>1</samples><resolution>1</resolution><min_angle>0</min_angle><max_angle>0</max_angle></vertical></scan><range><min>0.20</min><max>18.0</max><resolution>0.02</resolution></range></ray><plugin name="mkmini_lidar" filename="libgazebo_ros_ray_sensor.so"><ros><remapping>~/out:=/raw/lidar</remapping></ros><output_type>sensor_msgs/PointCloud2</output_type><frame_name>os_lidar</frame_name></plugin></sensor>
</link>
<link name="os_lidar"><pose>0 0 0.9 0 0 0</pose><inertial><mass>0.2</mass><inertia><ixx>0.001</ixx><iyy>0.001</iyy><izz>0.001</izz></inertia></inertial><visual name="visual"><geometry><cylinder><radius>0.075</radius><length>0.10</length></cylinder></geometry><material><ambient>0.85 0.12 0.10 1</ambient><diffuse>0.85 0.12 0.10 1</diffuse></material></visual><sensor name="lidar_2d" type="ray"><always_on>true</always_on><update_rate>10</update_rate><visualize>true</visualize><ray><scan><horizontal><samples>360</samples><resolution>1</resolution><min_angle>-3.14159</min_angle><max_angle>3.14159</max_angle></horizontal><vertical><samples>1</samples><resolution>1</resolution><min_angle>0</min_angle><max_angle>0</max_angle></vertical></scan><range><min>0.20</min><max>18.0</max><resolution>0.02</resolution></range></ray><plugin name="mkmini_lidar" filename="libgazebo_ros_ray_sensor.so"><ros><remapping>~/out:=/raw/lidar</remapping></ros><output_type>sensor_msgs/PointCloud2</output_type><frame_name>os_lidar</frame_name></plugin></sensor></link>
<joint name="os_lidar_joint" type="fixed"><parent>base_link</parent><child>os_lidar</child></joint>
<plugin name="mkmini_tricycle" filename="libgazebo_ros_tricycle_drive.so">
<ros><remapping>cmd_vel:=/cmd_vel</remapping><remapping>odom:=/odom</remapping></ros>
<steering_joint>front_steering_joint</steering_joint><actuated_wheel_joint>front_drive_joint</actuated_wheel_joint>
<encoder_wheel_left_joint>rear_left_joint</encoder_wheel_left_joint><encoder_wheel_right_joint>rear_right_joint</encoder_wheel_right_joint>
<wheel_separation>0.72</wheel_separation><encoder_wheel_diameter>0.28</encoder_wheel_diameter><actuated_wheel_diameter>0.28</actuated_wheel_diameter>
<max_wheel_torque>120.0</max_wheel_torque><max_wheel_acceleration>1.5</max_wheel_acceleration><max_steering_speed>0.8</max_steering_speed>
<publish_odom>true</publish_odom><publish_wheel_tf>true</publish_wheel_tf><publish_wheel_joint_state>true</publish_wheel_joint_state>
<odometry_frame>odom</odometry_frame><robot_base_frame>base_link</robot_base_frame>
</plugin>
<plugin name="mkmini_tricycle" filename="libgazebo_ros_tricycle_drive.so"><ros><remapping>cmd_vel:=/cmd_vel</remapping><remapping>odom:=/odom</remapping></ros><steering_joint>front_steering_joint</steering_joint><actuated_wheel_joint>front_drive_joint</actuated_wheel_joint><encoder_wheel_left_joint>rear_left_joint</encoder_wheel_left_joint><encoder_wheel_right_joint>rear_right_joint</encoder_wheel_right_joint><wheel_separation>0.72</wheel_separation><encoder_wheel_diameter>0.28</encoder_wheel_diameter><actuated_wheel_diameter>0.28</actuated_wheel_diameter><max_wheel_torque>120.0</max_wheel_torque><max_wheel_acceleration>1.5</max_wheel_acceleration><max_steering_speed>0.0</max_steering_speed><publish_odom>true</publish_odom><publish_wheel_tf>true</publish_wheel_tf><publish_wheel_joint_state>true</publish_wheel_joint_state><odometry_frame>odom</odometry_frame><robot_base_frame>base_link</robot_base_frame></plugin>
<plugin name="mkmini_visual_steering" filename="/modules/[lms-nav2-gazebo-simulator]nav2-simulator_v1.0.0_ws/install/mkmini_gazebo_steering_plugin/lib/libmkmini_gazebo_steering_plugin.so"><topic>/mkmini/steering_command</topic><left_joint>front_left_visual_steering_joint</left_joint><right_joint>front_right_visual_steering_joint</right_joint><wheelbase>0.60</wheelbase><track_width>0.72</track_width></plugin>
</model>
</sdf>
src/nav2_turtlebot_simulator/package.xml
+1
View File
@@ -6,6 +6,7 @@
<maintainer email="platform@autonomystudio.net">Autonomy Studio</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>mkmini_gazebo_steering_plugin</build_depend>
<exec_depend>gazebo_ros</exec_depend>
<exec_depend>launch</exec_depend>
<exec_depend>launch_ros</exec_depend>