Skip to content

Commit bf5ae77

Browse files
brkay54brkay54
committed
feat: improve style, change csv output stringstream.
Signed-off-by: Berkay Karaman <brkay54@gmail.com>
1 parent 4ad2aef commit bf5ae77

10 files changed

+902
-692
lines changed

‎tools/reaction_analyzer/README.md

+32-26
Original file line numberDiff line numberDiff line change
@@ -126,23 +126,30 @@ vehicle should be same.**
126126
**After you record the rosbags, you can set the `path_bag_without_object` and `path_bag_with_object` parameters with the
127127
paths of the recorded rosbags.**
128128

129+
## Results
130+
131+
The results will be stored in the `csv` file format and written to the `output_file_path` you defined. It shows each
132+
pipeline of the Autoware by using header timestamp of the messages, and it reports `Node Latency`, and `Total Latency`
133+
for each of the nodes.
134+
- `Node Latency`: The time difference between previous and current node's reaction timestamps.
135+
- `Total Latency`: The time difference between the message's published timestamp and the spawn obstacle command sent timestamp.
136+
129137
## Parameters
130138

131139
| Name | Type | Description |
132-
| ---------------------------------------------------------------------------- | ------ | --------------------------------------------------------------------------------------------------------------------------------------------- |
140+
|------------------------------------------------------------------------------|--------|-----------------------------------------------------------------------------------------------------------------------------------------------|
133141
| `timer_period` | double | [s] Period for the main processing timer. |
134142
| `test_iteration` | int | Number of iterations for the test. |
135143
| `output_file_path` | string | Directory path where test results and statistics will be stored. |
136-
| `object_search_radius_offset` | double | [m] Additional radius added to the search area when looking for objects. |
137144
| `spawn_time_after_init` | double | [s] Time delay after initialization before spawning objects. Only valid `perception_planning` mode. |
138145
| `spawn_distance_threshold` | double | [m] Distance threshold for spawning objects. Only valid `planning_control` mode. |
139-
| `spawned_pointcloud_sampling_distance` | double | [m] Sampling distance for point clouds of spawned objects. Only valid `planning_control` mode. |
140-
| `dummy_perception_publisher_period` | double | [s] Publishing period for the dummy perception data. Only valid `planning_control` mode. |
141146
| `poses.initialization_pose` | struct | Initial pose of the vehicle, containing `x`, `y`, `z`, `roll`, `pitch`, and `yaw` fields. Only valid `planning_control` mode. |
142147
| `poses.entity_params` | struct | Parameters for entities (e.g., obstacles), containing `x`, `y`, `z`, `roll`, `pitch`, `yaw`, `x_dimension`, `y_dimension`, and `z_dimension`. |
143148
| `poses.goal_pose` | struct | Goal pose of the vehicle, containing `x`, `y`, `z`, `roll`, `pitch`, and `yaw` fields. |
144149
| `topic_publisher.path_bag_without_object` | string | Path to the ROS bag file without objects. Only valid `perception_planning` mode. |
145150
| `topic_publisher.path_bag_with_object` | string | Path to the ROS bag file with objects. Only valid `perception_planning` mode. |
151+
| `topic_publisher.spawned_pointcloud_sampling_distance` | double | [m] Sampling distance for point clouds of spawned objects. Only valid `planning_control` mode. |
152+
| `topic_publisher.dummy_perception_publisher_period` | double | [s] Publishing period for the dummy perception data. Only valid `planning_control` mode. |
146153
| `topic_publisher.pointcloud_publisher.pointcloud_publisher_type` | string | Defines how the PointCloud2 messages are going to be published. Modes explained above. |
147154
| `topic_publisher.pointcloud_publisher.pointcloud_publisher_period` | double | [s] Publishing period of the PointCloud2 messages. |
148155
| `topic_publisher.pointcloud_publisher.publish_only_pointcloud_with_object` | bool | Default false. Publish only the point cloud messages with the object. |
@@ -161,35 +168,34 @@ paths of the recorded rosbags.**
161168

162169
- **Publisher Message Types:**
163170

