From 2f614c44ff44a6bdc9e7cbe2b744040feb3e93d9 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Sun, 29 Dec 2024 18:07:15 +0000
Subject: [PATCH 1/3] Use meaningful inertial parameter

---
 .../urdf/test_ackermann_drive.xacro.urdf      | 20 +++++++++----------
 1 file changed, 10 insertions(+), 10 deletions(-)

diff --git a/gz_ros2_control_demos/urdf/test_ackermann_drive.xacro.urdf b/gz_ros2_control_demos/urdf/test_ackermann_drive.xacro.urdf
index 99819e25..3bcaea91 100644
--- a/gz_ros2_control_demos/urdf/test_ackermann_drive.xacro.urdf
+++ b/gz_ros2_control_demos/urdf/test_ackermann_drive.xacro.urdf
@@ -34,8 +34,8 @@
 
     <inertial>
       <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.126164" ixy="0.0" ixz="0.0" iyy="0.416519" iyz="0.0" izz="0.481014" />
+      <mass value="100" />
+      <inertia ixx="10.4" ixy="0.0" ixz="0.0" iyy="35.4" iyz="0.0" izz="41.66" />
     </inertial>
   </link>
 
@@ -60,8 +60,8 @@
     </visual>
 
     <inertial>
-      <mass value="2" />
-      <inertia ixx="0.145833" ixy="0.0" ixz="0.0" iyy="0.145833" iyz="0.0" izz="0.125" />
+      <mass value="11.3" />
+      <inertia ixx="0.26" ixy="0.0" ixz="0.0" iyy="0.26" iyz="0.0" izz="0.51" />
     </inertial>
   </link>
 
@@ -87,8 +87,8 @@
       <material name="Black" />
     </visual>
     <inertial>
-      <mass value="2" />
-      <inertia ixx="0.145833" ixy="0.0" ixz="0.0" iyy="0.145833" iyz="0.0" izz="0.125" />
+      <mass value="11.3" />
+      <inertia ixx="0.26" ixy="0.0" ixz="0.0" iyy="0.26" iyz="0.0" izz="0.51" />
     </inertial>
   </link>
 
@@ -146,8 +146,8 @@
       <material name="Black" />
     </visual>
     <inertial>
-      <mass value="2" />
-      <inertia ixx="0.145833" ixy="0.0" ixz="0.0" iyy="0.145833" iyz="0.0" izz="0.125" />
+      <mass value="11.3" />
+      <inertia ixx="0.26" ixy="0.0" ixz="0.0" iyy="0.26" iyz="0.0" izz="0.51" />
     </inertial>
   </link>
 
@@ -173,8 +173,8 @@
       <material name="Black" />
     </visual>
     <inertial>
-      <mass value="2" />
-      <inertia ixx="0.145833" ixy="0.0" ixz="0.0" iyy="0.145833" iyz="0.0" izz="0.125" />
+      <mass value="11.3" />
+      <inertia ixx="0.26" ixy="0.0" ixz="0.0" iyy="0.26" iyz="0.0" izz="0.51" />
     </inertial>
   </link>
 

From b75f25701ac0d20999e8c32220a3991d185b3bb3 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Sun, 29 Dec 2024 18:15:56 +0000
Subject: [PATCH 2/3] Configure steering joints properly

---
 .../urdf/test_ackermann_drive.xacro.urdf               | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

diff --git a/gz_ros2_control_demos/urdf/test_ackermann_drive.xacro.urdf b/gz_ros2_control_demos/urdf/test_ackermann_drive.xacro.urdf
index 3bcaea91..bcdf8205 100644
--- a/gz_ros2_control_demos/urdf/test_ackermann_drive.xacro.urdf
+++ b/gz_ros2_control_demos/urdf/test_ackermann_drive.xacro.urdf
@@ -108,12 +108,13 @@
     </inertial>
   </link>
 
-  <joint name="left_wheel_steering_joint" type="continuous">
-    <origin xyz="0.9 0.5 -0.2" rpy="-1.57 0 0" />
+  <joint name="left_wheel_steering_joint" type="revolute">
+    <origin xyz="0.9 -0.5 -0.2" rpy="1.57 0 0" />
     <parent link="chassis" />
     <child link="left_wheel_steering" />
     <axis xyz="0 1 0" />
     <dynamics damping="0.2" />
+    <limit effort="1000.0" lower="-1.57" upper="1.57" velocity="30"/>
   </joint>
 
   <!-- right steer Link -->
@@ -124,12 +125,13 @@
     </inertial>
   </link>
 
-  <joint name="right_wheel_steering_joint" type="continuous">
-    <origin xyz="0.9 -0.5 -0.2" rpy="-1.57 0 0" />
+  <joint name="right_wheel_steering_joint" type="revolute">
+    <origin xyz="0.9 0.5 -0.2" rpy="1.57 0 0" />
     <parent link="chassis" />
     <child link="right_wheel_steering" />
     <axis xyz="0 1 0" />
     <dynamics damping="0.2" />
+    <limit effort="1000.0" lower="-1.57" upper="1.57" velocity="30"/>
   </joint>
 
   <!-- front left wheel Link -->

From 90a3fc87e478e5cd4d0b1083a526bc7f08089b61 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Sun, 29 Dec 2024 18:58:39 +0000
Subject: [PATCH 3/3] Use proper time

---
 .../examples/example_ackermann_drive.cpp               | 10 +++++++---
 1 file changed, 7 insertions(+), 3 deletions(-)

diff --git a/gz_ros2_control_demos/examples/example_ackermann_drive.cpp b/gz_ros2_control_demos/examples/example_ackermann_drive.cpp
index 35469e25..7be1633e 100644
--- a/gz_ros2_control_demos/examples/example_ackermann_drive.cpp
+++ b/gz_ros2_control_demos/examples/example_ackermann_drive.cpp
@@ -28,6 +28,8 @@ int main(int argc, char * argv[])
   std::shared_ptr<rclcpp::Node> node =
     std::make_shared<rclcpp::Node>("ackermann_drive_test_node");
 
+  node->set_parameter(rclcpp::Parameter("use_sim_time", true));
+
   auto publisher = node->create_publisher<geometry_msgs::msg::TwistStamped>(
     "/ackermann_steering_controller/reference", 10);
 
@@ -47,10 +49,12 @@ int main(int argc, char * argv[])
   command.twist = tw;
 
   while (1) {
-    command.header.stamp = node->now();
-    publisher->publish(command);
-    std::this_thread::sleep_for(50ms);
     rclcpp::spin_some(node);
+    if (node->get_clock()->started()) {
+      command.header.stamp = node->now();
+      publisher->publish(command);
+    }
+    std::this_thread::sleep_for(50ms);
   }
   rclcpp::shutdown();