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
|