164-
- `sensor_msgs/msg/PointCloud2`
165-
- `sensor_msgs/msg/CameraInfo`
166-
- `sensor_msgs/msg/Image`
167-
- `geometry_msgs/msg/PoseWithCovarianceStamped`
168-
- `sensor_msgs/msg/Imu`
169-
- `autoware_auto_vehicle_msgs/msg/ControlModeReport`
170-
- `autoware_auto_vehicle_msgs/msg/GearReport`
171-
- `autoware_auto_vehicle_msgs/msg/HazardLightsReport`
172-
- `autoware_auto_vehicle_msgs/msg/SteeringReport`
173-
- `autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport`
174-
- `autoware_auto_vehicle_msgs/msg/VelocityReport`
171+
- `sensor_msgs/msg/PointCloud2`
172+
- `sensor_msgs/msg/CameraInfo`
173+
- `sensor_msgs/msg/Image`
174+
- `geometry_msgs/msg/PoseWithCovarianceStamped`
175+
- `sensor_msgs/msg/Imu`
176+
- `autoware_auto_vehicle_msgs/msg/ControlModeReport`
177+
- `autoware_auto_vehicle_msgs/msg/GearReport`
178+
- `autoware_auto_vehicle_msgs/msg/HazardLightsReport`
179+
- `autoware_auto_vehicle_msgs/msg/SteeringReport`
180+
- `autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport`
181+
- `autoware_auto_vehicle_msgs/msg/VelocityReport`
175182

176183
- **Subscriber Message Types:**
177184

178-
- `sensor_msgs/msg/PointCloud2`
179-
- `autoware_auto_perception_msgs/msg/DetectedObjects`
180-
- `autoware_auto_perception_msgs/msg/TrackedObjects`
181-
- `autoware_auto_msgs/msg/PredictedObject`
182-
- `autoware_auto_planning_msgs/msg/Trajectory`
183-
- `autoware_auto_control_msgs/msg/AckermannControlCommand`
185+
- `sensor_msgs/msg/PointCloud2`
186+
- `autoware_auto_perception_msgs/msg/DetectedObjects`
187+
- `autoware_auto_perception_msgs/msg/TrackedObjects`
188+
- `autoware_auto_msgs/msg/PredictedObject`
189+
- `autoware_auto_planning_msgs/msg/Trajectory`
190+
- `autoware_auto_control_msgs/msg/AckermannControlCommand`
184191

185192
- **Reaction Types:**
186-
- `FIRST_BRAKE`
187-
- `SEARCH_ZERO_VEL`
188-
- `SEARCH_ENTITY`
193+
- `FIRST_BRAKE`
194+
- `SEARCH_ZERO_VEL`
195+
- `SEARCH_ENTITY`
189196

190197
## Future improvements
191198

192199
- The reaction analyzer can be improved by adding more reaction types. Currently, it is supporting only `FIRST_BRAKE`,
193200
`SEARCH_ZERO_VEL`, and `SEARCH_ENTITY` reaction types. It can be extended by adding more reaction types for each of
194-
the
195-
message types.
201+
the message types.

‎tools/reaction_analyzer/include/reaction_analyzer_node.hpp

+6-43
Original file line numberDiff line numberDiff line change
@@ -15,15 +15,8 @@
1515
#ifndef REACTION_ANALYZER_NODE_HPP_
1616
#define REACTION_ANALYZER_NODE_HPP_
1717

18-
#include "tf2/transform_datatypes.h"
19-
20-
#include <pcl/impl/point_types.hpp>
21-
#include <pcl_ros/transforms.hpp>
2218
#include <rclcpp/rclcpp.hpp>
2319
#include <subscriber.hpp>
24-
#include <tf2_eigen/tf2_eigen/tf2_eigen.hpp>
25-
#include <tier4_autoware_utils/geometry/geometry.hpp>
26-
#include <tier4_autoware_utils/math/unit_conversion.hpp>
2720
#include <topic_publisher.hpp>
2821
#include <utils.hpp>
2922

@@ -34,17 +27,7 @@
3427
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
3528
#include <nav_msgs/msg/odometry.hpp>
3629

