Skip to content

Commit 4ad2aef

Browse files
committed
feat: update
Signed-off-by: Berkay Karaman <brkay54@gmail.com>
1 parent 3684f1a commit 4ad2aef

9 files changed

+119
-103
lines changed

tools/reaction_analyzer/README.md

+20-13
Original file line numberDiff line numberDiff line change
@@ -2,10 +2,11 @@
22

33
## Description
44

5-
The main purpose of the reaction analyzer package is measuring reaction times of the nodes by listening the
6-
pre-determined topics. It can be used for measuring the reaction time of the perception pipeline, planning pipeline, and
7-
control pipeline. To be able to measure both control outputs and perception outputs, it was necessary to divide the node
8-
into two running_mode: `planning_control` and `perception_planning`.
5+
The main purpose of the reaction analyzer package is to measure the reaction times of various nodes within a ROS-based
6+
autonomous driving simulation environment by subscribing to pre-determined topics. This tool is particularly useful for
7+
evaluating the performance of perception, planning, and control pipelines in response to dynamic changes in the
8+
environment, such as sudden obstacles. To be able to measure both control outputs and perception outputs, it was
9+
necessary to divide the node into two running_mode: `planning_control` and `perception_planning`.
910

1011
![ReactionAnalyzerDesign.png](media%2FReactionAnalyzerDesign.png)
1112

