fix(nav2): improve mkmini gazebo steering simulation

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()

src/mkmini_gazebo_steering_plugin/package.xml

@@ -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

@@ -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