37-
#include <boost/uuid/string_generator.hpp>
38-
#include <boost/uuid/uuid.hpp>
39-
#include <boost/uuid/uuid_generators.hpp>
40-
#include <boost/uuid/uuid_io.hpp>
41-
42-
#include <pcl/common/distances.h>
43-
#include <pcl/point_types.h>
44-
#include <pcl_conversions/pcl_conversions.h>
45-
#include <tf2_ros/buffer.h>
46-
#include <tf2_ros/transform_listener.h>
47-
30+
#include <algorithm>
4831
#include <memory>
4932
#include <mutex>
5033
#include <string>
@@ -61,17 +44,12 @@ using autoware_adapi_v1_msgs::msg::RouteState;
6144
using autoware_adapi_v1_msgs::srv::ChangeOperationMode;
6245
using autoware_auto_perception_msgs::msg::PredictedObject;
6346
using autoware_auto_perception_msgs::msg::PredictedObjects;
47+
using autoware_internal_msgs::msg::PublishedTime;
6448
using geometry_msgs::msg::Pose;
6549
using geometry_msgs::msg::PoseStamped;
6650
using nav_msgs::msg::Odometry;
6751
using sensor_msgs::msg::PointCloud2;
6852

69-
// The running mode of the node
70-
enum class RunningMode {
71-
PerceptionPlanning = 0,
72-
PlanningControl = 1,
73-
};
74-
7553
struct NodeParams
7654
{
7755
std::string running_mode;
@@ -80,8 +58,6 @@ struct NodeParams
8058
size_t test_iteration;
8159
double spawn_time_after_init;
8260
double spawn_distance_threshold;
83-
double spawned_pointcloud_sampling_distance;
84-
double dummy_perception_publisher_period;
8561
PoseParams initial_pose;
8662
PoseParams goal_pose;
8763
EntityParams entity_params;
@@ -112,33 +88,25 @@ class ReactionAnalyzerNode : public rclcpp::Node
11288
rclcpp::Subscription<RouteState>::SharedPtr sub_route_state_;
11389
rclcpp::Subscription<LocalizationInitializationState>::SharedPtr sub_localization_init_state_;
11490
rclcpp::Subscription<OperationModeState>::SharedPtr sub_operation_mode_;
115-
rclcpp::Subscription<PoseStamped>::SharedPtr sub_ground_truth_pose_;
91+
rclcpp::Subscription<PoseStamped>::SharedPtr
92+
sub_ground_truth_pose_; // Only for perception_planning mode
11693

11794
// Publishers
118-
rclcpp::Publisher<PointCloud2>::SharedPtr pub_pointcloud_;
119-
rclcpp::Publisher<PredictedObjects>::SharedPtr pub_predicted_objects_;
12095
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pub_initial_pose_;
12196
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub_goal_pose_;
12297

123-
// tf
124-
tf2_ros::Buffer tf_buffer_{get_clock()};
125-
tf2_ros::TransformListener tf_listener_{tf_buffer_};
126-
12798
// Variables
128-
std::unordered_map<std::string, std::vector<double>> test_results_;
99+
std::vector<PipelineMap> pipeline_map_vector_;
129100
std::optional<rclcpp::Time> last_test_environment_init_request_time_;
130101
std::optional<rclcpp::Time> test_environment_init_time_;
131102
std::optional<rclcpp::Time> spawn_cmd_time_;
132103
std::atomic<bool> spawn_object_cmd_{false};
133-
std::atomic<bool> is_object_spawned_message_published_{false};
134104
bool is_vehicle_initialized_{false};
135105
bool is_route_set_{false};
136106
size_t test_iteration_count_{0};
137107

138108
// Functions
139109
void init_analyzer_variables();
140-
void init_pointcloud();
141-
void init_predicted_objects();
142110
void init_ego(
143111
const LocalizationInitializationState::ConstSharedPtr & initialization_state_ptr,
144112
const RouteState::ConstSharedPtr & route_state_ptr,
@@ -148,12 +116,10 @@ class ReactionAnalyzerNode : public rclcpp::Node
148116
void call_operation_mode_service_without_response();
149117
void spawn_obstacle(const geometry_msgs::msg::Point & ego_pose);
150118
void calculate_results(
151-
const std::unordered_map<std::string, subscriber::BufferVariant> & message_buffers,
119+
const std::unordered_map<std::string, subscriber::MessageBufferVariant> & message_buffers,
152120
const rclcpp::Time & spawn_cmd_time);
153121
void on_timer();
154-
void dummy_perception_publisher();
155122
void reset();
156-
void write_results();
157123

158124
// Callbacks
159125
void on_vehicle_pose(Odometry::ConstSharedPtr msg_ptr);
@@ -165,16 +131,13 @@ class ReactionAnalyzerNode : public rclcpp::Node
165131

166132
// Timer
167133
rclcpp::TimerBase::SharedPtr timer_;
168-
rclcpp::TimerBase::SharedPtr dummy_perception_timer_;
169134

170135
// Client
171136
rclcpp::Client<ChangeOperationMode>::SharedPtr client_change_to_autonomous_;
172137

173138
// Pointers
174139
std::unique_ptr<topic_publisher::TopicPublisher> topic_publisher_ptr_;
175140
std::unique_ptr<subscriber::SubscriberBase> subscriber_ptr_;
176-
PointCloud2::SharedPtr entity_pointcloud_ptr_;
177-
PredictedObjects::SharedPtr predicted_objects_ptr_;
178141
LocalizationInitializationState::ConstSharedPtr initialization_state_ptr_;
179142
RouteState::ConstSharedPtr current_route_state_ptr_;
180143
OperationModeState::ConstSharedPtr operation_mode_ptr_;

‎tools/reaction_analyzer/include/subscriber.hpp

+16-38
Original file line numberDiff line numberDiff line change
@@ -15,31 +15,17 @@
1515
#ifndef SUBSCRIBER_HPP_
1616
#define SUBSCRIBER_HPP_
1717
#include <motion_utils/trajectory/trajectory.hpp>
18-
#include <pcl/impl/point_types.hpp>
19-
#include <pcl_ros/transforms.hpp>
2018
#include <rclcpp/rclcpp.hpp>
2119
#include <tf2_eigen/tf2_eigen/tf2_eigen.hpp>
22-
#include <tier4_autoware_utils/geometry/geometry.hpp>
23-
#include <tier4_autoware_utils/math/unit_conversion.hpp>
2420
#include <utils.hpp>
2521

26-
#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
27-
#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
28-
#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
29-
#include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
30-
#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
31-
#include <autoware_internal_msgs/msg/published_time.hpp>
3222
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
3323
#include <nav_msgs/msg/odometry.hpp>
34-
#include <sensor_msgs/msg/point_cloud2.hpp>
3524

3625
#include <message_filters/cache.h>
3726
#include <message_filters/subscriber.h>
3827
#include <message_filters/sync_policies/exact_time.h>
3928
#include <message_filters/synchronizer.h>
40-
#include <pcl/common/distances.h>
41-
#include <pcl/point_types.h>
42-
#include <pcl_conversions/pcl_conversions.h>
4329
#include <tf2_ros/buffer.h>
4430
#include <tf2_ros/transform_listener.h>
4531

@@ -66,19 +52,12 @@ using geometry_msgs::msg::Pose;
6652
using nav_msgs::msg::Odometry;
6753
using sensor_msgs::msg::PointCloud2;
6854

69-
// Buffers to be used to store subscribed messages
70-
using ControlCommandBuffer =
71-
std::pair<std::vector<AckermannControlCommand>, std::optional<AckermannControlCommand>>;
72-
using TrajectoryBuffer = std::optional<Trajectory>;
73-
using PointCloud2Buffer = std::optional<PointCloud2>;
74-
using PredictedObjectsBuffer = std::optional<PredictedObjects>;
75-
using DetectedObjectsBuffer = std::optional<DetectedObjects>;
76-
using TrackedObjectsBuffer = std::optional<TrackedObjects>;
77-
55+
// Buffers to be used to store subscribed messages' data, pipeline Header, and PublishedTime
56+
using MessageBuffer = std::optional<PublishedTime>;
57+
// We need to store the past AckermannControlCommands to analyze the first brake
58+
using ControlCommandBuffer = std::pair<std::vector<AckermannControlCommand>, MessageBuffer>;
7859
// Variant to store different types of buffers
79-
using BufferVariant = std::variant<
80-
ControlCommandBuffer, TrajectoryBuffer, PointCloud2Buffer, PredictedObjectsBuffer,
81-
DetectedObjectsBuffer, TrackedObjectsBuffer>;
60+
using MessageBufferVariant = std::variant<ControlCommandBuffer, MessageBuffer>;
8261

8362
template <typename MessageType>
8463
struct SubscriberVariables
@@ -142,7 +121,7 @@ struct SearchZeroVelParams
142121

143122
struct SearchEntityParams
144123
{
145-
double search_radius;
124+
double search_radius_offset;
146125
};
147126

148127
// Place for the store the reaction parameter configuration (currently only for first brake)
@@ -159,33 +138,36 @@ class SubscriberBase
159138
{
160139
public:
161140
explicit SubscriberBase(
162-
rclcpp::Node * node, Odometry::ConstSharedPtr & odometry, geometry_msgs::msg::Pose entity_pose,
163-
std::atomic<bool> & spawn_object_cmd);
141+
rclcpp::Node * node, Odometry::ConstSharedPtr & odometry, std::atomic<bool> & spawn_object_cmd,
142+
const EntityParams & entity_params);
164143

165144
// Instances of SubscriberBase cannot be copied
166145
SubscriberBase(const SubscriberBase &) = delete;
167146
SubscriberBase & operator=(const SubscriberBase &) = delete;
168147

169148
~SubscriberBase() = default;
170149

171-
std::optional<std::unordered_map<std::string, BufferVariant>> getMessageBuffersMap();
150+
std::optional<std::unordered_map<std::string, MessageBufferVariant>> getMessageBuffersMap();
172151
void reset();
173152

174153
private:
175154
std::mutex mutex_;
176155

156+
// Init
177157
rclcpp::Node * node_;
178158
Odometry::ConstSharedPtr odometry_;
179-
geometry_msgs::msg::Pose entity_pose_;
180159
std::atomic<bool> & spawn_object_cmd_;
160+
EntityParams entity_params_;
181161

182162
// Variables to be initialized in constructor
183163
ChainModules chain_modules_;
184164
ReactionParams reaction_params_{};
165+
geometry_msgs::msg::Pose entity_pose_;
166+
double entity_search_radius_;
185167

186168
// Variants
187169
std::unordered_map<std::string, SubscriberVariablesVariant> subscriber_variables_map_;
188-
std::unordered_map<std::string, BufferVariant> message_buffers_;
170+
std::unordered_map<std::string, MessageBufferVariant> message_buffers_;
189171

190172
// tf
191173
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
@@ -194,14 +176,10 @@ class SubscriberBase
194176
// Functions
195177
void init_reaction_chains_and_params();
196178
bool init_subscribers();
197-
bool search_pointcloud_near_entity(const pcl::PointCloud<pcl::PointXYZ> & pcl_pointcloud);
198-
bool search_predicted_objects_near_entity(const PredictedObjects & predicted_objects);
199-
bool search_detected_objects_near_entity(const DetectedObjects & detected_objects);
200-
bool search_tracked_objects_near_entity(const TrackedObjects & tracked_objects);
201-
void set_control_command_to_buffer(
202-
std::vector<AckermannControlCommand> & buffer, const AckermannControlCommand & cmd);
203179
std::optional<size_t> find_first_brake_idx(
204180
const std::vector<AckermannControlCommand> & cmd_array);
181+
void set_control_command_to_buffer(
182+
std::vector<AckermannControlCommand> & buffer, const AckermannControlCommand & cmd);
205183

206184
// Callbacks for modules are subscribed
207185
void on_control_command(

0 commit comments

Comments
 (0)