diff --git a/vehicle/vehicle_info_util/CMakeLists.txt b/vehicle/vehicle_info_util/CMakeLists.txt new file mode 100644 index 0000000000000..4b7f3b80160e4 --- /dev/null +++ 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 new file mode 100644 index 0000000000000..8d5747ea60d60 --- /dev/null +++ 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 new file mode 100644 index 0000000000000..b1279b50ef04a --- /dev/null +++ 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 new file mode 100644 index 0000000000000..e7ba446051889 --- /dev/null +++ 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 new file mode 100644 index 0000000000000..5e1623ada5ed3 --- /dev/null +++ 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 + +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 new file mode 100644 index 0000000000000..c0be2bc4c4c48 --- /dev/null +++ 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 new file mode 100644 index 0000000000000..ea8760f959bbb --- /dev/null +++ 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), + ] + ) diff --git a/vehicle/vehicle_info_util/media/vehicle_info.drawio.svg b/vehicle/vehicle_info_util/media/vehicle_info.drawio.svg new file mode 100644 index 0000000000000..d53ca71ea90f5 --- /dev/null +++ b/vehicle/vehicle_info_util/media/vehicle_info.drawio.svg @@ -0,0 +1,215 @@ + + + + + + + + + + + + +
+
+
+ + height + +
+
+
+ + + height + + + + + + + + + +
+
+
+ + front overhang + +
+
+
+ + + front over... + + + + + + + + + +
+
+
+ + wheel base + +
+
+
+ + + wheel base + + + + + + + + + +
+
+
+ + rear overhang + +
+
+
+ + + rear overh... + + + + + + + + + +
+
+
+ + longitudinal offset + +
+
+
+ + + longitudin... + + + + + + + + + +
+
+
+ + vehicle length + +
+
+
+ + + vehicle le... + + + + + + + + + + + + +
+
+
+ wheel radius +
+
+
+ + + wheel radi... + + + + + + + + + + + + + + +
+
+
+ + wheel tread + +
+
+
+ + + wheel tread + + + + + + + + + +
+
+
+ + left overhang + +
+
+
+ + + left overh... + + + + + + + + + + + + + Viewer does not support full SVG 1.1 + + + + \ No newline at end of file diff --git a/vehicle/vehicle_info_util/package.xml b/vehicle/vehicle_info_util/package.xml new file mode 100644 index 0000000000000..0619cf1f08d44 --- /dev/null +++ b/vehicle/vehicle_info_util/package.xml @@ -0,0 +1,21 @@ + + + + vehicle_info_util + 0.1.0 + The vehicle_info_util package + + Yamato ANDO + Apache License 2.0 + + ament_cmake_auto + + rclcpp + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/vehicle/vehicle_info_util/src/vehicle_info_util.cpp b/vehicle/vehicle_info_util/src/vehicle_info_util.cpp new file mode 100644 index 0000000000000..6cc2d2f27e9ce --- /dev/null +++ b/vehicle/vehicle_info_util/src/vehicle_info_util.cpp @@ -0,0 +1,62 @@ +// 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. + +#include "vehicle_info_util/vehicle_info_util.hpp" + +#include + +namespace +{ +template +T getParameter(rclcpp::Node & node, const std::string & name) +{ + if (node.has_parameter(name)) { + return node.get_parameter(name).get_value(); + } + + try { + return node.declare_parameter(name); + } catch (const rclcpp::ParameterTypeException & ex) { + RCLCPP_ERROR( + node.get_logger(), "Failed to get parameter `%s`, please set it when you launch the node.", + name.c_str()); + throw(ex); + } +} +} // namespace + +namespace vehicle_info_util +{ +VehicleInfoUtil::VehicleInfoUtil(rclcpp::Node & node) +{ + vehicle_info_.wheel_radius_m = getParameter(node, "wheel_radius"); + vehicle_info_.wheel_width_m = getParameter(node, "wheel_width"); + vehicle_info_.wheel_base_m = getParameter(node, "wheel_base"); + vehicle_info_.wheel_tread_m = getParameter(node, "wheel_tread"); + vehicle_info_.front_overhang_m = getParameter(node, "front_overhang"); + vehicle_info_.rear_overhang_m = getParameter(node, "rear_overhang"); + vehicle_info_.left_overhang_m = getParameter(node, "left_overhang"); + vehicle_info_.right_overhang_m = getParameter(node, "right_overhang"); + vehicle_info_.vehicle_height_m = getParameter(node, "vehicle_height"); +} + +VehicleInfo VehicleInfoUtil::getVehicleInfo() +{ + return createVehicleInfo( + vehicle_info_.wheel_radius_m, vehicle_info_.wheel_width_m, vehicle_info_.wheel_base_m, + vehicle_info_.wheel_tread_m, vehicle_info_.front_overhang_m, vehicle_info_.rear_overhang_m, + vehicle_info_.left_overhang_m, vehicle_info_.right_overhang_m, vehicle_info_.vehicle_height_m); +} + +} // namespace vehicle_info_util