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

fix(nav2): align mkmini gazebo vehicle geometry

Browse Source
This commit is contained in:
chyoo
2026-08-12 10:17:21 +09:00
parent 22cdb0aff9
commit 42231d0681
3 changed files with 34 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/mkmini_gazebo_steering_plugin/src/steering_command_plugin.cpp
+21 -4
View File
@@ -21,6 +21,7 @@ class SteeringCommandPlugin : public gazebo::ModelPlugin
public: public:
void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) override void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) override
{ {
model_ = model;
left_joint_ = model->GetJoint(sdf->Get<std::string>("left_joint")); left_joint_ = model->GetJoint(sdf->Get<std::string>("left_joint"));
right_joint_ = model->GetJoint(sdf->Get<std::string>("right_joint")); right_joint_ = model->GetJoint(sdf->Get<std::string>("right_joint"));
if (!left_joint_ || !right_joint_) { if (!left_joint_ || !right_joint_) {
@@ -28,7 +29,8 @@ public:
return; return;
} }
wheelbase_ = sdf->Get<double>("wheelbase", 0.60).first; 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); node_ = gazebo_ros::Node::Get(sdf);
const auto topic = sdf->Get<std::string>("topic", "/mkmini/steering_command").first; const auto topic = sdf->Get<std::string>("topic", "/mkmini/steering_command").first;
subscription_ = node_->create_subscription<std_msgs::msg::Float64>( subscription_ = node_->create_subscription<std_msgs::msg::Float64>(
@@ -54,9 +56,21 @@ private:
{ {
std::lock_guard<std::mutex> lock(mutex_); std::lock_guard<std::mutex> lock(mutex_);
if (!has_target_ || !left_joint_ || !right_joint_) return; 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_); const auto [left_angle, right_angle] = AckermannAngles(target_angle_);
left_joint_->SetPosition(0, left_angle); const auto max_step = steering_speed_ * dt;
right_joint_->SetPosition(0, right_angle); 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 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); 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 left_joint_;
gazebo::physics::JointPtr right_joint_; gazebo::physics::JointPtr right_joint_;
gazebo::event::ConnectionPtr update_connection_; gazebo::event::ConnectionPtr update_connection_;
@@ -77,8 +92,10 @@ private:
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_subscription_; rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_subscription_;
std::mutex mutex_; std::mutex mutex_;
double wheelbase_{0.60}; double wheelbase_{0.60};
double track_width_{0.72}; double track_width_{0.51};
double steering_speed_{1.2};
double target_angle_{0.0}; double target_angle_{0.0};
gazebo::common::Time last_update_time_;
bool has_target_{false}; bool has_target_{false};
}; };
src/nav2_turtlebot_simulator/launch/simulator.launch.py
+1 -1
View File
@@ -393,7 +393,7 @@ def launch_gazebo(context):
executable='static_transform_publisher', executable='static_transform_publisher',
name='mkmini_lidar_tf', name='mkmini_lidar_tf',
arguments=[ arguments=[
'--x', '0', '--y', '0', '--z', '0.9', '--x', '0.15', '--y', '0', '--z', '0.578',
'--roll', '0', '--pitch', '0', '--yaw', '0', '--roll', '0', '--pitch', '0', '--yaw', '0',
'--frame-id', 'base_link', '--child-frame-id', 'os_lidar', '--frame-id', 'base_link', '--child-frame-id', 'os_lidar',
], ],
src/nav2_turtlebot_simulator/models/mkmini_gazebo/model.sdf
+12 -12
View File
@@ -4,34 +4,34 @@
Its visible front wheels are separate steering links so the Gazebo view Its visible front wheels are separate steering links so the Gazebo view
follows real left/right Ackermann angles instead of a rotating axle. --> follows real left/right Ackermann angles instead of a rotating axle. -->
<model name="mkmini_gazebo"> <model name="mkmini_gazebo">
<pose>0 0 0.16 0 0 0</pose> <pose>0 0 0.12 0 0 0</pose>
<link name="base_link"> <link name="base_link">
<inertial><mass>38.0</mass><inertia><ixx>2.0</ixx><iyy>2.6</iyy><izz>3.2</izz></inertia></inertial> <inertial><mass>38.0</mass><inertia><ixx>2.0</ixx><iyy>2.6</iyy><izz>3.2</izz></inertia></inertial>
<collision name="chassis_collision"><pose>0 0 0.41 0 0 0</pose><geometry><box><size>1.05 0.70 0.50</size></box></geometry></collision> <collision name="chassis_collision"><pose>0 0 0.334 0 0 0</pose><geometry><box><size>0.84 0.61 0.407</size></box></geometry></collision>
<visual name="chassis_visual"><pose>0 0 0.41 0 0 0</pose><geometry><box><size>1.05 0.70 0.50</size></box></geometry><material><ambient>0.12 0.20 0.25 1</ambient><diffuse>0.12 0.35 0.42 1</diffuse></material></visual> <visual name="chassis_visual"><pose>0 0 0.334 0 0 0</pose><geometry><box><size>0.84 0.61 0.407</size></box></geometry><material><ambient>0.12 0.20 0.25 1</ambient><diffuse>0.12 0.35 0.42 1</diffuse></material></visual>
<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> <visual name="front_marker"><pose>0.42 0 0.515 0 0 0</pose><geometry><box><size>0.049 0.366 0.081</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>
<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> <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> <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> <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.12</radius><length>0.05</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> <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> <link name="front_left_visual_wheel"><pose>0.30 0.255 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.12</radius><length>0.05</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> <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> <link name="front_right_visual_wheel"><pose>0.30 -0.255 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.12</radius><length>0.05</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> <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> <link name="rear_left_wheel"><pose>-0.30 0.255 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.12</radius><length>0.05</length></cylinder></geometry></collision><visual name="visual"><pose>0 0 0 1.5708 0 0</pose><geometry><cylinder><radius>0.12</radius><length>0.05</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> <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> <link name="rear_right_wheel"><pose>-0.30 -0.255 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.12</radius><length>0.05</length></cylinder></geometry></collision><visual name="visual"><pose>0 0 0 1.5708 0 0</pose><geometry><cylinder><radius>0.12</radius><length>0.05</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> <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.15 0 0.578 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.061</radius><length>0.081</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> <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.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_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.51</wheel_separation><encoder_wheel_diameter>0.24</encoder_wheel_diameter><actuated_wheel_diameter>0.24</actuated_wheel_diameter><max_wheel_torque>120.0</max_wheel_torque><max_wheel_acceleration>1.5</max_wheel_acceleration><max_steering_speed>1.2</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> <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.51</track_width><steering_speed>1.2</steering_speed></plugin>
</model> </model>
</sdf> </sdf>