feat(default_ad_api): add vehicle info api

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 <rclcpp/qos.hpp>
+
+#include <autoware_adapi_v1_msgs/srv/get_vehicle_dimensions.hpp>
+
+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_

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)

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",

system/default_ad_api/package.xml

@@ -28,6 +28,7 @@
   <depend>std_srvs</depend>
   <depend>tier4_control_msgs</depend>
   <depend>tier4_system_msgs</depend>
+  <depend>vehicle_info_util</depend>
 
   <exec_depend>python3-flask</exec_depend>
 

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 <vehicle_info_util/vehicle_info_util.hpp>
+
+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_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::VehicleInfoNode)

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 <autoware_ad_api_specs/vehicle.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+// 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<autoware_ad_api::vehicle::Dimensions> srv_dimensions_;
+  autoware_adapi_v1_msgs::msg::VehicleDimensions dimensions_;
+};
+
+}  // namespace default_ad_api
+
+#endif  // VEHICLE_INFO_HPP_