From 8ca946c21bdd4c4740399246275cf43755662225 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 7 May 2024 13:27:44 +0000 Subject: [PATCH 1/4] Add checks for non-zero values to diff_drive --- .../src/diff_drive_controller_parameter.yaml | 6 ++++++ diff_drive_controller/test/test_diff_drive_controller.cpp | 4 ++++ 2 files changed, 10 insertions(+) diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index 9720e068e1..755259cc2a 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -19,11 +19,17 @@ 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] + } } wheel_radius: { 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 9ab3022a9f..45bdcd218e 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(0.4))); + parameter_overrides.push_back(rclcpp::Parameter("wheel_radius", rclcpp::ParameterValue(0.02))); parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); node_options.parameter_overrides(parameter_overrides); From 665340dbb07cae30872ed4c03243319efa1862f6 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 7 May 2024 13:29:22 +0000 Subject: [PATCH 2/4] Add check also for steering controllers --- .../src/ackermann_steering_controller.yaml | 15 +++++++++++++++ .../src/bicycle_steering_controller.yaml | 9 +++++++++ .../src/tricycle_steering_controller.yaml | 12 ++++++++++++ 3 files changed, 36 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/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] + } } From 9b990627f07f88fad08ee386afbd0bd39bca001d Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 7 May 2024 13:32:15 +0000 Subject: [PATCH 3/4] Add check also for tricycle_controller --- tricycle_controller/src/tricycle_controller_parameter.yaml | 6 ++++++ tricycle_controller/test/test_tricycle_controller.cpp | 3 +++ 2 files changed, 9 insertions(+) diff --git a/tricycle_controller/src/tricycle_controller_parameter.yaml b/tricycle_controller/src/tricycle_controller_parameter.yaml index 68fc2202c2..34662242fd 100644 --- a/tricycle_controller/src/tricycle_controller_parameter.yaml +++ b/tricycle_controller/src/tricycle_controller_parameter.yaml @@ -19,11 +19,17 @@ tricycle_controller: type: double, default_value: 0.0, description: "Shortest distance between the front wheel and the rear axle. If this parameter is wrong, the robot will not behave correctly in curves.", + validation: { + gt<>: [0.0] + } } wheel_radius: { 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] + } } odom_frame_id: { type: string, diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index 5280aaf244..4be1680980 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -164,6 +164,9 @@ class TestTricycleController : public ::testing::Test rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name_init))); parameter_overrides.push_back( rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name_init))); + // default parameters + parameter_overrides.push_back(rclcpp::Parameter("wheelbase", rclcpp::ParameterValue(1.))); + 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); From 70ceae39399b6f1d42db9c97f0c22767c3f25368 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 7 May 2024 13:32:29 +0000 Subject: [PATCH 4/4] Set hardcoded value different from yaml file --- diff_drive_controller/test/test_diff_drive_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 45bdcd218e..f3f99ac8d8 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -186,8 +186,8 @@ class TestDiffDriveController : public ::testing::Test rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_joints_init))); // default parameters parameter_overrides.push_back( - rclcpp::Parameter("wheel_separation", rclcpp::ParameterValue(0.4))); - parameter_overrides.push_back(rclcpp::Parameter("wheel_radius", rclcpp::ParameterValue(0.02))); + 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);