From 04e961f91f461826b66daf5ffa36d415a891f66f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?=
 <christophfroehlich@users.noreply.github.com>
Date: Thu, 9 May 2024 11:07:21 +0200
Subject: [PATCH] Add parameter check for geometric values (backport #1120)

---
 .../src/ackermann_steering_controller.yaml        | 15 +++++++++++++++
 .../src/bicycle_steering_controller.yaml          |  9 +++++++++
 .../src/diff_drive_controller_parameter.yaml      |  6 ++++++
 .../test/test_diff_drive_controller.cpp           |  4 ++++
 .../src/tricycle_steering_controller.yaml         | 12 ++++++++++++
 5 files changed, 46 insertions(+)

diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.yaml b/ackermann_steering_controller/src/ackermann_steering_controller.yaml
index 3726146919..1ec0b41c9f 100644
--- a/ackermann_steering_controller/src/ackermann_steering_controller.yaml
+++ b/ackermann_steering_controller/src/ackermann_steering_controller.yaml
@@ -5,6 +5,9 @@ ackermann_steering_controller:
       default_value: 0.0,
       description: "Front wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase",
       read_only: false,
+      validation: {
+        gt<>: [0.0]
+      }
     }
 
   rear_wheel_track:
@@ -13,6 +16,9 @@ ackermann_steering_controller:
       default_value: 0.0,
       description: "Rear wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase",
       read_only: false,
+      validation: {
+        gt<>: [0.0]
+      }
     }
 
   wheelbase:
@@ -21,6 +27,9 @@ ackermann_steering_controller:
       default_value: 0.0,
       description: "Distance between front and rear wheels. For details see: https://en.wikipedia.org/wiki/Wheelbase",
       read_only: false,
+      validation: {
+        gt<>: [0.0]
+      }
     }
 
   front_wheels_radius:
@@ -29,6 +38,9 @@ ackermann_steering_controller:
       default_value: 0.0,
       description: "Front wheels radius.",
       read_only: false,
+      validation: {
+        gt<>: [0.0]
+      }
     }
 
   rear_wheels_radius:
@@ -37,4 +49,7 @@ ackermann_steering_controller:
       default_value: 0.0,
       description: "Rear wheels radius.",
       read_only: false,
+      validation: {
+        gt<>: [0.0]
+      }
     }
diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.yaml b/bicycle_steering_controller/src/bicycle_steering_controller.yaml
index c40e27ef96..fde323ef74 100644
--- a/bicycle_steering_controller/src/bicycle_steering_controller.yaml
+++ b/bicycle_steering_controller/src/bicycle_steering_controller.yaml
@@ -5,6 +5,9 @@ bicycle_steering_controller:
       default_value: 0.0,
       description: "Distance between front and rear wheel. For details see: https://en.wikipedia.org/wiki/Wheelbase",
       read_only: false,
+      validation: {
+        gt<>: [0.0]
+      }
     }
 
   front_wheel_radius:
@@ -13,6 +16,9 @@ bicycle_steering_controller:
       default_value: 0.0,
       description: "Front wheel radius.",
       read_only: false,
+      validation: {
+        gt<>: [0.0]
+      }
     }
 
   rear_wheel_radius:
@@ -21,4 +27,7 @@ bicycle_steering_controller:
       default_value: 0.0,
       description: "Rear wheel radius.",
       read_only: false,
+      validation: {
+        gt<>: [0.0]
+      }
     }
diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml
index c9cfc0542f..3f370bcc65 100644
--- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml
+++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml
@@ -19,6 +19,9 @@ diff_drive_controller:
     type: double,
     default_value: 0.0,
     description: "Shortest distance between the left and right wheels. If this parameter is wrong, the robot will not behave correctly in curves.",
+    validation: {
+      gt<>: [0.0]
+    }
   }
   wheels_per_side: {
     type: int,
@@ -29,6 +32,9 @@ diff_drive_controller:
     type: double,
     default_value: 0.0,
     description: "Radius of a wheel, i.e., wheels size, used for transformation of linear velocity into wheel rotations. If this parameter is wrong the robot will move faster or slower then expected.",
+    validation: {
+      gt<>: [0.0]
+    }
   }
   wheel_separation_multiplier: {
     type: double,
diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp
index 61ab57c0a4..8c09e5042e 100644
--- a/diff_drive_controller/test/test_diff_drive_controller.cpp
+++ b/diff_drive_controller/test/test_diff_drive_controller.cpp
@@ -184,6 +184,10 @@ class TestDiffDriveController : public ::testing::Test
       rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_joints_init)));
     parameter_overrides.push_back(
       rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_joints_init)));
+    // default parameters
+    parameter_overrides.push_back(
+      rclcpp::Parameter("wheel_separation", rclcpp::ParameterValue(1.0)));
+    parameter_overrides.push_back(rclcpp::Parameter("wheel_radius", rclcpp::ParameterValue(0.1)));
 
     parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end());
     node_options.parameter_overrides(parameter_overrides);
diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.yaml b/tricycle_steering_controller/src/tricycle_steering_controller.yaml
index 1015865fd9..6e5ae2b477 100644
--- a/tricycle_steering_controller/src/tricycle_steering_controller.yaml
+++ b/tricycle_steering_controller/src/tricycle_steering_controller.yaml
@@ -5,6 +5,9 @@ tricycle_steering_controller:
       default_value: 0.0,
       description: "Wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase",
       read_only: false,
+      validation: {
+        gt<>: [0.0]
+      }
     }
 
   wheelbase:
@@ -13,6 +16,9 @@ tricycle_steering_controller:
       default_value: 0.0,
       description: "Distance between front and rear wheels. For details see: https://en.wikipedia.org/wiki/Wheelbase",
       read_only: false,
+      validation: {
+        gt<>: [0.0]
+      }
     }
 
   front_wheels_radius:
@@ -21,6 +27,9 @@ tricycle_steering_controller:
       default_value: 0.0,
       description: "Front wheels radius.",
       read_only: false,
+      validation: {
+        gt<>: [0.0]
+      }
     }
 
   rear_wheels_radius:
@@ -29,4 +38,7 @@ tricycle_steering_controller:
       default_value: 0.0,
       description: "Rear wheels radius.",
       read_only: false,
+      validation: {
+        gt<>: [0.0]
+      }
     }