Merge latest converter

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
autowarefoundation · Dec 17, 2021 · 10d40df · 10d40df
1 parent 25bec20
commit 10d40df
Show file tree

Hide file tree

Showing 2 changed files with 94 additions and 0 deletions.
diff --git a/...tier4_auto_msgs_converter/include/tier4_auto_msgs_converter/tier4_auto_msgs_converter.hpp b/...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
@@ -10,9 +10,11 @@
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
 
+  <depend>autoware_auto_perception_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>
   <depend>autoware_auto_system_msgs</depend>
   <depend>autoware_auto_vehicle_msgs</depend>
+  <depend>tier4_perception_msgs</depend>
   <depend>tier4_planning_msgs</depend>
   <depend>tier4_system_msgs</depend>
   <depend>tier4_vehicle_msgs</depend>