7
7
* You may use, distribute and modify this code under the BSD-3-Clause license.
8
8
*/
9
9
10
+ #include < stdlib.h>
11
+ #include < sys/wait.h>
12
+
10
13
#include < vector>
11
14
#include < chrono>
12
15
#include < iostream>
13
16
#include < fstream>
14
17
#include < string>
15
- #include < stdlib.h>
16
- #include < sys/wait.h>
17
18
18
- #include < rclcpp/rclcpp.hpp>
19
+ #include " rclcpp/rclcpp.hpp"
19
20
20
21
#include " performance_test/communication.hpp"
21
22
#include " performance_test/performance_node.hpp"
30
31
using namespace std ::chrono_literals;
31
32
32
33
static void run_test (
33
- int argc, char ** argv,
34
- const performance_test::Options &options,
35
- const std::string &topology_json,
36
- pid_t &pid,
37
- const std::string &resources_output_path,
38
- const std::string &events_output_path,
39
- const std::string &latency_all_output_path,
40
- const std::string &latency_total_output_path)
34
+ int argc, char ** argv,
35
+ const performance_test::Options & options,
36
+ const std::string & topology_json,
37
+ pid_t & pid,
38
+ const std::string & resources_output_path,
39
+ const std::string & events_output_path,
40
+ const std::string & latency_all_output_path,
41
+ const std::string & latency_total_output_path)
41
42
{
42
43
// Start resources logger
43
44
performance_test::ResourceUsageLogger ru_logger (resources_output_path);
@@ -46,12 +47,18 @@ static void run_test(
46
47
47
48
rclcpp::init (argc, argv);
48
49
49
- performance_test::System ros2_system (static_cast <performance_test::ExecutorType>(options.executor ));
50
+ performance_test::System ros2_system (
51
+ static_cast <performance_test::ExecutorType>(options.executor ));
50
52
51
53
// Get the node type from options
52
54
auto node_type = static_cast <performance_test::NodeType>(options.node );
53
55
// Load topology from json file
54
- auto factory = performance_test::TemplateFactory (options.ipc , options.ros_params , false , " " , node_type);
56
+ auto factory = performance_test::TemplateFactory (
57
+ options.ipc ,
58
+ options.ros_params ,
59
+ false ,
60
+ " " ,
61
+ node_type);
55
62
56
63
if (options.tracking_options .is_enabled ) {
57
64
ros2_system.enable_events_logger (events_output_path);
@@ -71,8 +78,7 @@ static void run_test(
71
78
72
79
ros2_system.print_latency_all_stats ();
73
80
74
- if (options.topology_json_list .size () > 1 )
75
- {
81
+ if (options.topology_json_list .size () > 1 ) {
76
82
std::cout << std::endl << " Process total:" << std::endl;
77
83
ros2_system.print_latency_total_stats ();
78
84
}
@@ -82,18 +88,16 @@ static void run_test(
82
88
ros2_system.save_latency_total_stats (latency_total_output_path);
83
89
84
90
// Parent process: wait for children to exit and print system stats
85
- if (pid != 0 )
86
- {
87
- if (options.topology_json_list .size () > 1 )
88
- {
91
+ if (pid != 0 ) {
92
+ if (options.topology_json_list .size () > 1 ) {
89
93
waitpid (getpid () + 1 , &pid, 0 );
90
94
}
91
95
std::cout << " System total:" << std::endl;
92
96
ros2_system.print_agregate_stats (options.topology_json_list );
93
97
}
94
98
}
95
99
96
- int main (int argc, char ** argv)
100
+ int main (int argc, char ** argv)
97
101
{
98
102
auto non_ros_args = rclcpp::remove_ros_arguments (argc, argv);
99
103
std::vector<char *> non_ros_args_c_strings;
@@ -103,7 +107,7 @@ int main(int argc, char** argv)
103
107
int non_ros_argc = static_cast <int >(non_ros_args_c_strings.size ());
104
108
auto options = performance_test::Options (non_ros_argc, non_ros_args_c_strings.data ());
105
109
106
- std::cout<< options<< " \n " << " Start test!" << std::endl;
110
+ std::cout << options << " \n " << " Start test!" << std::endl;
107
111
108
112
pid_t pid = getpid ();
109
113
size_t process_index = performance_test::fork_process (options.topology_json_list .size ());
@@ -113,14 +117,14 @@ int main(int argc, char** argv)
113
117
// Create results dir based on the topology name
114
118
const size_t last_slash = topology_json.find_last_of (" /" );
115
119
std::string topology_basename = topology_json.substr (last_slash + 1 , topology_json.length ());
116
- std::string dir_name = topology_basename.substr (0 ,topology_basename.length ()- 5 ) + " _log" ;
120
+ std::string dir_name = topology_basename.substr (0 , topology_basename.length () - 5 ) + " _log" ;
117
121
118
122
std::string make_dir = " mkdir -p " + dir_name;
119
123
const auto ret = system (make_dir.c_str ());
120
124
static_cast <void >(ret);
121
- std::string resources_output_path = dir_name + " /resources.txt" ;
122
- std::string events_output_path = dir_name + " /events.txt" ;
123
- std::string latency_all_output_path = dir_name + " /latency_all.txt" ;
125
+ std::string resources_output_path = dir_name + " /resources.txt" ;
126
+ std::string events_output_path = dir_name + " /events.txt" ;
127
+ std::string latency_all_output_path = dir_name + " /latency_all.txt" ;
124
128
std::string latency_total_output_path = dir_name + " /latency_total.txt" ;
125
129
126
130
run_test (
0 commit comments