Add parameter check for geometric values (#1120)

(cherry picked from commit 7170328)

# Conflicts:
#	diff_drive_controller/test/test_diff_drive_controller.cpp
#	tricycle_controller/src/tricycle_controller_parameter.yaml
#	tricycle_controller/test/test_tricycle_controller.cpp

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]
+      }
     }

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_drive_controller/src/diff_drive_controller_parameter.yaml

@@ -13,6 +13,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,
@@ -23,6 +26,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_drive_controller/test/test_diff_drive_controller.cpp

@@ -166,6 +166,32 @@ class TestDiffDriveController : public ::testing::Test
     controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs));
   }
 
+<<<<<<< HEAD
+=======
+  controller_interface::return_type InitController(
+    const std::vector<std::string> left_wheel_joints_init = left_wheel_names,
+    const std::vector<std::string> right_wheel_joints_init = right_wheel_names,
+    const std::vector<rclcpp::Parameter> & parameters = {}, const std::string ns = "")
+  {
+    auto node_options = rclcpp::NodeOptions();
+    std::vector<rclcpp::Parameter> parameter_overrides;
+
+    parameter_overrides.push_back(
+      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);
+
+    return controller_->init(controller_name, urdf_, 0, ns, node_options);
+  }
+
+>>>>>>> 7170328 (Add parameter check for geometric values (#1120))
   const std::string controller_name = "test_diff_drive_controller";
   std::unique_ptr<TestableDiffDriveController> controller_;
 

tricycle_controller/src/tricycle_controller_parameter.yaml

@@ -0,0 +1,155 @@
+tricycle_controller:
+  traction_joint_name: {
+    type: string,
+    default_value: "",
+    description: "Name of the traction joint",
+    validation: {
+      not_empty<>: []
+    }
+  }
+  steering_joint_name: {
+    type: string,
+    default_value: "",
+    description: "Name of the steering joint",
+    validation: {
+      not_empty<>: []
+    }
+  }
+  wheelbase: {
+    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,
+    default_value: "odom",
+    description:  "Name of the frame for odometry. This frame is parent of ``base_frame_id`` when controller publishes odometry.",
+  }
+  base_frame_id: {
+    type: string,
+    default_value: "base_link",
+    description: "Name of the robot's base frame that is child of the odometry frame.",
+  }
+  pose_covariance_diagonal: {
+    type: double_array,
+    default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+    description: "Odometry covariance for the encoder output of the robot for the pose. These values should be tuned to your robot's sample odometry data, but these values are a good place to start: ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``.",
+  }
+  twist_covariance_diagonal: {
+    type: double_array,
+    default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+    description: "Odometry covariance for the encoder output of the robot for the speed. These values should be tuned to your robot's sample odometry data, but these values are a good place to start: ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``.",
+  }
+  open_loop: {
+    type: bool,
+    default_value: false,
+    description: "If set to true the odometry of the robot will be calculated from the commanded values and not from feedback.",
+  }
+  enable_odom_tf: {
+    type: bool,
+    default_value: false,
+    description: "Publish transformation between ``odom_frame_id`` and ``base_frame_id``.",
+  }
+  odom_only_twist: {
+    type: bool,
+    default_value: false,
+    description: "for doing the pose integration in separate node.",
+  }
+  cmd_vel_timeout: {
+    type: int,
+    default_value: 500, # milliseconds
+    description: "Timeout in milliseconds, after which input command on ``cmd_vel`` topic is considered staled.",
+  }
+  publish_ackermann_command: {
+    type: bool,
+    default_value: false,
+    description: "Publish limited commands.",
+  }
+  velocity_rolling_window_size: {
+    type: int,
+    default_value: 10,
+    description: "Size of the rolling window for calculation of mean velocity use in odometry.",
+    validation: {
+      gt<>: [0]
+    }
+  }
+  use_stamped_vel: {
+    type: bool,
+    default_value: true,
+    description: "(deprecated) Use stamp from input velocity message to calculate how old the command actually is.",
+  }
+  traction:
+    # "The positive limit will be applied to both directions. Setting different limits for positive "
+    # "and negative directions is not supported. Actuators are "
+    # "assumed to have the same constraints in both directions"
+    max_velocity: {
+      type: double,
+      default_value: .NAN,
+    }
+    min_velocity: {
+      type: double,
+      default_value: .NAN,
+    }
+    max_acceleration: {
+      type: double,
+      default_value: .NAN,
+    }
+    min_acceleration: {
+      type: double,
+      default_value: .NAN,
+    }
+    max_deceleration: {
+      type: double,
+      default_value: .NAN,
+    }
+    min_deceleration: {
+      type: double,
+      default_value: .NAN,
+    }
+    max_jerk: {
+      type: double,
+      default_value: .NAN,
+    }
+    min_jerk: {
+      type: double,
+      default_value: .NAN,
+    }
+  steering:
+    # "The positive limit will be applied to both directions. Setting different limits for positive "
+    # "and negative directions is not supported. Actuators are "
+    # "assumed to have the same constraints in both directions"
+    max_position: {
+      type: double,
+      default_value: .NAN,
+    }
+    min_position: {
+      type: double,
+      default_value: .NAN,
+    }
+    max_velocity: {
+      type: double,
+      default_value: .NAN,
+    }
+    min_velocity: {
+      type: double,
+      default_value: .NAN,
+    }
+    max_acceleration: {
+      type: double,
+      default_value: .NAN,
+    }
+    min_acceleration: {
+      type: double,
+      default_value: .NAN,
+    }

tricycle_controller/test/test_tricycle_controller.cpp

@@ -146,6 +146,31 @@ class TestTricycleController : public ::testing::Test
     controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs));
   }
 
+<<<<<<< HEAD
+=======
+  controller_interface::return_type InitController(
+    const std::string traction_joint_name_init = traction_joint_name,
+    const std::string steering_joint_name_init = steering_joint_name,
+    const std::vector<rclcpp::Parameter> & parameters = {})
+  {
+    auto node_options = rclcpp::NodeOptions();
+    std::vector<rclcpp::Parameter> parameter_overrides;
+
+    parameter_overrides.push_back(
+      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);
+
+    return controller_->init(controller_name, urdf_, 0, "", node_options);
+  }
+
+>>>>>>> 7170328 (Add parameter check for geometric values (#1120))
   const std::string controller_name = "test_tricycle_controller";
   std::unique_ptr<TestableTricycleController> controller_;
 

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]
+      }
     }