From 752a4a1ccabc520c90adc2e040e99bce1a7825b1 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?=
 <christophfroehlich@users.noreply.github.com>
Date: Mon, 6 Jan 2025 14:33:10 +0100
Subject: [PATCH] Fix ackermann demo (#470)

(cherry picked from commit 6df12df48083f6c6cdafda0c84af5de525a0218d)
---
 .../examples/example_ackermann_drive.cpp      | 10 +++++--
 .../urdf/test_ackermann_drive.xacro.urdf      | 30 ++++++++++---------
 2 files changed, 23 insertions(+), 17 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();
 
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..bcdf8205 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>
 
@@ -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 -->
@@ -146,8 +148,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 +175,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>