feat: add vehicle info util (autowarefoundation#22)

* vehicle_info_util package for ROS2 port (autowarefoundation#58) * vehicle_info package for ROS2 port * Whitespace * Add explanatory comment * Rename factory function * Fix static method * Move to directory that corresponds to package name * [vehicle_info_util] Build as shared library (autowarefoundation#112) * [ROS2] Fix behavior velocity planner (autowarefoundation#110) * install launch and config folders Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * port launch file Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * fix config files Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * improve vehicle_info_utils to enable multiple declaration of VehicleInfo objects Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * fix sementation fault Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * Adjust copyright notice on 532 out of 699 source files (autowarefoundation#143) * Use quotes for includes where appropriate (autowarefoundation#144) * Use quotes for includes where appropriate * Fix lint tests * Make tests pass hopefully * Run uncrustify on the entire Pilot.Auto codebase (autowarefoundation#151) * Run uncrustify on the entire Pilot.Auto codebase * Exclude open PRs * ROS2 Linting: vehicle_info_util (autowarefoundation#163) * Sync public repo (autowarefoundation#1228) * [simple_planning_simulator] add readme (autowarefoundation#424) * add readme of simple_planning_simulator Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * Update simulator/simple_planning_simulator/README.md * set transit_margin_time to intersect. planner (autowarefoundation#460) * Fix pose2twist (autowarefoundation#462) Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Ros2 vehicle info param server (autowarefoundation#447) * add vehicle_info_param_server * update vehicle info * apply format * fix bug * skip unnecessary search * delete vehicle param file * fix bug * Ros2 fix topic name part2 (autowarefoundation#425) * Fix topic name of traffic_light_classifier Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix topic name of traffic_light_visualization Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix topic name of traffic_light_ssd_fine_detector Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix topic name of traffic_light_map_based_detector Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix lint traffic_light_recognition Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix lint traffic_light_ssd_fine_detector Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix lint traffic_light_classifier Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix lint traffic_light_classifier Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix lint traffic_light_ssd_fine_detector Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix issues in hdd_reader (autowarefoundation#466) * Fix some issues detected by Coverity Scan and Clang-Tidy * Update launch command * Add more `close(new_sock)` * Simplify the definitions of struct * fix: re-construct laneletMapLayer for reindex RTree (autowarefoundation#463) * Rviz overlay render fix (autowarefoundation#461) * Moved painiting in SteeringAngle plugin to update() Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai> * super class now back to MFD Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai> * uncrustified Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai> * acquire data in mutex Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai> * back to RTD as superclass Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai> * Rviz overlay render in update (autowarefoundation#465) * Moved painiting in SteeringAngle plugin to update() Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai> * super class now back to MFD Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai> * uncrustified Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai> * acquire data in mutex Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai> * removed unnecessary includes and some dead code Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai> * Adepted remaining vehicle plugin classes to render-in-update concept. Returned to MFD superclass Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai> * restored RTD superclass Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai> Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> Co-authored-by: Makoto Tokunaga <vios-fish@users.noreply.github.com> Co-authored-by: Adam Dąbrowski <adam.dabrowski@robotec.ai> * Remove vehicle info param server (autowarefoundation#1304) * Remove vehicle info param server Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Fix ament_uncrustify Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix ament_uncrustify Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp> * Refactor vehicle info util (autowarefoundation#1305) * Update license Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Refactor vehicle_info_util Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Rename and split files Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix interfaces Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix bug and add error handling Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Add "// namespace" Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Add missing include Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix/fix utils (autowarefoundation#1310) * Add missing namespace to autoware_utils Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Add createVehicleInfo Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Add rethrow Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Format package.xml Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix usage of autoware_utils Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Add missing namespace comment Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Move autoware_global_parameter_loader to the appropriate place (autowarefoundation#1701) * Move autoware_global_parameter_loader to the appropriate place Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix package name Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * suppress warnings for declare parameters (autowarefoundation#1724) * fix for lanelet2_extension * fix for traffic light ssd fine detector * fix for topic_state_monitor * fix for dummy diag publisher * fix for remote cmd converter * fix for vehicle_info_util * fix for multi object tracker * fix for freespace planner * fix for autoware_error_monitor * add Werror for multi object tracker * fix for multi object tracker * add Werror for liraffic light ssd fine detector * add Werror for topic state monitor * add Werror * add Werror * add Werror * add Werror * fix style * Fix -Wunused-parameter (autowarefoundation#1836) * Fix -Wunused-parameter Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix mistake Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * fix spell * Fix lint issues Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Ignore flake8 warnings Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp> * Rename vehicle_info param arg to avoid conflicts (autowarefoundation#2074) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Change formatter to clang-format and black (autowarefoundation#2332) * Revert "Temporarily comment out pre-commit hooks" This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3. * Replace ament_lint_common with autoware_lint_common Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply Black Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Add COLCON_IGNORE (autowarefoundation#500) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * remove COLCON_IGNORE (autowarefoundation#505) Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp> * [vehicle_info_util] add readme (autowarefoundation#560) * add readme * fix typo * replace to valid link * add default config file * add sample launch * ci(pre-commit): autofix * remove unused Co-authored-by: Nikolai Morin <nnmmgit@gmail.com> Co-authored-by: Frederik Beaujean <72439809+fred-apex-ai@users.noreply.github.com> Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Co-authored-by: Jilada Eccleston <jilada.eccleston@gmail.com> Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> Co-authored-by: Makoto Tokunaga <vios-fish@users.noreply.github.com> Co-authored-by: Adam Dąbrowski <adam.dabrowski@robotec.ai> Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp> Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com> Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
badai-nguyen · Dec 2, 2021 · 5a02f13 · 5a02f13
1 parent ede45b0
commit 5a02f13
Show file tree

Hide file tree

Showing 10 changed files with 577 additions and 0 deletions.
diff --git a/vehicle/vehicle_info_util/CMakeLists.txt b/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
+)
diff --git a/vehicle/vehicle_info_util/Readme.md b/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.
diff --git a/vehicle/vehicle_info_util/config/vehicle_info.param.yaml b/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
diff --git a/vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info.hpp b/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_
diff --git a/vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info_util.hpp b/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_
diff --git a/vehicle/vehicle_info_util/launch/sample.launch.py b/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),
+        ]
+    )
diff --git a/vehicle/vehicle_info_util/launch/vehicle_info.launch.py b/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),
+        ]
+    )