From 88e416a08fce1836689fe8690e1bbda3f49f6553 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Mon, 20 Feb 2023 16:37:31 +0900 Subject: [PATCH 1/3] feat(default_ad_api): add vehicle dimensions api Signed-off-by: Takagi, Isamu --- .../include/autoware_ad_api_specs/vehicle.hpp | 33 +++++++++++++ system/default_ad_api/CMakeLists.txt | 2 + .../launch/default_ad_api.launch.py | 1 + system/default_ad_api/package.xml | 1 + system/default_ad_api/src/vehicle_info.cpp | 48 +++++++++++++++++++ system/default_ad_api/src/vehicle_info.hpp | 39 +++++++++++++++ 6 files changed, 124 insertions(+) create mode 100644 common/autoware_ad_api_specs/include/autoware_ad_api_specs/vehicle.hpp create mode 100644 system/default_ad_api/src/vehicle_info.cpp create mode 100644 system/default_ad_api/src/vehicle_info.hpp diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/vehicle.hpp b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/vehicle.hpp new file mode 100644 index 0000000000000..3bd6a5f02dfbb --- /dev/null +++ b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/vehicle.hpp @@ -0,0 +1,33 @@ +// Copyright 2023 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. + +#ifndef AUTOWARE_AD_API_SPECS__VEHICLE_HPP_ +#define AUTOWARE_AD_API_SPECS__VEHICLE_HPP_ + +#include + +#include + +namespace autoware_ad_api::vehicle +{ + +struct Dimensions +{ + using Service = autoware_adapi_v1_msgs::srv::GetVehicleDimensions; + static constexpr char name[] = "/api/vehicle/info/dimensions"; +}; + +} // namespace autoware_ad_api::vehicle + +#endif // AUTOWARE_AD_API_SPECS__VEHICLE_HPP_ diff --git a/system/default_ad_api/CMakeLists.txt b/system/default_ad_api/CMakeLists.txt index 6221a370eab72..188b9b07f5ca8 100644 --- a/system/default_ad_api/CMakeLists.txt +++ b/system/default_ad_api/CMakeLists.txt @@ -12,6 +12,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/operation_mode.cpp src/planning.cpp src/routing.cpp + src/vehicle_info.cpp src/utils/route_conversion.cpp src/compatibility/autoware_state.cpp ) @@ -25,6 +26,7 @@ rclcpp_components_register_nodes(${PROJECT_NAME} "default_ad_api::OperationModeNode" "default_ad_api::PlanningNode" "default_ad_api::RoutingNode" + "default_ad_api::VehicleInfoNode" ) if(BUILD_TESTING) diff --git a/system/default_ad_api/launch/default_ad_api.launch.py b/system/default_ad_api/launch/default_ad_api.launch.py index 50211ede05e86..9401f5ace96d1 100644 --- a/system/default_ad_api/launch/default_ad_api.launch.py +++ b/system/default_ad_api/launch/default_ad_api.launch.py @@ -49,6 +49,7 @@ def generate_launch_description(): create_api_node("operation_mode", "OperationModeNode"), create_api_node("planning", "PlanningNode"), create_api_node("routing", "RoutingNode"), + create_api_node("vehicle_info", "VehicleInfoNode"), ] container = ComposableNodeContainer( namespace="default_ad_api", diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index b19caeb4ca005..3632135d04e38 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -28,6 +28,7 @@ std_srvs tier4_control_msgs tier4_system_msgs + vehicle_info_util python3-flask diff --git a/system/default_ad_api/src/vehicle_info.cpp b/system/default_ad_api/src/vehicle_info.cpp new file mode 100644 index 0000000000000..9383442374d82 --- /dev/null +++ b/system/default_ad_api/src/vehicle_info.cpp @@ -0,0 +1,48 @@ +// Copyright 2023 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. + +#include "vehicle_info.hpp" + +#include + +namespace default_ad_api +{ + +VehicleInfoNode::VehicleInfoNode(const rclcpp::NodeOptions & options) +: Node("vehicle_info", options) +{ + const auto on_vehicle_dimensions = [this](auto, auto res) { + res->status.success = true; + res->dimensions = dimensions_; + }; + + const auto vehicle = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + dimensions_.wheel_radius = vehicle.wheel_radius_m; + dimensions_.wheel_width = vehicle.wheel_width_m; + dimensions_.wheel_base = vehicle.wheel_base_m; + dimensions_.wheel_tread = vehicle.wheel_tread_m; + dimensions_.front_overhang = vehicle.front_overhang_m; + dimensions_.rear_overhang = vehicle.rear_overhang_m; + dimensions_.left_overhang = vehicle.left_overhang_m; + dimensions_.right_overhang = vehicle.right_overhang_m; + dimensions_.height = vehicle.vehicle_height_m; + + const auto adaptor = component_interface_utils::NodeAdaptor(this); + adaptor.init_srv(srv_dimensions_, on_vehicle_dimensions); +} + +} // namespace default_ad_api + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::VehicleInfoNode) diff --git a/system/default_ad_api/src/vehicle_info.hpp b/system/default_ad_api/src/vehicle_info.hpp new file mode 100644 index 0000000000000..c7171ddf52485 --- /dev/null +++ b/system/default_ad_api/src/vehicle_info.hpp @@ -0,0 +1,39 @@ +// Copyright 2023 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. + +#ifndef VEHICLE_INFO_HPP_ +#define VEHICLE_INFO_HPP_ + +#include +#include + +// This file should be included after messages. +#include "utils/types.hpp" + +namespace default_ad_api +{ + +class VehicleInfoNode : public rclcpp::Node +{ +public: + explicit VehicleInfoNode(const rclcpp::NodeOptions & options); + +private: + Srv srv_dimensions_; + autoware_adapi_v1_msgs::msg::VehicleDimensions dimensions_; +}; + +} // namespace default_ad_api + +#endif // VEHICLE_INFO_HPP_ From 290d3c2f57b0c94da032525c37b9ca7cd498b34d Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 10 May 2023 17:44:03 +0900 Subject: [PATCH 2/3] feat: add footprint Signed-off-by: Takagi, Isamu --- system/default_ad_api/src/vehicle_info.cpp | 23 ++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/system/default_ad_api/src/vehicle_info.cpp b/system/default_ad_api/src/vehicle_info.cpp index 9383442374d82..cff7ac5cd5eae 100644 --- a/system/default_ad_api/src/vehicle_info.cpp +++ b/system/default_ad_api/src/vehicle_info.cpp @@ -16,6 +16,20 @@ #include +namespace +{ + +auto create_point(double x, double y) +{ + geometry_msgs::msg::Point32 point; + point.x = x; + point.y = y; + point.z = 0.0; + return point; +} + +} // namespace + namespace default_ad_api { @@ -38,6 +52,15 @@ VehicleInfoNode::VehicleInfoNode(const rclcpp::NodeOptions & options) dimensions_.right_overhang = vehicle.right_overhang_m; dimensions_.height = vehicle.vehicle_height_m; + const auto l = (vehicle.wheel_tread_m / 2.0) + vehicle.left_overhang_m; + const auto r = (vehicle.wheel_tread_m / 2.0) + vehicle.right_overhang_m; + const auto b = vehicle.rear_overhang_m; + const auto f = vehicle.front_overhang_m + vehicle.wheel_base_m; + dimensions_.footprint.points.push_back(create_point(+f, +r)); + dimensions_.footprint.points.push_back(create_point(+f, -l)); + dimensions_.footprint.points.push_back(create_point(-b, -l)); + dimensions_.footprint.points.push_back(create_point(-b, +r)); + const auto adaptor = component_interface_utils::NodeAdaptor(this); adaptor.init_srv(srv_dimensions_, on_vehicle_dimensions); } From ceb262575a861a88134c1af3d21eab14a3d14213 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Tue, 20 Jun 2023 17:36:55 +0900 Subject: [PATCH 3/3] update api name Signed-off-by: Takagi, Isamu --- .../include/autoware_ad_api_specs/vehicle.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/vehicle.hpp b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/vehicle.hpp index 3bd6a5f02dfbb..4d5acd544b41d 100644 --- a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/vehicle.hpp +++ b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/vehicle.hpp @@ -25,7 +25,7 @@ namespace autoware_ad_api::vehicle struct Dimensions { using Service = autoware_adapi_v1_msgs::srv::GetVehicleDimensions; - static constexpr char name[] = "/api/vehicle/info/dimensions"; + static constexpr char name[] = "/api/vehicle/dimensions"; }; } // namespace autoware_ad_api::vehicle