LCOV - code coverage report
Current view: top level - src/tools/lidar_integration/src - point_cloud_mutation_spoofer.cpp (source / functions) Hit Total Coverage
Test: lcov.total.filtered Lines: 66 67 98.5 %
Date: 2023-03-03 05:44:19 Functions: 6 6 100.0 %
Legend: Lines: hit not hit | Branches: + taken - not taken # not executed Branches: 58 119 48.7 %

           Branch data     Line data    Source code
       1                 :            : // Copyright 2018 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 <common/types.hpp>
      18                 :            : 
      19                 :            : #include <string>
      20                 :            : #include <thread>
      21                 :            : 
      22                 :            : #include "lidar_integration/point_cloud_mutation_spoofer.hpp"
      23                 :            : #include "lidar_integration/lidar_integration_common.hpp"
      24                 :            : 
      25                 :            : using autoware::common::types::char8_t;
      26                 :            : using autoware::common::types::float32_t;
      27                 :            : 
      28                 :            : namespace lidar_integration
      29                 :            : {
      30                 :          3 : PointCloudMutationSpooferNode::PointCloudMutationSpooferNode(
      31                 :            :   const char8_t * topic,
      32                 :            :   const uint32_t step_mean,
      33                 :            :   const uint32_t step_std,
      34                 :            :   const uint32_t width_mean,
      35                 :            :   const uint32_t width_std,
      36                 :          3 :   const float32_t freq)
      37                 :            : : rclcpp::Node("point_cloud_mutation_spoofer"),
      38                 :            :   m_dis_width(width_mean, width_std),
      39                 :            :   m_dis_row_step(step_mean, step_std),
      40                 :            :   m_dis_data(0, 255),
      41                 :            :   m_dice(0, 16),
      42         [ +  - ]:          3 :   m_sleep_time(static_cast<uint32_t>(1000UL / freq)),
      43   [ +  -  +  -  :          9 :   m_running(false)
          +  -  +  -  +  
                      - ]
      44                 :            : {
      45                 :          3 :   m_mt.seed(m_rd());
      46   [ +  -  +  -  :          9 :   m_pub = create_publisher<sensor_msgs::msg::PointCloud2>(topic, 10);
          +  -  +  -  -  
                      - ]
      47                 :            : 
      48         [ +  - ]:          3 :   std::string frame_id("frameid");
      49         [ +  - ]:          3 :   m_msg.header.frame_id = frame_id;
      50                 :            : 
      51                 :          3 :   m_msg.height = 1U;
      52                 :            : 
      53         [ +  - ]:          3 :   m_msg.fields.resize(4U);
      54         [ +  - ]:          3 :   m_msg.fields[0U].name = "x";
      55         [ +  - ]:          3 :   m_msg.fields[1U].name = "y";
      56         [ +  - ]:          3 :   m_msg.fields[2U].name = "z";
      57         [ +  - ]:          3 :   m_msg.fields[3U].name = "intensity";
      58         [ +  + ]:         15 :   for (uint32_t idx = 0U; idx < 4U; ++idx) {
      59                 :         12 :     m_msg.fields[idx].offset = static_cast<uint32_t>(idx * sizeof(float32_t));
      60                 :         12 :     m_msg.fields[idx].datatype = sensor_msgs::msg::PointField::FLOAT32;
      61                 :         12 :     m_msg.fields[idx].count = 1U;
      62                 :         12 :     m_msg.point_step += static_cast<uint32_t>(sizeof(float32_t));
      63                 :            :   }
      64                 :            : 
      65                 :          3 :   m_msg.is_bigendian = false;
      66         [ -  + ]:          3 :   m_msg.is_dense = false;
      67                 :          3 : }
      68                 :            : 
      69                 :          3 : void PointCloudMutationSpooferNode::init()
      70                 :            : {
      71                 :          3 :   wait_for_matched(1U, std::chrono::seconds(10));
      72   [ -  +  -  -  :         21 :   LIDAR_INTEGRATION_INFO("Spoofer initialized.");
          +  -  +  -  +  
          -  +  -  +  -  
          +  -  +  -  +  
          -  +  -  -  -  
                   -  - ]
      73                 :          3 : }
      74                 :            : 
      75                 :          3 : void PointCloudMutationSpooferNode::wait_for_matched(
      76                 :            :   const uint32_t num_expected_subs,
      77                 :            :   std::chrono::milliseconds match_timeout)
      78                 :            : {
      79                 :          3 :   const auto match_start = std::chrono::steady_clock::now();
      80         [ +  + ]:          5 :   while (m_pub->get_subscription_count() < num_expected_subs) {
      81   [ +  -  +  -  :          2 :     rclcpp::sleep_for(std::chrono::milliseconds(100));
                   -  - ]
      82         [ +  - ]:          2 :     if (std::chrono::steady_clock::now() - match_start > match_timeout) {
      83                 :            :       throw std::runtime_error(
      84                 :            :               "PointCloudMutationSpooferNode: couldn't match "
      85         [ #  # ]:          0 :               "any subscriptions within the initialization time budget.");
      86                 :            :     }
      87                 :            :   }
      88                 :          3 : }
      89                 :            : 
      90                 :          3 : void PointCloudMutationSpooferNode::start()
      91                 :            : {
      92                 :            :   m_running.store(true);
      93                 :          6 :   m_thread = std::thread {[this] {task_function();}};
      94                 :          3 : }
      95                 :            : 
      96         [ +  - ]:          3 : void PointCloudMutationSpooferNode::stop()
      97                 :            : {
      98                 :            :   m_running.store(false);
      99         [ +  - ]:          3 :   if (m_thread.joinable()) {
     100                 :          3 :     m_thread.join();
     101                 :            :   }
     102                 :          3 : }
     103                 :            : 
     104                 :          3 : void PointCloudMutationSpooferNode::task_function()
     105                 :            : {
     106         [ +  + ]:        761 :   while (m_running.load(std::memory_order_relaxed)) {
     107                 :        758 :     m_msg.width = static_cast<uint32_t>(m_dis_width(m_mt));
     108                 :            : 
     109                 :        758 :     uint32_t data_length = static_cast<uint32_t>(m_dis_row_step(m_mt));
     110                 :        758 :     m_msg.row_step = data_length;
     111                 :            : 
     112                 :        758 :     uint32_t capacity = m_msg.row_step * m_msg.height;
     113                 :            : 
     114         [ +  + ]:        758 :     m_msg.data.clear();
     115                 :        758 :     m_msg.data.reserve(capacity);
     116                 :            : 
     117         [ +  + ]:  202566000 :     for (std::size_t i = 0; i < capacity; ++i) {
     118                 :  202565000 :       m_msg.data.push_back(static_cast<uint8_t>(m_dis_data(m_mt)));
     119                 :            :     }
     120                 :            : 
     121   [ +  +  +  +  :        758 :     switch (m_dice(m_mt)) {
                      + ]
     122                 :         43 :       case 0:
     123                 :            :         // Shuffle the data randomly.
     124                 :         43 :         std::random_shuffle(m_msg.data.begin(), m_msg.data.end());
     125                 :         43 :         break;
     126                 :         52 :       case 1:
     127                 :            :         // Data length is not as described. Equation not holds.
     128                 :         52 :         m_msg.row_step = static_cast<uint32_t>(m_dis_row_step(m_mt));
     129                 :         52 :         break;
     130                 :         42 :       case 2:
     131                 :            :         // change in frequency, or simulate communication delay.
     132                 :            :         // This will introduce 0~12ms lag.
     133                 :         42 :         std::this_thread::sleep_for(std::chrono::milliseconds(m_dis_data(m_mt) / 20U));
     134                 :         42 :         break;
     135                 :         43 :       case 3:
     136                 :            :         // not full scan
     137                 :         43 :         m_msg.data.resize(m_msg.row_step * 3U / 4U);
     138                 :         43 :         break;
     139                 :            :       case 4:
     140                 :            :         // data from rosbag message in the future?
     141                 :            :         break;
     142                 :            :       default:
     143                 :            :         break;
     144                 :            :     }
     145                 :            : 
     146                 :        758 :     this->m_pub->publish(this->m_msg);
     147                 :            : 
     148   [ -  +  -  -  :       5306 :     LIDAR_INTEGRATION_INFO("New message is published. m_msg.data.size=%lu", m_msg.data.size());
          +  -  +  -  +  
          -  +  -  +  -  
          +  -  +  -  +  
          -  +  -  -  -  
                   -  - ]
     149                 :            : 
     150                 :        758 :     std::this_thread::sleep_for(std::chrono::milliseconds(m_sleep_time));
     151                 :            :   }
     152                 :          3 : }
     153                 :            : }  // namespace lidar_integration

Generated by: LCOV version 1.14