Branch data Line data Source code
1 : : // Copyright 2017-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 <rclcpp/rclcpp.hpp>
18 : : #include <rcutils/cmdline_parser.h>
19 : : #include <common/types.hpp>
20 : : #include <lidar_integration/lidar_integration_common.hpp>
21 : : #include <lidar_integration/vlp16_integration_spoofer.hpp>
22 : :
23 : : #include <string>
24 : : #include <thread>
25 : : #include <chrono>
26 : :
27 : : using autoware::common::types::float32_t;
28 : : using autoware::common::types::float64_t;
29 : : using autoware::common::types::bool8_t;
30 : : using autoware::common::types::char8_t;
31 : :
32 : 4 : int32_t main(const int32_t argc, char8_t ** const argv)
33 : : {
34 : : using namespace std::chrono_literals; // NOLINT
35 : :
36 : : int32_t ret;
37 : : try {
38 [ + - + - : 4 : rclcpp::init(argc, argv);
+ - - - ]
39 : :
40 : : // common stuff
41 [ + - ]: 4 : const char8_t * arg = rcutils_cli_get_option(argv, &argv[argc], "--rpm");
42 : : float32_t rpm = 600.0F;
43 [ + - ]: 4 : if (nullptr != arg) {
44 [ + - ]: 8 : rpm = std::stof(arg);
45 : : }
46 [ + - ]: 4 : arg = rcutils_cli_get_option(argv, &argv[argc], "--runtime");
47 : : uint32_t runtime = 30U;
48 [ - + ]: 4 : if (nullptr != arg) {
49 [ # # # # ]: 0 : runtime = static_cast<uint32_t>(std::stoul(arg));
50 : : }
51 : : // individual spoofers
52 [ + - ]: 4 : arg = rcutils_cli_get_option(argv, &argv[argc], "--ip1");
53 : : const char8_t * ip1 = "127.0.0.1";
54 [ + - ]: 4 : if (nullptr != arg) {
55 : : ip1 = arg;
56 : : }
57 [ + - ]: 4 : arg = rcutils_cli_get_option(argv, &argv[argc], "--port1");
58 : : uint16_t port1 = 5001U;
59 [ + - ]: 4 : if (nullptr != arg) {
60 [ + - - + ]: 8 : port1 = static_cast<uint16_t>(std::stoul(arg));
61 : : }
62 : : const bool8_t do_second_spoof =
63 [ + - ]: 4 : rcutils_cli_option_exist(argv, &argv[argc], "--do_second_spoof");
64 [ + - ]: 4 : arg = rcutils_cli_get_option(argv, &argv[argc], "--ip2");
65 : : const char8_t * ip2 = "127.0.0.1";
66 [ + - ]: 4 : if (nullptr != arg) {
67 : : ip2 = arg;
68 : : }
69 [ + - ]: 4 : arg = rcutils_cli_get_option(argv, &argv[argc], "--port2");
70 : : uint16_t port2 = 5002U;
71 [ - + ]: 4 : if (nullptr != arg) {
72 [ # # # # ]: 0 : port2 = static_cast<uint16_t>(std::stoul(arg));
73 : : }
74 [ + - ]: 4 : const bool8_t do_third_spoof = rcutils_cli_option_exist(argv, &argv[argc], "--do_third_spoof");
75 [ + - ]: 4 : arg = rcutils_cli_get_option(argv, &argv[argc], "--ip3");
76 : : const char8_t * ip3 = "127.0.0.1";
77 [ - + ]: 4 : if (nullptr != arg) {
78 : : ip3 = arg;
79 : : }
80 [ + - ]: 4 : arg = rcutils_cli_get_option(argv, &argv[argc], "--port3");
81 : : uint16_t port3 = 5003U;
82 [ - + ]: 4 : if (nullptr != arg) {
83 [ # # # # ]: 0 : port3 = static_cast<uint16_t>(std::stoul(arg));
84 : : }
85 : : const bool8_t do_fourth_spoof =
86 [ + - ]: 4 : rcutils_cli_option_exist(argv, &argv[argc], "--do_fourth_spoof");
87 [ + - ]: 4 : arg = rcutils_cli_get_option(argv, &argv[argc], "--ip4");
88 : : const char8_t * ip4 = "127.0.0.1";
89 [ - + ]: 4 : if (nullptr != arg) {
90 : : ip4 = arg;
91 : : }
92 [ + - ]: 4 : arg = rcutils_cli_get_option(argv, &argv[argc], "--port4");
93 : : uint16_t port4 = 5004U;
94 [ - + ]: 4 : if (nullptr != arg) {
95 [ # # # # ]: 0 : port4 = static_cast<uint16_t>(std::stoul(arg));
96 : : }
97 : : // help
98 [ + - ]: 4 : bool8_t needs_help = rcutils_cli_option_exist(argv, &argv[argc], "-h");
99 [ + - + - : 4 : needs_help = rcutils_cli_option_exist(argv, &argv[argc], "--help") || needs_help;
+ - ]
100 : : if (needs_help) {
101 : : std::cout << "vlp16_integration_spoofer [OPTION VALUE [...]]" << std::endl;
102 : : std::cout << "Usage:" << std::endl;
103 : : std::cout << "OPTION" << std::endl;
104 : : std::cout << "--rpm\tEquivalent rotation speed of spoofed packets\t" <<
105 : : "Default=600" << std::endl;
106 : : std::cout << "--runtime\tApproximate time this executable runs for(s)\t" <<
107 : : "Default=30" << std::endl;
108 : : std::cout << "--ip1\tTarget ip for the first spoofer\t" <<
109 : : "Default=127.0.0.1" << std::endl;
110 : : std::cout << "--port1\tTarget port for the first spoofer\t" <<
111 : : "Default=5001" << std::endl;
112 : : std::cout << "--do_second_spoof\tIf present, does first and second spoof" << std::endl;
113 : : std::cout << "--ip2\tTarget ip for the second spoofer\t" <<
114 : : "Default=127.0.0.1" << std::endl;
115 : : std::cout << "--port2\tTarget port for the second spoofer\t" <<
116 : : "Default=5001" << std::endl;
117 : : std::cout << "--do_third_spoof\tIf present, does third spoof if second spoof" << std::endl;
118 : : std::cout << "--ip3\tTarget ip for the third spoofer\t" <<
119 : : "Default=127.0.0.1" << std::endl;
120 : : std::cout << "--port3\tTarget port for the third spoofer\t" <<
121 : : "Default=5003" << std::endl;
122 : : std::cout << "--do_fourth_spoof\tIf present, does fourth spoof if second and third spoof" <<
123 : : std::endl;
124 : : std::cout << "--ip4\tTarget ip for the fourth spoofer\t" <<
125 : : "Default=127.0.0.1" << std::endl;
126 : : std::cout << "--port4\tTarget port for the fourth spoofer\t" <<
127 : : "Default=5004" << std::endl;
128 [ # # ]: 0 : throw std::runtime_error{"Exiting due to help"};
129 : : }
130 : :
131 : : ret = 0;
132 : :
133 [ + - - + ]: 4 : if ((rpm < lidar_integration::Vlp16IntegrationSpoofer::MIN_RPM) ||
134 : : (rpm > lidar_integration::Vlp16IntegrationSpoofer::MAX_RPM))
135 : : {
136 [ # # ]: 0 : throw std::runtime_error("Invalid RPM");
137 : : }
138 [ + - ]: 8 : lidar_integration::Vlp16IntegrationSpoofer spoof1(ip1, port1, rpm);
139 [ + - ]: 8 : lidar_integration::Vlp16IntegrationSpoofer spoof2(ip2, port2, rpm);
140 [ + - ]: 8 : lidar_integration::Vlp16IntegrationSpoofer spoof3(ip3, port3, rpm);
141 [ + - ]: 8 : lidar_integration::Vlp16IntegrationSpoofer spoof4(ip4, port4, rpm);
142 : :
143 [ + - ]: 4 : spoof1.start();
144 : :
145 : : uint32_t spoofer_count = 1;
146 [ - + ]: 4 : if (do_second_spoof) {
147 [ # # ]: 0 : spoof2.start();
148 : : spoofer_count++;
149 [ # # ]: 0 : if (do_third_spoof) {
150 [ # # ]: 0 : spoof3.start();
151 : : spoofer_count++;
152 [ # # ]: 0 : if (do_fourth_spoof) {
153 [ # # ]: 0 : spoof4.start();
154 : : spoofer_count++;
155 : : }
156 : : }
157 : : }
158 [ - + - - : 28 : LIDAR_INTEGRATION_INFO("Spoofer(s) number is: %d", spoofer_count);
- - - - -
- - - - -
- - - - +
- + - + -
+ - + - +
- + - + -
+ - + - -
- - - ]
159 : : // FIXME required for integration test due to buffered output
160 : : std::cout << "Spoofer(s) number is: " << spoofer_count << ::std::endl;
161 : :
162 : 4 : const auto end = std::chrono::steady_clock::now() + std::chrono::seconds(runtime);
163 [ + - + + ]: 82724 : while (rclcpp::ok()) {
164 [ + - ]: 41358 : if (end < std::chrono::steady_clock::now()) {
165 : : break;
166 : : }
167 [ + - ]: 41358 : std::this_thread::sleep_for(1ms);
168 : : }
169 : :
170 [ - + - - : 28 : LIDAR_INTEGRATION_INFO("spoofer done");
- - - - -
- - - - -
- - - - +
- + - + -
+ - + - +
- + - + -
+ - + - -
- - - ]
171 [ + - ]: 4 : spoof1.stop();
172 [ + - ]: 4 : std::cout << spoof1.send_count() << " spoofed UDP packets were sent" << std::endl;
173 [ - + ]: 4 : if (do_second_spoof) {
174 [ # # ]: 0 : spoof2.stop();
175 [ # # ]: 0 : if (do_third_spoof) {
176 [ # # ]: 0 : spoof3.stop();
177 [ # # ]: 0 : if (do_fourth_spoof) {
178 [ # # ]: 0 : spoof4.stop();
179 : : }
180 : : }
181 : : }
182 [ - + - - : 28 : LIDAR_INTEGRATION_INFO("Spoofer(s) finished.");
- - - - -
- - - - -
- - - - +
- + - + -
+ - + - +
- + - + -
+ - + - -
- - - ]
183 : 0 : } catch (const std::runtime_error & e) {
184 [ - - - - : 0 : LIDAR_INTEGRATION_ERROR("Got error: %s", e.what());
- - - - -
- - - - -
- - - - -
- - - - -
- - - - -
- - - - -
- - - - -
- - - ]
185 : : ret = 2;
186 : 0 : } catch (...) {
187 [ - - - - : 0 : LIDAR_INTEGRATION_FATAL("Unknown error occured");
- - - - -
- - - - -
- - - - -
- - - - -
- - - - -
- - - - -
- - - - -
- - - ]
188 : : }
189 : :
190 : : return ret;
191 : : }
|