fix(nav2): align mkmini gazebo vehicle geometry

src/mkmini_gazebo_steering_plugin/src/steering_command_plugin.cpp

@@ -21,6 +21,7 @@ class SteeringCommandPlugin : public gazebo::ModelPlugin
 public:
   void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) override
   {
+    model_ = model;
     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_) {
@@ -28,7 +29,8 @@ public:
       return;
     }
     wheelbase_ = sdf->Get<double>("wheelbase", 0.60).first;
-    track_width_ = sdf->Get<double>("track_width", 0.72).first;
+    track_width_ = sdf->Get<double>("track_width", 0.51).first;
+    steering_speed_ = sdf->Get<double>("steering_speed", 1.2).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>(
@@ -54,9 +56,21 @@ private:
   {
     std::lock_guard<std::mutex> lock(mutex_);
     if (!has_target_ || !left_joint_ || !right_joint_) return;
+
+    const auto now = model_->GetWorld()->SimTime();
+    if (last_update_time_ == gazebo::common::Time::Zero) {
+      last_update_time_ = now;
+      return;
+    }
+    const auto dt = std::clamp((now - last_update_time_).Double(), 0.0, 0.1);
+    last_update_time_ = now;
     const auto [left_angle, right_angle] = AckermannAngles(target_angle_);
-    left_joint_->SetPosition(0, left_angle);
-    right_joint_->SetPosition(0, right_angle);
+    const auto max_step = steering_speed_ * dt;
+    const auto step_towards = [max_step](double current, double target) {
+      return current + std::clamp(target - current, -max_step, max_step);
+    };
+    left_joint_->SetPosition(0, step_towards(left_joint_->Position(0), left_angle));
+    right_joint_->SetPosition(0, step_towards(right_joint_->Position(0), right_angle));
   }
 
   std::pair<double, double> AckermannAngles(double steering_angle) const
@@ -69,6 +83,7 @@ private:
     return direction > 0.0 ? std::make_pair(inner, outer) : std::make_pair(-outer, -inner);
   }
 
+  gazebo::physics::ModelPtr model_;
   gazebo::physics::JointPtr left_joint_;
   gazebo::physics::JointPtr right_joint_;
   gazebo::event::ConnectionPtr update_connection_;
@@ -77,8 +92,10 @@ private:
   rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_subscription_;
   std::mutex mutex_;
   double wheelbase_{0.60};
-  double track_width_{0.72};
+  double track_width_{0.51};
+  double steering_speed_{1.2};
   double target_angle_{0.0};
+  gazebo::common::Time last_update_time_;
   bool has_target_{false};
 };