@@ -94,21 +95,26 @@ to test. After the test is completed, the results will be stored in the `output_
9495

9596
### Custom Test Environment
9697

97-
**If you want to run the reaction analyzer with your custom test environment, you need to redefine some of the parameters.
98+
**If you want to run the reaction analyzer with your custom test environment, you need to redefine some of the
99+
parameters.
98100
The parameters you need to redefine are `initialization_pose`, `entity_params`, `goal_pose`, and `topic_publisher` (
99101
for `perception_planning` mode) parameters.**
100102

101103
- To set `initialization_pose`, `entity_params`, `goal_pose`:
102-
- Upload your `.osm` map file into the [scenario editor](https://scenario.ci.tier4.jp/scenario_editor/) to define the position of the position parameters.
104+
- Upload your `.osm` map file into the [scenario editor](https://scenario.ci.tier4.jp/scenario_editor/) to define the
105+
position of the position parameters.
103106
- Add EGO vehicle from edit/add entity/Ego to map.
104107
- Set destination to EGO vehicle and add another dummy object in same way. The dummy object represents the object spawn
105108
suddenly in the reaction analyzer test.
106109

107-
**After you set up the positions in the map, we should get the positions of these entities in the map frame. To achieve this:**
110+
**After you set up the positions in the map, we should get the positions of these entities in the map frame. To achieve
111+
this:**
108112

109-
- Convert the positions to map frame by changing Map/Coordinate to World and Map/Orientation to Euler in Scenario Editor.
113+
- Convert the positions to map frame by changing Map/Coordinate to World and Map/Orientation to Euler in Scenario
114+
Editor.
110115

111-
- After these steps, you can see the positions in map frame and euler angles. You can change the `initialization_pose`, `entity_params`, `goal_pose` parameters with the values you get from the website.
116+
- After these steps, you can see the positions in map frame and euler angles. You can change
117+
the `initialization_pose`, `entity_params`, `goal_pose` parameters with the values you get from the website.
112118

113119
**For the `topic_publisher` parameters, you need to record the rosbags from the AWSIM. After opened your AWSIM
114120
environment, you should record two different rosbags. However, the environment should be static and the position of the
@@ -117,7 +123,8 @@ vehicle should be same.**
117123
- Record a rosbag in empty environment (without an obstacle in front of the vehicle).
118124
- After that, record another rosbag in the same environment except add an object in front of the vehicle.
119125

120-
**After you record the rosbags, you can set the `path_bag_without_object` and `path_bag_with_object` parameters with the paths of the recorded rosbags.**
126+
**After you record the rosbags, you can set the `path_bag_without_object` and `path_bag_with_object` parameters with the
127+
paths of the recorded rosbags.**
121128

122129
## Parameters
123130

@@ -131,9 +138,9 @@ vehicle should be same.**
131138
| `spawn_distance_threshold` | double | [m] Distance threshold for spawning objects. Only valid `planning_control` mode. |
132139
| `spawned_pointcloud_sampling_distance` | double | [m] Sampling distance for point clouds of spawned objects. Only valid `planning_control` mode. |
133140
| `dummy_perception_publisher_period` | double | [s] Publishing period for the dummy perception data. Only valid `planning_control` mode. |
134-
| `initialization_pose` | struct | Initial pose of the vehicle, containing `x`, `y`, `z`, `roll`, `pitch`, and `yaw` fields. Only valid `planning_control` mode. |
135-
| `entity_params` | struct | Parameters for entities (e.g., obstacles), containing `x`, `y`, `z`, `roll`, `pitch`, `yaw`, `x_dimension`, `y_dimension`, and `z_dimension`. |
136-
| `goal_pose` | struct | Goal pose of the vehicle, containing `x`, `y`, `z`, `roll`, `pitch`, and `yaw` fields. |
141+
| `poses.initialization_pose` | struct | Initial pose of the vehicle, containing `x`, `y`, `z`, `roll`, `pitch`, and `yaw` fields. Only valid `planning_control` mode. |
142+
| `poses.entity_params` | struct | Parameters for entities (e.g., obstacles), containing `x`, `y`, `z`, `roll`, `pitch`, `yaw`, `x_dimension`, `y_dimension`, and `z_dimension`. |
143+
| `poses.goal_pose` | struct | Goal pose of the vehicle, containing `x`, `y`, `z`, `roll`, `pitch`, and `yaw` fields. |
137144
| `topic_publisher.path_bag_without_object` | string | Path to the ROS bag file without objects. Only valid `perception_planning` mode. |
138145
| `topic_publisher.path_bag_with_object` | string | Path to the ROS bag file with objects. Only valid `perception_planning` mode. |
139146
| `topic_publisher.pointcloud_publisher.pointcloud_publisher_type` | string | Defines how the PointCloud2 messages are going to be published. Modes explained above. |

tools/reaction_analyzer/include/reaction_analyzer_node.hpp

+1-24
Original file line numberDiff line numberDiff line change
@@ -72,29 +72,6 @@ enum class RunningMode {
7272
PlanningControl = 1,
7373
};
7474

75-
struct PoseParams
76-
{
77-
double x;
78-
double y;
79-
double z;
80-
double roll;
81-
double pitch;
82-
double yaw;
83-
};
84-
85-
struct EntityParams
86-
{
87-
double x;
88-
double y;
89-
double z;
90-
double roll;
91-
double pitch;
92-
double yaw;
93-
double x_l;
94-
double y_l;
95-
double z_l;
96-
};
97-
9875
struct NodeParams
9976
{
10077
std::string running_mode;
@@ -194,7 +171,7 @@ class ReactionAnalyzerNode : public rclcpp::Node
194171
rclcpp::Client<ChangeOperationMode>::SharedPtr client_change_to_autonomous_;
195172

196173
// Pointers
197-
std::shared_ptr<topic_publisher::TopicPublisher> topic_publisher_ptr_;
174+
std::unique_ptr<topic_publisher::TopicPublisher> topic_publisher_ptr_;
198175
std::unique_ptr<subscriber::SubscriberBase> subscriber_ptr_;
199176
PointCloud2::SharedPtr entity_pointcloud_ptr_;
200177
PredictedObjects::SharedPtr predicted_objects_ptr_;

tools/reaction_analyzer/include/subscriber.hpp

+4
Original file line numberDiff line numberDiff line change
@@ -162,6 +162,10 @@ class SubscriberBase
162162
rclcpp::Node * node, Odometry::ConstSharedPtr & odometry, geometry_msgs::msg::Pose entity_pose,
163163
std::atomic<bool> & spawn_object_cmd);
164164

165+
// Instances of SubscriberBase cannot be copied
166+
SubscriberBase(const SubscriberBase &) = delete;
167+
SubscriberBase & operator=(const SubscriberBase &) = delete;
168+
165169
~SubscriberBase() = default;
166170

167171
std::optional<std::unordered_map<std::string, BufferVariant>> getMessageBuffersMap();

tools/reaction_analyzer/include/topic_publisher.hpp

+7-6
Original file line numberDiff line numberDiff line change
@@ -95,7 +95,7 @@ enum class PointcloudPublisherType {
9595
template <typename MessageType>
9696
struct PublisherVariables
9797
{
98-
double period_ns{0.0};
98+
std::chrono::milliseconds period_ms{0};
9999
typename MessageType::SharedPtr empty_area_message;
100100
typename MessageType::SharedPtr object_spawned_message;
101101
typename rclcpp::Publisher<MessageType>::SharedPtr publisher;
@@ -150,15 +150,15 @@ struct PublisherVarAccessor
150150
}
151151

152152
template <typename T>
153-
void set_period(T & publisherVar, double newPeriod)
153+
void set_period(T & publisherVar, std::chrono::milliseconds new_period)
154154
{
155-
publisherVar.period_ns = newPeriod;
155+
publisherVar.period_ms = new_period;
156156
}
157157

158158
template <typename T>
159-
double get_period(const T & publisherVar) const
159+
std::chrono::milliseconds get_period(const T & publisherVar) const
160160
{
161-
return publisherVar.period_ns;
161+
return publisherVar.period_ms;
162162
}
163163

164164
template <typename T>
@@ -207,7 +207,8 @@ class TopicPublisher
207207
// Initialized variables
208208
rclcpp::Node * node_;
209209
std::atomic<bool> & spawn_object_cmd_;
210-
std::optional<rclcpp::Time> & spawn_cmd_time_;
210+
std::optional<rclcpp::Time> & spawn_cmd_time_; // Set by a publisher function when the
211+
// spawn_object_cmd_ is true
211212

212213
// Parameters
213214
TopicPublisherParams topic_publisher_params_;

tools/reaction_analyzer/include/utils.hpp

+26
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,32 @@ namespace reaction_analyzer
3131
using autoware_auto_planning_msgs::msg::Trajectory;
3232
using autoware_auto_planning_msgs::msg::TrajectoryPoint;
3333

34+
/**
35+
* @brief A struct containing the parameters of a pose.
36+
*/
37+
struct PoseParams
38+
{
39+
double x;
40+
double y;
41+
double z;
42+
double roll;
43+
double pitch;
44+
double yaw;
45+
};
46+
47+
struct EntityParams
48+
{
49+
double x;
50+
double y;
51+
double z;
52+
double roll;
53+
double pitch;
54+
double yaw;
55+
double x_l;
56+
double y_l;
57+
double z_l;
58+
};
59+
3460
/**
3561
* @brief Sorts the test results by their median value.
3662
*

tools/reaction_analyzer/param/reaction_analyzer.param.yaml

+25-24
Original file line numberDiff line numberDiff line change
@@ -8,30 +8,31 @@
88
spawn_distance_threshold: 15.0 # m # for planning_control mode
99
spawned_pointcloud_sampling_distance: 0.1
1010
dummy_perception_publisher_period: 0.1 # s
11-
initialization_pose:
12-
x: 81247.9609375
13-
y: 49828.87890625
14-
z: 0.0
15-
roll: 0.0
16-
pitch: 0.0
17-
yaw: 35.5
18-
entity_params:
19-
x: 81392.97671487389
20-
y: 49927.361443601316
21-
z: 42.13605499267578
22-
roll: 0.2731273
23-
pitch: -0.6873504
24-
yaw: 33.7664809
25-
x_dimension: 4.118675972722859
26-
y_dimension: 1.7809072588403219
27-
z_dimension: 0.8328610206872963
28-
goal_pose:
29-
x: 81824.90625
30-
y: 50069.34375
31-
z: 0.0
32-
roll: 0.0
33-
pitch: 0.0
34-
yaw: 8.9305903
11+
poses:
12+
initialization_pose:
13+
x: 81247.9609375
14+
y: 49828.87890625
15+
z: 0.0
16+
roll: 0.0
17+
pitch: 0.0
18+
yaw: 35.5
19+
entity_params:
20+
x: 81392.97671487389
21+
y: 49927.361443601316
22+
z: 42.13605499267578
23+
roll: 0.2731273
24+
pitch: -0.6873504
25+
yaw: 33.7664809
26+
x_dimension: 4.118675972722859
27+
y_dimension: 1.7809072588403219
28+
z_dimension: 0.8328610206872963
29+
goal_pose:
30+
x: 81824.90625
31+
y: 50069.34375
32+
z: 0.0
33+
roll: 0.0
34+
pitch: 0.0
35+
yaw: 8.9305903
3536
topic_publisher:
3637
path_bag_without_object: <PATH_TO_YOUR_BAGS>/lexus_bag_without_car/rosbag2_2024_01_30-13_50_45_0.db3
3738
path_bag_with_object: <PATH_TO_YOUR_BAGS>/lexus_bag_with_car/rosbag2_2024_01_30-13_50_17_0.db3

tools/reaction_analyzer/src/reaction_analyzer_node.cpp

+28-24
Original file line numberDiff line numberDiff line change
@@ -80,29 +80,29 @@ ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions options)
8080
get_parameter("dummy_perception_publisher_period").as_double();
8181

8282
// Position parameters
83-
node_params_.initial_pose.x = get_parameter("initialization_pose.x").as_double();
84-
node_params_.initial_pose.y = get_parameter("initialization_pose.y").as_double();
85-
node_params_.initial_pose.z = get_parameter("initialization_pose.z").as_double();
86-
node_params_.initial_pose.roll = get_parameter("initialization_pose.roll").as_double();
87-
node_params_.initial_pose.pitch = get_parameter("initialization_pose.pitch").as_double();
88-
node_params_.initial_pose.yaw = get_parameter("initialization_pose.yaw").as_double();
89-
90-
node_params_.goal_pose.x = get_parameter("goal_pose.x").as_double();
91-
node_params_.goal_pose.y = get_parameter("goal_pose.y").as_double();
92-
node_params_.goal_pose.z = get_parameter("goal_pose.z").as_double();
93-
node_params_.goal_pose.roll = get_parameter("goal_pose.roll").as_double();
94-
node_params_.goal_pose.pitch = get_parameter("goal_pose.pitch").as_double();
95-
node_params_.goal_pose.yaw = get_parameter("goal_pose.yaw").as_double();
96-
97-
node_params_.entity_params.x = get_parameter("entity_params.x").as_double();
98-
node_params_.entity_params.y = get_parameter("entity_params.y").as_double();
99-
node_params_.entity_params.z = get_parameter("entity_params.z").as_double();
100-
node_params_.entity_params.roll = get_parameter("entity_params.roll").as_double();
101-
node_params_.entity_params.pitch = get_parameter("entity_params.pitch").as_double();
102-
node_params_.entity_params.yaw = get_parameter("entity_params.yaw").as_double();
103-
node_params_.entity_params.x_l = get_parameter("entity_params.x_dimension").as_double();
104-
node_params_.entity_params.y_l = get_parameter("entity_params.y_dimension").as_double();
105-
node_params_.entity_params.z_l = get_parameter("entity_params.z_dimension").as_double();
83+
node_params_.initial_pose.x = get_parameter("poses.initialization_pose.x").as_double();
84+
node_params_.initial_pose.y = get_parameter("poses.initialization_pose.y").as_double();
85+
node_params_.initial_pose.z = get_parameter("poses.initialization_pose.z").as_double();
86+
node_params_.initial_pose.roll = get_parameter("poses.initialization_pose.roll").as_double();
87+
node_params_.initial_pose.pitch = get_parameter("poses.initialization_pose.pitch").as_double();
88+
node_params_.initial_pose.yaw = get_parameter("poses.initialization_pose.yaw").as_double();
89+
90+
node_params_.goal_pose.x = get_parameter("poses.goal_pose.x").as_double();
91+
node_params_.goal_pose.y = get_parameter("poses.goal_pose.y").as_double();
92+
node_params_.goal_pose.z = get_parameter("poses.goal_pose.z").as_double();
93+
node_params_.goal_pose.roll = get_parameter("poses.goal_pose.roll").as_double();
94+
node_params_.goal_pose.pitch = get_parameter("poses.goal_pose.pitch").as_double();
95+
node_params_.goal_pose.yaw = get_parameter("poses.goal_pose.yaw").as_double();
96+
97+
node_params_.entity_params.x = get_parameter("poses.entity_params.x").as_double();
98+
node_params_.entity_params.y = get_parameter("poses.entity_params.y").as_double();
99+
node_params_.entity_params.z = get_parameter("poses.entity_params.z").as_double();
100+
node_params_.entity_params.roll = get_parameter("poses.entity_params.roll").as_double();
101+
node_params_.entity_params.pitch = get_parameter("poses.entity_params.pitch").as_double();
102+
node_params_.entity_params.yaw = get_parameter("poses.entity_params.yaw").as_double();
103+
node_params_.entity_params.x_l = get_parameter("poses.entity_params.x_dimension").as_double();
104+
node_params_.entity_params.y_l = get_parameter("poses.entity_params.y_dimension").as_double();
105+
node_params_.entity_params.z_l = get_parameter("poses.entity_params.z_dimension").as_double();
106106

107107
sub_kinematics_ = create_subscription<Odometry>(
108108
"input/kinematics", 1, std::bind(&ReactionAnalyzerNode::on_vehicle_pose, this, _1),
@@ -143,7 +143,7 @@ ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions options)
143143
} else if (node_running_mode_ == RunningMode::PerceptionPlanning) {
144144
// Create topic publishers
145145
topic_publisher_ptr_ =
146-
std::make_shared<topic_publisher::TopicPublisher>(this, spawn_object_cmd_, spawn_cmd_time_);
146+
std::make_unique<topic_publisher::TopicPublisher>(this, spawn_object_cmd_, spawn_cmd_time_);
147147

148148
// Subscribe to the ground truth position
149149
sub_ground_truth_pose_ = create_subscription<PoseStamped>(
@@ -183,15 +183,19 @@ void ReactionAnalyzerNode::on_timer()
183183
return;
184184
}
185185

186+
// Spawn the obstacle if the conditions are met
186187
spawn_obstacle(current_odometry_ptr->pose.pose.position);
187188

189+
// If the spawn_cmd_time is not set by pointcloud publishers, don't do anything
188190
if (!spawn_cmd_time) return;
189191

192+
// Get the reacted messages, if all of the modules reacted
190193
const auto message_buffers = subscriber_ptr_->getMessageBuffersMap();
191194

192195
if (message_buffers) {
193196
// if reacted, calculate the results
194197
calculate_results(message_buffers.value(), spawn_cmd_time.value());
198+
// reset the variables if the iteration is not finished, otherwise write the results
195199
reset();
196200
}
197201
}

tools/reaction_analyzer/src/topic_publisher.cpp

+5-4
Original file line numberDiff line numberDiff line change
@@ -771,14 +771,15 @@ void TopicPublisher::set_period_to_variable_map(
771771
total_time_diff_ns += (timestamps_tmp[i] - timestamps_tmp[i - 1]).nanoseconds();
772772
}
773773

774-
// Convert to double for the division to get the average period in nanoseconds
775-
double period_ns =
776-
static_cast<double>(total_time_diff_ns) / static_cast<double>(timestamps_tmp.size() - 1);
774+
// Conversion to std::chrono::milliseconds
775+
auto total_duration_ns = std::chrono::nanoseconds(total_time_diff_ns);
776+
auto period_ms = std::chrono::duration_cast<std::chrono::milliseconds>(total_duration_ns) /
777+
(timestamps_tmp.size() - 1);
777778

778779
PublisherVariablesVariant & publisherVar = topic_publisher_map_[topic_name];
779780
PublisherVarAccessor accessor;
780781

781-
std::visit([&](auto & var) { accessor.set_period(var, period_ns); }, publisherVar);
782+
std::visit([&](auto & var) { accessor.set_period(var, period_ms); }, publisherVar);
782783
}
783784
}
784785
}

tools/reaction_analyzer/src/utils.cpp

+3-8
Original file line numberDiff line numberDiff line change
@@ -35,14 +35,9 @@ std::vector<std::tuple<std::string, std::vector<double>, double>> sort_results_b
3535
auto vec = pair.second;
3636

3737
// Calculate the median
38-
std::sort(vec.begin(), vec.end());
39-
double median = 0.0;
40-
size_t size = vec.size();
41-
if (size % 2 == 0) {
42-
median = (vec[size / 2 - 1] + vec[size / 2]) / 2.0;
43-
} else {
44-
median = vec[size / 2];
45-
}
38+
const size_t mid_index = vec.size() / 2;
39+
std::nth_element(vec.begin(), vec.begin() + mid_index, vec.end());
40+
const double median = vec[mid_index];
4641

4742
sorted_data.emplace_back(pair.first, pair.second, median);
4843
}

0 commit comments

Comments
 (0)