From 14580aaa8865b4ef8a835baf7469b1a9d99f985c Mon Sep 17 00:00:00 2001
From: chyoo <chyoo@futuredrive.net>
Date: Wed, 19 Aug 2026 09:53:09 +0900
Subject: [PATCH] Align MKMini Gazebo steering and world scale

---
 .../src/steering_command_plugin.cpp                 | 13 ++++++++++++-
 .../launch/simulator.launch.py                      |  8 ++++----
 2 files changed, 16 insertions(+), 5 deletions(-)

diff --git a/src/mkmini_gazebo_steering_plugin/src/steering_command_plugin.cpp b/src/mkmini_gazebo_steering_plugin/src/steering_command_plugin.cpp
index 9d7afe8..3659211 100644
--- a/src/mkmini_gazebo_steering_plugin/src/steering_command_plugin.cpp
+++ b/src/mkmini_gazebo_steering_plugin/src/steering_command_plugin.cpp
@@ -44,7 +44,18 @@ public:
       "/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);
+        // Twist.angular.z is yaw rate, not steering angle. Convert it using
+        // the signed vehicle speed so reverse Reeds-Shepp segments keep the
+        // same physical curvature as their planned path.
+        if (std::abs(message->linear.x) >= 0.01) {
+          target_angle_ = std::clamp(
+            std::atan(wheelbase_ * message->angular.z / message->linear.x),
+            -0.5,
+            0.5);
+        } else {
+          // Preserve the direct angle fallback used by manual J/L steering.
+          target_angle_ = std::clamp(message->angular.z, -0.5, 0.5);
+        }
         has_target_ = true;
       });
     update_connection_ = gazebo::event::Events::ConnectWorldUpdateBegin(
diff --git a/src/nav2_turtlebot_simulator/launch/simulator.launch.py b/src/nav2_turtlebot_simulator/launch/simulator.launch.py
index 3713ccc..05d5187 100644
--- a/src/nav2_turtlebot_simulator/launch/simulator.launch.py
+++ b/src/nav2_turtlebot_simulator/launch/simulator.launch.py
@@ -33,7 +33,7 @@ LOCAL_SUN_LIGHT = '''    <light type="directional" name="sun">
       <cast_shadows>false</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><direction>-0.5 0.1 -0.9</direction>
     </light>'''
 
-MKMINI_WORLD_SPACING_SCALE = 1.6
+MKMINI_WORLD_SPACING_SCALE = 1.0
 
 
 def replace_house_visual_meshes_with_boxes(house_model):
@@ -177,7 +177,7 @@ def create_flat_house_world(gazebo_dir):
 
 
 def create_mkmini_world(gazebo_dir):
-    """Expand the TurtleBot3 obstacle layout without changing obstacle sizes."""
+    """Create the MKMini world variant without changing map-scale coordinates."""
     source_model_path = os.path.join(gazebo_dir, 'models', 'turtlebot3_world', 'model.sdf')
     models_dir = os.path.join(tempfile.gettempdir(), 'nav2_mkmini_models')
     model_dir = os.path.join(models_dir, 'turtlebot3_world_mkmini')
@@ -190,8 +190,8 @@ def create_mkmini_world(gazebo_dir):
         raise RuntimeError('turtlebot3_world model is missing from model.sdf')
     model.attrib['name'] = 'turtlebot3_world_mkmini'
 
-    # Only object positions change. Keeping their geometry unchanged widens the
-    # driving lanes for the larger MKMini collision footprint.
+    # Keep object geometry and coordinates aligned with the base world. MKMini
+    # clearance comes from its footprint and inflation configuration.
     for pose in model.findall('.//pose'):
         values = pose.text.split()
         if len(values) != 6: