feat: add vehicle info util

vehicle/vehicle_info_util/CMakeLists.txt

@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 3.5)
+project(vehicle_info_util)
+
+### Compile options
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Werror)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+
+ament_auto_add_library(vehicle_info_util SHARED
+  src/vehicle_info_util.cpp
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    config
+    launch
+)

vehicle/vehicle_info_util/Readme.md

@@ -0,0 +1,13 @@
+# Vehicle Info Util
+
+## Purpose
+
+This package is to get vehicle info parameters.
+
+### Description
+
+![description](./media/vehicle_info.drawio.svg)
+
+## Assumptions / Known limits
+
+TBD.

vehicle/vehicle_info_util/config/vehicle_info.param.yaml

@@ -0,0 +1,11 @@
+/**:
+  ros__parameters:
+    wheel_radius: 0.39
+    wheel_width: 0.42
+    wheel_base: 2.74 # between front wheel center and rear wheel center
+    wheel_tread: 1.63 # between left wheel center and right wheel center
+    front_overhang: 1.0 # between front wheel center and vehicle front
+    rear_overhang: 1.03 # between rear wheel center and vehicle rear
+    left_overhang: 0.1 # between left wheel center and vehicle left
+    right_overhang: 0.1 # between right wheel center and vehicle right
+    vehicle_height: 2.5

vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info.hpp

@@ -0,0 +1,88 @@
+// Copyright 2015-2021 Autoware Foundation
+//
+// 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.
+
+#ifndef VEHICLE_INFO_UTIL__VEHICLE_INFO_HPP_
+#define VEHICLE_INFO_UTIL__VEHICLE_INFO_HPP_
+
+namespace vehicle_info_util
+{
+/// Data class for vehicle info
+struct VehicleInfo
+{
+  // Base parameters. These describe the vehicle's bounding box and the
+  // position and radius of the wheels.
+  double wheel_radius_m;
+  double wheel_width_m;
+  double wheel_base_m;
+  double wheel_tread_m;
+  double front_overhang_m;
+  double rear_overhang_m;
+  double left_overhang_m;
+  double right_overhang_m;
+  double vehicle_height_m;
+
+  // Derived parameters, i.e. calculated from base parameters
+  // The offset values are relative to the base frame origin, which is located
+  // on the ground below the middle of the rear axle, and can be negative.
+  double vehicle_length_m;
+  double vehicle_width_m;
+  double min_longitudinal_offset_m;
+  double max_longitudinal_offset_m;
+  double min_lateral_offset_m;
+  double max_lateral_offset_m;
+  double min_height_offset_m;
+  double max_height_offset_m;
+};
+
+/// Create vehicle info from base parameters
+inline VehicleInfo createVehicleInfo(
+  const double wheel_radius_m, const double wheel_width_m, const double wheel_base_m,
+  const double wheel_tread_m, const double front_overhang_m, const double rear_overhang_m,
+  const double left_overhang_m, const double right_overhang_m, const double vehicle_height_m)
+{
+  // Calculate derived parameters
+  const double vehicle_length_m_ = front_overhang_m + wheel_base_m + rear_overhang_m;
+  const double vehicle_width_m_ = wheel_tread_m + left_overhang_m + right_overhang_m;
+  const double min_longitudinal_offset_m_ = -rear_overhang_m;
+  const double max_longitudinal_offset_m_ = front_overhang_m + wheel_base_m;
+  const double min_lateral_offset_m_ = -(wheel_tread_m / 2.0 + right_overhang_m);
+  const double max_lateral_offset_m_ = wheel_tread_m / 2.0 + left_overhang_m;
+  const double min_height_offset_m_ = 0.0;
+  const double max_height_offset_m_ = vehicle_height_m;
+
+  return VehicleInfo{
+    // Base parameters
+    wheel_radius_m,
+    wheel_width_m,
+    wheel_base_m,
+    wheel_tread_m,
+    front_overhang_m,
+    rear_overhang_m,
+    left_overhang_m,
+    right_overhang_m,
+    vehicle_height_m,
+    // Derived parameters
+    vehicle_length_m_,
+    vehicle_width_m_,
+    min_longitudinal_offset_m_,
+    max_longitudinal_offset_m_,
+    min_lateral_offset_m_,
+    max_lateral_offset_m_,
+    min_height_offset_m_,
+    max_height_offset_m_,
+  };
+}
+}  // namespace vehicle_info_util
+
+#endif  // VEHICLE_INFO_UTIL__VEHICLE_INFO_HPP_

vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info_util.hpp

@@ -0,0 +1,43 @@
+// Copyright 2015-2021 Autoware Foundation
+//
+// 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.
+
+#ifndef VEHICLE_INFO_UTIL__VEHICLE_INFO_UTIL_HPP_
+#define VEHICLE_INFO_UTIL__VEHICLE_INFO_UTIL_HPP_
+
+#include "vehicle_info_util/vehicle_info.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+namespace vehicle_info_util
+{
+/// This is a convenience class for saving you from declaring all parameters
+/// manually and calculating derived parameters.
+/// This class supposes that necessary parameters are set when the node is launched.
+class VehicleInfoUtil
+{
+public:
+  /// Constructor
+  explicit VehicleInfoUtil(rclcpp::Node & node);
+
+  /// Get vehicle info
+  VehicleInfo getVehicleInfo();
+
+private:
+  /// Buffer for base parameters
+  VehicleInfo vehicle_info_;
+};
+
+}  // namespace vehicle_info_util
+
+#endif  // VEHICLE_INFO_UTIL__VEHICLE_INFO_UTIL_HPP_

vehicle/vehicle_info_util/launch/sample.launch.py

@@ -0,0 +1,57 @@
+# Copyright 2021 Tier IV, Inc. All rights reserved.
+#
+# 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 os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.actions import IncludeLaunchDescription
+from launch.actions import OpaqueFunction
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import SetParameter
+from launch_ros.substitutions import FindPackageShare
+
+
+def launch_setup(context, *args, **kwargs):
+    # use_sim_time
+    set_use_sim_time = SetParameter(name="use_sim_time", value=LaunchConfiguration("use_sim_time"))
+
+    vehicle_info_param_path = os.path.join(
+        get_package_share_directory("vehicle_info_util"),
+        "config",
+        "vehicle_info.param.yaml",
+    )
+    # vehicle_info
+    load_vehicle_info = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [FindPackageShare("vehicle_info_util"), "/launch/vehicle_info.launch.py"]
+        ),
+        launch_arguments={"vehicle_info_param_file": [vehicle_info_param_path]}.items(),
+    )
+
+    return [
+        set_use_sim_time,
+        load_vehicle_info,
+    ]
+
+
+def generate_launch_description():
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument("use_sim_time", default_value="false"),
+            OpaqueFunction(function=launch_setup),
+        ]
+    )

vehicle/vehicle_info_util/launch/vehicle_info.launch.py

@@ -0,0 +1,36 @@
+# Copyright 2021 Tier IV, Inc. All rights reserved.
+#
+# 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.
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.actions import OpaqueFunction
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import SetParameter
+import yaml
+
+
+def launch_setup(context, *args, **kwargs):
+    vehicle_param_file = LaunchConfiguration("vehicle_info_param_file").perform(context)
+    with open(vehicle_param_file, "r") as f:
+        vehicle_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+    return [SetParameter(name=k, value=v) for (k, v) in vehicle_param.items()]
+
+
+def generate_launch_description():
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument("vehicle_info_param_file"),
+            OpaqueFunction(function=launch_setup),
+        ]
+    )