feat(default_ad_api): add vehicle info api (autowarefoundation#2984)

* feat(default_ad_api): add vehicle dimensions api

Signed-off-by: Takagi, Isamu <[email protected]>

* feat: add footprint

Signed-off-by: Takagi, Isamu <[email protected]>

* update api name

Signed-off-by: Takagi, Isamu <[email protected]>

---------

Signed-off-by: Takagi, Isamu <[email protected]>

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_