Branch data     Line data    Source code

       1                 :            : // Copyright 2021 The Autoware Foundation
       2                 :            : //
       3                 :            : // Licensed under the Apache License, Version 2.0 (the "License");
       4                 :            : // you may not use this file except in compliance with the License.
       5                 :            : // You may obtain a copy of the License at
       6                 :            : //
       7                 :            : //     http://www.apache.org/licenses/LICENSE-2.0
       8                 :            : //
       9                 :            : // Unless required by applicable law or agreed to in writing, software
      10                 :            : // distributed under the License is distributed on an "AS IS" BASIS,
      11                 :            : // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
      12                 :            : // See the License for the specific language governing permissions and
      13                 :            : // limitations under the License.
      14                 :            : //
      15                 :            : // Co-developed by Tier IV, Inc. and Apex.AI, Inc.
      16                 :            : 
      17                 :            : #include "ground_truth_detections/ground_truth_detections_node.hpp"
      18                 :            : 
      19                 :            : #include <autoware_auto_perception_msgs/msg/classified_roi.hpp>
      20                 :            : #include <autoware_auto_perception_msgs/msg/detected_object.hpp>
      21                 :            : #include <algorithm>
      22                 :            : 
      23                 :            : namespace autoware
      24                 :            : {
      25                 :            : namespace ground_truth_detections
      26                 :            : {
      27                 :            : 
      28                 :            : constexpr char GroundTruthDetectionsNode::kFrameId3d[];
      29                 :            : 
      30                 :          2 : GroundTruthDetectionsNode::GroundTruthDetectionsNode(const rclcpp::NodeOptions & options)
      31                 :            : : Node("ground_truth_detections", options),
      32                 :            :   m_detection2d_pub{create_publisher<autoware_auto_perception_msgs::msg::ClassifiedRoiArray>(
      33         [ +  - ]:          2 :       "/perception/ground_truth_detections_2d", rclcpp::QoS{10})},
      34                 :            :   m_detection2d_sub{create_subscription<lgsvl_msgs::msg::Detection2DArray>(
      35                 :          0 :       "/simulator/ground_truth/detections2D", rclcpp::QoS{10},
      36                 :          1 :       [this](lgsvl_msgs::msg::Detection2DArray::SharedPtr msg) {on_detection(*msg);}
      37                 :            :     )},
      38                 :            :   m_vision_frame_id(this->declare_parameter("vision_frame_id", "camera")),
      39                 :            :   m_detection3d_pub{create_publisher<autoware_auto_perception_msgs::msg::DetectedObjects>(
      40         [ +  - ]:          2 :     "/perception/ground_truth_detections_3d", rclcpp::QoS{10})},
      41                 :            : m_detection3d_sub{create_subscription<lgsvl_msgs::msg::Detection3DArray>(
      42                 :          0 :     "/simulator/ground_truth/detections3D", rclcpp::QoS{10},
      43                 :          2 :     [this](lgsvl_msgs::msg::Detection3DArray::SharedPtr msg) {on_detection(*msg);}
      44   [ +  -  +  -  :          6 :   )}
          +  -  +  -  +  
          -  +  -  +  -  
          +  -  +  -  +  
          -  +  -  +  -  
          +  -  +  -  +  
          -  +  -  +  -  
             +  -  +  - ]
      45                 :            : {
      46                 :          2 : }
      47                 :            : 
      48                 :          1 : void GroundTruthDetectionsNode::on_detection(const lgsvl_msgs::msg::Detection2DArray & msg)
      49                 :            : {
      50         [ +  - ]:          2 :   autoware_auto_perception_msgs::msg::ClassifiedRoiArray roi_array;
      51         [ +  - ]:          1 :   roi_array.header = msg.header;
      52         [ +  - ]:          1 :   roi_array.header.frame_id = m_vision_frame_id;
      53                 :            : 
      54         [ +  - ]:          1 :   roi_array.rois.resize(msg.detections.size());
      55                 :            :   std::transform(
      56                 :            :     msg.detections.begin(), msg.detections.end(), roi_array.rois.begin(),
      57                 :          7 :     [](const lgsvl_msgs::msg::Detection2D & detection) {
      58                 :            :       return autoware_auto_perception_msgs::build<
      59                 :         14 :         autoware_auto_perception_msgs::msg::ClassifiedRoi>()
      60   [ +  -  +  -  :         14 :       .classifications({make_classification(detection.label)})
                   +  - ]
      61         [ +  - ]:         14 :       .polygon(make_polygon(detection));
      62         [ +  - ]:          1 :     });
      63         [ +  - ]:          1 :   m_detection2d_pub->publish(roi_array);
      64                 :          1 : }
      65                 :            : 
      66                 :          2 : void GroundTruthDetectionsNode::on_detection(const lgsvl_msgs::msg::Detection3DArray & msg)
      67                 :            : {
      68         [ +  - ]:          4 :   autoware_auto_perception_msgs::msg::DetectedObjects detected_objects;
      69         [ +  - ]:          2 :   detected_objects.header = msg.header;
      70         [ +  - ]:          2 :   detected_objects.header.frame_id = kFrameId3d;
      71                 :            : 
      72         [ +  - ]:          2 :   detected_objects.objects.resize(msg.detections.size());
      73                 :            :   const auto create_detected_object =
      74                 :          2 :     [](const lgsvl_msgs::msg::Detection3D & detection) {
      75                 :            :       return autoware_auto_perception_msgs::build<
      76                 :          4 :         autoware_auto_perception_msgs::msg::DetectedObject>()
      77         [ +  - ]:          4 :              .existence_probability(1.0F)
      78   [ +  -  +  -  :          4 :              .classification({make_classification(detection.label)})
                   +  - ]
      79   [ +  -  +  - ]:          4 :              .kinematics(make_kinematics(detection))
      80         [ +  - ]:          4 :              .shape(make_shape(detection));
      81                 :            :     };
      82                 :            : 
      83                 :            :   std::transform(
      84                 :            :     msg.detections.begin(), msg.detections.end(),
      85         [ +  - ]:          2 :     detected_objects.objects.begin(), create_detected_object);
      86         [ +  - ]:          2 :   m_detection3d_pub->publish(detected_objects);
      87                 :          2 : }
      88                 :            : }  // namespace ground_truth_detections
      89                 :            : }  // namespace autoware
      90                 :            : 
      91                 :            : #include "rclcpp_components/register_node_macro.hpp"
      92                 :            : 
      93                 :            : // This acts as an entry point, allowing the component to be
      94                 :            : // discoverable when its library is being loaded into a running process
      95   [ +  -  -  +  :          1 : RCLCPP_COMPONENTS_REGISTER_NODE(autoware::ground_truth_detections::GroundTruthDetectionsNode)
          +  -  +  -  +  
                      - ]