feat(vehicle_info_util): add script to calculate turning radius (auto…

…warefoundation#1855)

vehicle/vehicle_info_util/CMakeLists.txt

@@ -13,3 +13,8 @@ ament_auto_package(
     config
     launch
 )
+
+install(PROGRAMS
+  scripts/min_turning_radius_calculator.py
+  DESTINATION lib/${PROJECT_NAME}
+)

vehicle/vehicle_info_util/Readme.md

@@ -8,6 +8,22 @@ This package is to get vehicle info parameters.
 
 In [here](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/components/vehicle-dimensions/), you can check the vehicle dimensions with more detail.
 
+### Scripts
+
+#### Minimum turning radius
+
+```sh
+$ ros2 run vehicle_info_util min_turning_radius_calculator.py
+yaml path is /home/autoware/pilot-auto/install/vehicle_info_util/share/vehicle_info_util/config/vehicle_info.param.yaml
+Minimum turning radius is 3.253042620027102 [m] for rear, 4.253220695862465 [m] for front.
+```
+
+You can designate yaml file with `-y` option as follows.
+
+```sh
+ros2 run vehicle_info_util min_turning_radius_calculator.py -y <path-to-yaml>
+```
+
 ## Assumptions / Known limits
 
 TBD.

vehicle/vehicle_info_util/scripts/min_turning_radius_calculator.py

@@ -0,0 +1,55 @@
+#!/usr/bin/env python3
+
+# Copyright 2022 Tier IV, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import argparse
+import math
+
+from ament_index_python.packages import get_package_share_directory
+import yaml
+
+
+def main(yaml_path):
+    with open(yaml_path) as f:
+        config = yaml.safe_load(f)
+
+    wheel_base = config["/**"]["ros__parameters"]["wheel_base"]
+    max_steer_angle = config["/**"]["ros__parameters"]["max_steer_angle"]
+
+    rear_radius = wheel_base / math.tan(max_steer_angle)
+    front_radius = wheel_base / math.sin(max_steer_angle)
+
+    print("yaml path is {}".format(yaml_path))
+    print(
+        "Minimum turning radius is {} [m] for rear, {} [m] for front.".format(
+            rear_radius, front_radius
+        )
+    )
+
+
+if __name__ == "__main__":
+    default_yaml_path = (
+        get_package_share_directory("vehicle_info_util") + "/config/vehicle_info.param.yaml"
+    )
+
+    parser = argparse.ArgumentParser()
+    parser.add_argument(
+        "-y", "--yaml", default=default_yaml_path, help="vehicle_info.param.yaml path"
+    )
+
+    args = parser.parse_args()
+    yaml_path = args.yaml
+
+    main(yaml_path)