diff --git a/src/mkmini_gazebo_steering_plugin/CMakeLists.txt b/src/mkmini_gazebo_steering_plugin/CMakeLists.txt
new file mode 100644
index 0000000..0b69002
--- /dev/null
+++ b/src/mkmini_gazebo_steering_plugin/CMakeLists.txt
@@ -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()
diff --git a/src/mkmini_gazebo_steering_plugin/package.xml b/src/mkmini_gazebo_steering_plugin/package.xml
new file mode 100644
index 0000000..a9adcb0
--- /dev/null
+++ b/src/mkmini_gazebo_steering_plugin/package.xml
@@ -0,0 +1,13 @@
+
+
+ mkmini_gazebo_steering_plugin
+ 1.0.0
+ Independent visual steering for the Gazebo MK-mini front wheels.
+ Autonomy Studio
+ Apache-2.0
+ ament_cmake
+ gazebo_ros
+ geometry_msgs
+ rclcpp
+ std_msgs
+
diff --git a/src/mkmini_gazebo_steering_plugin/src/steering_command_plugin.cpp b/src/mkmini_gazebo_steering_plugin/src/steering_command_plugin.cpp
new file mode 100644
index 0000000..802ffd2
--- /dev/null
+++ b/src/mkmini_gazebo_steering_plugin/src/steering_command_plugin.cpp
@@ -0,0 +1,86 @@
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+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("left_joint"));
+ right_joint_ = model->GetJoint(sdf->Get("right_joint"));
+ if (!left_joint_ || !right_joint_) {
+ gzerr << "MK-mini visual steering joints were not found.\n";
+ return;
+ }
+ wheelbase_ = sdf->Get("wheelbase", 0.60).first;
+ track_width_ = sdf->Get("track_width", 0.72).first;
+ node_ = gazebo_ros::Node::Get(sdf);
+ const auto topic = sdf->Get("topic", "/mkmini/steering_command").first;
+ subscription_ = node_->create_subscription(
+ topic, rclcpp::QoS(1),
+ [this](const std_msgs::msg::Float64::SharedPtr message) {
+ std::lock_guard lock(mutex_);
+ target_angle_ = std::clamp(message->data, -0.5, 0.5);
+ has_target_ = true;
+ });
+ cmd_vel_subscription_ = node_->create_subscription(
+ "/cmd_vel", rclcpp::QoS(10),
+ [this](const geometry_msgs::msg::Twist::SharedPtr message) {
+ std::lock_guard 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 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 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::SharedPtr subscription_;
+ rclcpp::Subscription::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
diff --git a/src/nav2_turtlebot_simulator/CMakeLists.txt b/src/nav2_turtlebot_simulator/CMakeLists.txt
index a11cfc3..30b70f5 100644
--- a/src/nav2_turtlebot_simulator/CMakeLists.txt
+++ b/src/nav2_turtlebot_simulator/CMakeLists.txt
@@ -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})
diff --git a/src/nav2_turtlebot_simulator/launch/simulator.launch.py b/src/nav2_turtlebot_simulator/launch/simulator.launch.py
index 324ecd3..e74be44 100644
--- a/src/nav2_turtlebot_simulator/launch/simulator.launch.py
+++ b/src/nav2_turtlebot_simulator/launch/simulator.launch.py
@@ -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')):
- 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:
- continue
- values = pose.text.split()
- if len(values) == 6:
+ # 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
+ pose = element.find('pose')
+ values = pose.text.split() if pose is not None and pose.text else []
+ if len(values) != 6:
+ continue
+ 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)
diff --git a/src/nav2_turtlebot_simulator/models/mkmini_gazebo/model.sdf b/src/nav2_turtlebot_simulator/models/mkmini_gazebo/model.sdf
index 63e3325..129d046 100644
--- a/src/nav2_turtlebot_simulator/models/mkmini_gazebo/model.sdf
+++ b/src/nav2_turtlebot_simulator/models/mkmini_gazebo/model.sdf
@@ -1,8 +1,8 @@
-
+
0 0 0.16 0 0 0
@@ -13,37 +13,25 @@
0.56 0 0.68 0 0 00.06 0.42 0.100.95 0.65 0.08 10.95 0.65 0.08 1
- 0.30 0 0 0 0 01.00.010.010.01
-
- 0 0.36 0 1.5708 0 00.140.100.04 0.04 0.04 10.04 0.04 0.04 1
- 0 -0.36 0 1.5708 0 00.140.100.04 0.04 0.04 10.04 0.04 0.04 1
-
+ 0.30 0 0 0 0 01.00.010.010.01
base_linkfront_fork0 0 1-0.50.5
-
0.30 0 0 0 0 01.20.020.010.020 0 0 1.5708 0 00.140.18
front_forkfront_drive_wheel0 1 0
+ 0.30 0.36 0 0 0 00.050.00010.00010.00010 0 0 1.5708 0 00.140.100.04 0.04 0.04 10.04 0.04 0.04 1
+ base_linkfront_left_visual_wheel0 0 1-0.60.6
+ 0.30 -0.36 0 0 0 00.050.00010.00010.00010 0 0 1.5708 0 00.140.100.04 0.04 0.04 10.04 0.04 0.04 1
+ base_linkfront_right_visual_wheel0 0 1-0.60.6
+
-0.30 0.36 0 0 0 01.00.020.010.020 0 0 1.5708 0 00.140.100 0 0 1.5708 0 00.140.100.04 0.04 0.04 10.04 0.04 0.04 1
base_linkrear_left_wheel0 1 0
-
-0.30 -0.36 0 0 0 01.00.020.010.020 0 0 1.5708 0 00.140.100 0 0 1.5708 0 00.140.100.04 0.04 0.04 10.04 0.04 0.04 1
base_linkrear_right_wheel0 1 0
- 0 0 0.9 0 0 00.20.0010.0010.001
- 0.0750.100.85 0.12 0.10 10.85 0.12 0.10 1
- true10true3601-3.141593.1415911000.2018.00.02~/out:=/raw/lidarsensor_msgs/PointCloud2os_lidar
-
+ 0 0 0.9 0 0 00.20.0010.0010.0010.0750.100.85 0.12 0.10 10.85 0.12 0.10 1true10true3601-3.141593.1415911000.2018.00.02~/out:=/raw/lidarsensor_msgs/PointCloud2os_lidar
base_linkos_lidar
-
- cmd_vel:=/cmd_velodom:=/odom
- front_steering_jointfront_drive_joint
- rear_left_jointrear_right_joint
- 0.720.280.28
- 120.01.50.8
- truetruetrue
- odombase_link
-
+ cmd_vel:=/cmd_velodom:=/odomfront_steering_jointfront_drive_jointrear_left_jointrear_right_joint0.720.280.28120.01.50.0truetruetrueodombase_link
+ /mkmini/steering_commandfront_left_visual_steering_jointfront_right_visual_steering_joint0.600.72
diff --git a/src/nav2_turtlebot_simulator/package.xml b/src/nav2_turtlebot_simulator/package.xml
index 47de6a7..dad7608 100644
--- a/src/nav2_turtlebot_simulator/package.xml
+++ b/src/nav2_turtlebot_simulator/package.xml
@@ -6,6 +6,7 @@
Autonomy Studio
Apache-2.0
ament_cmake
+ mkmini_gazebo_steering_plugin
gazebo_ros
launch
launch_ros