feat(default_ad_api): add object recognition api (autowarefoundation#…

…2887)

* add object recognition api

Signed-off-by: tkhmy <tkh.my.p@gmail.com>

* add unorder map

Signed-off-by: tkhmy <tkh.my.p@gmail.com>

* pre-commit

Signed-off-by: tkhmy <tkh.my.p@gmail.com>

* add missing time_span

Signed-off-by: tkhmy <tkh.my.p@gmail.com>

* change naming

Signed-off-by: tkhmy <tkh.my.p@gmail.com>

* update message

Signed-off-by: tkhmy <tkh.my.p@gmail.com>

* change style

Signed-off-by: tkhmy <tkh.my.p@gmail.com>

* change topic naming

Signed-off-by: tkhmy <tkh.my.p@gmail.com>

---------

Signed-off-by: tkhmy <tkh.my.p@gmail.com>
Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com>

common/autoware_ad_api_specs/include/autoware_ad_api_specs/perception.hpp

@@ -0,0 +1,36 @@
+// Copyright 2022 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__PERCEPTION_HPP_
+#define AUTOWARE_AD_API_SPECS__PERCEPTION_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/dynamic_object_array.hpp>
+
+namespace autoware_ad_api::perception
+{
+
+struct DynamicObjectArray
+{
+  using Message = autoware_adapi_v1_msgs::msg::DynamicObjectArray;
+  static constexpr char name[] = "/api/perception/objects";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
+}  // namespace autoware_ad_api::perception
+
+#endif  // AUTOWARE_AD_API_SPECS__PERCEPTION_HPP_

common/component_interface_specs/include/component_interface_specs/perception.hpp

@@ -0,0 +1,36 @@
+// Copyright 2022 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 COMPONENT_INTERFACE_SPECS__PERCEPTION_HPP_
+#define COMPONENT_INTERFACE_SPECS__PERCEPTION_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
+
+namespace perception_interface
+{
+
+struct ObjectRecognition
+{
+  using Message = autoware_auto_perception_msgs::msg::PredictedObjects;
+  static constexpr char name[] = "/perception/object_recognition/objects";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
+}  // namespace perception_interface
+
+#endif  // COMPONENT_INTERFACE_SPECS__PERCEPTION_HPP_

system/default_ad_api/CMakeLists.txt

@@ -10,6 +10,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/localization.cpp
   src/motion.cpp
   src/operation_mode.cpp
+  src/perception.cpp
   src/planning.cpp
   src/routing.cpp
   src/vehicle.cpp
@@ -25,6 +26,7 @@ rclcpp_components_register_nodes(${PROJECT_NAME}
   "default_ad_api::LocalizationNode"
   "default_ad_api::MotionNode"
   "default_ad_api::OperationModeNode"
+  "default_ad_api::PerceptionNode"
   "default_ad_api::PlanningNode"
   "default_ad_api::RoutingNode"
   "default_ad_api::VehicleNode"

system/default_ad_api/launch/default_ad_api.launch.py

@@ -47,6 +47,7 @@ def generate_launch_description():
         create_api_node("localization", "LocalizationNode"),
         create_api_node("motion", "MotionNode"),
         create_api_node("operation_mode", "OperationModeNode"),
+        create_api_node("perception", "PerceptionNode"),
         create_api_node("planning", "PlanningNode"),
         create_api_node("routing", "RoutingNode"),
         create_api_node("vehicle", "VehicleNode"),

system/default_ad_api/package.xml

@@ -28,6 +28,7 @@
   <depend>nav_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
+  <depend>shape_msgs</depend>
   <depend>std_srvs</depend>
   <depend>tier4_api_msgs</depend>
   <depend>tier4_control_msgs</depend>

system/default_ad_api/src/perception.cpp

@@ -0,0 +1,93 @@
+// Copyright 2022 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 "perception.hpp"
+
+#include <vector>
+
+namespace default_ad_api
+{
+
+using DynamicObjectArray = autoware_ad_api::perception::DynamicObjectArray;
+using ObjectClassification = autoware_adapi_v1_msgs::msg::ObjectClassification;
+using DynamicObject = autoware_adapi_v1_msgs::msg::DynamicObject;
+using DynamicObjectPath = autoware_adapi_v1_msgs::msg::DynamicObjectPath;
+using API_Shape = shape_msgs::msg::SolidPrimitive;
+using Shape = autoware_auto_perception_msgs::msg::Shape;
+
+std::unordered_map<uint8_t, uint8_t> shape_type_ = {
+  {Shape::BOUNDING_BOX, API_Shape::BOX},
+  {Shape::CYLINDER, API_Shape::CYLINDER},
+  {Shape::POLYGON, API_Shape::PRISM},
+};
+
+PerceptionNode::PerceptionNode(const rclcpp::NodeOptions & options) : Node("perception", options)
+{
+  const auto adaptor = component_interface_utils::NodeAdaptor(this);
+  adaptor.init_pub(pub_object_recognized_);
+  adaptor.init_sub(sub_object_recognized_, this, &PerceptionNode::object_recognize);
+}
+
+uint8_t PerceptionNode::mapping(
+  std::unordered_map<uint8_t, uint8_t> hash_map, uint8_t input, uint8_t default_value)
+{
+  if (hash_map.find(input) == hash_map.end()) {
+    return default_value;
+  } else {
+    return hash_map[input];
+  }
+}
+
+void PerceptionNode::object_recognize(
+  const perception_interface::ObjectRecognition::Message::ConstSharedPtr msg)
+{
+  DynamicObjectArray::Message objects;
+  objects.header = msg->header;
+  for (const auto & msg_object : msg->objects) {
+    DynamicObject object;
+    object.id = msg_object.object_id;
+    object.existence_probability = msg_object.existence_probability;
+    for (const auto & msg_classification : msg_object.classification) {
+      ObjectClassification classification;
+      classification.label = msg_classification.label;
+      classification.probability = msg_classification.probability;
+      object.classification.insert(object.classification.begin(), classification);
+    }
+    object.kinematics.pose = msg_object.kinematics.initial_pose_with_covariance.pose;
+    object.kinematics.twist = msg_object.kinematics.initial_twist_with_covariance.twist;
+    object.kinematics.accel = msg_object.kinematics.initial_acceleration_with_covariance.accel;
+    for (const auto & msg_predicted_path : msg_object.kinematics.predicted_paths) {
+      DynamicObjectPath predicted_path;
+      for (const auto & msg_path : msg_predicted_path.path) {
+        predicted_path.path.insert(predicted_path.path.begin(), msg_path);
+      }
+      predicted_path.time_step = msg_predicted_path.time_step;
+      predicted_path.confidence = msg_predicted_path.confidence;
+      object.kinematics.predicted_paths.insert(
+        object.kinematics.predicted_paths.begin(), predicted_path);
+    }
+    object.shape.type = mapping(shape_type_, msg_object.shape.type, API_Shape::PRISM);
+    object.shape.dimensions = {
+      msg_object.shape.dimensions.x, msg_object.shape.dimensions.y, msg_object.shape.dimensions.z};
+    object.shape.polygon = msg_object.shape.footprint;
+    objects.objects.insert(objects.objects.begin(), object);
+  }
+
+  pub_object_recognized_->publish(objects);
+}
+
+}  // namespace default_ad_api
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::PerceptionNode)

system/default_ad_api/src/perception.hpp

@@ -0,0 +1,52 @@
+// Copyright 2022 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 PERCEPTION_HPP_
+#define PERCEPTION_HPP_
+
+#include <autoware_ad_api_specs/perception.hpp>
+#include <component_interface_specs/perception.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/dynamic_object.hpp>
+#include <autoware_adapi_v1_msgs/msg/dynamic_object_path.hpp>
+#include <autoware_adapi_v1_msgs/msg/object_classification.hpp>
+#include <autoware_auto_perception_msgs/msg/shape.hpp>
+#include <shape_msgs/msg/solid_primitive.hpp>
+
+#include <unordered_map>
+#include <vector>
+
+// This file should be included after messages.
+#include "utils/types.hpp"
+
+namespace default_ad_api
+{
+
+class PerceptionNode : public rclcpp::Node
+{
+public:
+  explicit PerceptionNode(const rclcpp::NodeOptions & options);
+
+private:
+  Pub<autoware_ad_api::perception::DynamicObjectArray> pub_object_recognized_;
+  Sub<perception_interface::ObjectRecognition> sub_object_recognized_;
+  void object_recognize(const perception_interface::ObjectRecognition::Message::ConstSharedPtr msg);
+  uint8_t mapping(
+    std::unordered_map<uint8_t, uint8_t> hash_map, uint8_t input, uint8_t default_value);
+};
+
+}  // namespace default_ad_api
+
+#endif  // PERCEPTION_HPP_