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..4d5acd544b41d --- /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/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 a09cb1222d2f9..28eec2e8eefc0 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 1cc57d5436cd0..c9a3abf9f8e5d 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 d99037fdc42d0..3e11cbb2952ed 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..cff7ac5cd5eae --- /dev/null +++ b/system/default_ad_api/src/vehicle_info.cpp @@ -0,0 +1,71 @@ +// 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 +{ + +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 +{ + +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 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); +} + +} // 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_