From 10d40dfee76233208ecb1aad3a56c737c33beb39 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Thu, 16 Dec 2021 17:33:26 +0900 Subject: [PATCH] Merge latest converter Signed-off-by: Takagi, Isamu --- .../tier4_auto_msgs_converter.hpp | 92 +++++++++++++++++++ .../tier4_auto_msgs_converter/package.xml | 2 + 2 files changed, 94 insertions(+) diff --git a/messages/tier4_auto_msgs_converter/include/tier4_auto_msgs_converter/tier4_auto_msgs_converter.hpp b/messages/tier4_auto_msgs_converter/include/tier4_auto_msgs_converter/tier4_auto_msgs_converter.hpp index 422623d368535..6784b9bb23bf7 100644 --- a/messages/tier4_auto_msgs_converter/include/tier4_auto_msgs_converter/tier4_auto_msgs_converter.hpp +++ b/messages/tier4_auto_msgs_converter/include/tier4_auto_msgs_converter/tier4_auto_msgs_converter.hpp @@ -15,6 +15,7 @@ #ifndef TIER4_AUTO_MSGS_CONVERTER__TIER4_AUTO_MSGS_CONVERTER_HPP_ #define TIER4_AUTO_MSGS_CONVERTER__TIER4_AUTO_MSGS_CONVERTER_HPP_ +#include "autoware_auto_perception_msgs/msg/tracked_objects.hpp" #include "autoware_auto_planning_msgs/msg/path.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_system_msgs/msg/autoware_state.hpp" @@ -29,6 +30,7 @@ #include "autoware_auto_vehicle_msgs/msg/turn_indicators_report.hpp" #include "tier4_external_api_msgs/msg/gear_shift_stamped.hpp" #include "tier4_external_api_msgs/msg/turn_signal_stamped.hpp" +#include "tier4_perception_msgs/msg/dynamic_object_array.hpp" #include "tier4_planning_msgs/msg/path.hpp" #include "tier4_planning_msgs/msg/trajectory.hpp" #include "tier4_system_msgs/msg/autoware_state.hpp" @@ -276,6 +278,96 @@ inline auto convert(const autoware_auto_vehicle_msgs::msg::SteeringReport & stee return iv_steering; } +inline auto convert(const autoware_auto_perception_msgs::msg::ObjectClassification & classification) +{ + tier4_perception_msgs::msg::Semantic iv_semantic; + iv_semantic.confidence = classification.probability; + switch (classification.label) { + case autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN: + iv_semantic.type = tier4_perception_msgs::msg::Semantic::UNKNOWN; + break; + case autoware_auto_perception_msgs::msg::ObjectClassification::CAR: + iv_semantic.type = tier4_perception_msgs::msg::Semantic::CAR; + break; + case autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK: + iv_semantic.type = tier4_perception_msgs::msg::Semantic::TRUCK; + break; + case autoware_auto_perception_msgs::msg::ObjectClassification::BUS: + iv_semantic.type = tier4_perception_msgs::msg::Semantic::BUS; + break; + case autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER: + iv_semantic.type = tier4_perception_msgs::msg::Semantic::TRUCK; + break; + case autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE: + iv_semantic.type = tier4_perception_msgs::msg::Semantic::MOTORBIKE; + break; + case autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE: + iv_semantic.type = tier4_perception_msgs::msg::Semantic::BICYCLE; + break; + case autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN: + iv_semantic.type = tier4_perception_msgs::msg::Semantic::PEDESTRIAN; + break; + default: + iv_semantic.type = tier4_perception_msgs::msg::Semantic::UNKNOWN; + break; + } + return iv_semantic; +} + +inline auto convert(const autoware_auto_perception_msgs::msg::TrackedObjectKinematics & kinematics) +{ + using Kinematics = autoware_auto_perception_msgs::msg::TrackedObjectKinematics; + tier4_perception_msgs::msg::State iv_state; + iv_state.pose_covariance = kinematics.pose_with_covariance; + iv_state.orientation_reliable = (kinematics.orientation_availability == Kinematics::AVAILABLE); + iv_state.twist_covariance = kinematics.twist_with_covariance; + iv_state.twist_reliable = true; + iv_state.acceleration_covariance = kinematics.acceleration_with_covariance; + iv_state.acceleration_reliable = true; + iv_state.predicted_paths = {}; + return iv_state; +} + +inline auto convert(const autoware_auto_perception_msgs::msg::Shape & shape) +{ + tier4_perception_msgs::msg::Shape iv_shape; + iv_shape.dimensions = shape.dimensions; + iv_shape.footprint = shape.footprint; + switch (shape.type) { + case autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX: + iv_shape.type = tier4_perception_msgs::msg::Shape::BOUNDING_BOX; + break; + case autoware_auto_perception_msgs::msg::Shape::CYLINDER: + iv_shape.type = tier4_perception_msgs::msg::Shape::CYLINDER; + break; + case autoware_auto_perception_msgs::msg::Shape::POLYGON: + iv_shape.type = tier4_perception_msgs::msg::Shape::POLYGON; + break; + } + return iv_shape; +} + +inline auto convert(const autoware_auto_perception_msgs::msg::TrackedObject & object) +{ + tier4_perception_msgs::msg::DynamicObject iv_object; + iv_object.id = object.object_id; + iv_object.semantic = convert(object.classification.front()); + iv_object.state = convert(object.kinematics); + iv_object.shape = convert(object.shape); + return iv_object; +} + +inline auto convert(const autoware_auto_perception_msgs::msg::TrackedObjects & objects) +{ + tier4_perception_msgs::msg::DynamicObjectArray iv_objects; + iv_objects.header = objects.header; + iv_objects.objects.reserve(objects.objects.size()); + for (const auto & object : objects.objects) { + iv_objects.objects.push_back(convert(object)); + } + return iv_objects; +} + } // namespace tier4_auto_msgs_converter #endif // TIER4_AUTO_MSGS_CONVERTER__TIER4_AUTO_MSGS_CONVERTER_HPP_ diff --git a/messages/tier4_auto_msgs_converter/package.xml b/messages/tier4_auto_msgs_converter/package.xml index 5058c429189e2..6d3e19a44216f 100644 --- a/messages/tier4_auto_msgs_converter/package.xml +++ b/messages/tier4_auto_msgs_converter/package.xml @@ -10,9 +10,11 @@ ament_cmake_auto + autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_system_msgs autoware_auto_vehicle_msgs + tier4_perception_msgs tier4_planning_msgs tier4_system_msgs tier4_vehicle_msgs