Skip to content

Commit

Permalink
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
feat: update style and readme
Browse files Browse the repository at this point in the history 
Signed-off-by: Berkay Karaman <brkay54@gmail.com>
@brkay54
brkay54 committed Mar 5, 2024
1 parent 3c480f1 commit 70dcaf6
Showing 6 changed files with 164 additions and 118 deletions.
115 changes: 82 additions & 33 deletions tools/reaction_analyzer/README.md
View file
Original file line number Diff line number Diff line change
@@ -55,60 +55,69 @@ pre-defined topics you want to measure their reaction times.

### Prepared Test Environment

Firstly, you need to download the demonstration test map from the
link [here](https://github.com/tier4/AWSIM/releases/download/v1.1.0/nishishinjuku_autoware_map.zip). After downloading,
extract the zip file and use its path as `[MAP_PATH]` in the following commands.
- Download the demonstration test map from the
link [here](https://github.com/tier4/AWSIM/releases/download/v1.1.0/nishishinjuku_autoware_map.zip). After
downloading,
extract the zip file and use its path as `[MAP_PATH]` in the following commands.

#### Planning Control Mode

Firstly, you need to define only Planning and Control nodes in the `reaction_chain` list. With the default parameters,
you can start to test with the following command:
- You need to define only Planning and Control nodes in the `reaction_chain` list. With the default parameters,
you can start to test with the following command:

```bash
ros2 launch reaction_analyzer reaction_analyzer.launch.xml running_mode:=planning_control vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit map_path:=[MAP_PATH]
```

After the command, the simple_planning_simulator and the reaction_analyzer will be launched. It will automatically start
to test. After the test is completed, the results will be stored in the `output_file_path` you defined.
After the command, the `simple_planning_simulator` and the `reaction_analyzer` will be launched. It will automatically
start to test. After the test is completed, the results will be stored in the `output_file_path` you defined.

#### Perception Planning Mode

Firstly, you need to download the rosbag files from the
link [here](https://drive.google.com/file/d/1_P-3oy_M6eJ7fk8h5CP8V6s6da6pPrBu/view?usp=sharing). After downloading,
extract the zip file and set the path of the `.db3` files to parameters `path_bag_without_object`
and `path_bag_with_object`. Because custom sensor setup, you need to checkout the following branches before launch the
reaction analyzer: For the `autoware_individual_params` repository, checkout the
branch [here](https://github.com/brkay54/autoware_individual_params/tree/bk/reaction-analyzer-config). For
the `awsim_sensor_kit_launch` repository, checkout the
branch [here](https://github.com/brkay54/awsim_sensor_kit_launch/tree/bk/reaction-analyzer-config).
After you checkouted the branches, you can start to test with the following command:
- Download the rosbag files from the Google Drive
link [here](https://drive.google.com/file/d/1_P-3oy_M6eJ7fk8h5CP8V6s6da6pPrBu/view?usp=sharing).
- Extract the zip file and set the path of the `.db3` files to parameters `path_bag_without_object`
and `path_bag_with_object`.
- Because custom sensor setup, you need to checkout the following branches before launch the
reaction analyzer: For the `autoware_individual_params` repository, checkout the
branch [here](https://github.com/brkay54/autoware_individual_params/tree/bk/reaction-analyzer-config).
- For the `awsim_sensor_kit_launch` repository, checkout the
branch [here](https://github.com/brkay54/awsim_sensor_kit_launch/tree/bk/reaction-analyzer-config).
- After you checkouted the branches, you can start to test with the following command:

Check warning on line 86 in tools/reaction_analyzer/README.md

GitHub Actions / spell-check-partial 

Unknown word (checkouted)

```bash
ros2 launch reaction_analyzer reaction_analyzer.launch.xml running_mode:=perception_planning vehicle_model:=sample_vehicle sensor_model:=awsim_sensor_kit map_path:=[MAP_PATH]
```

After the command, the e2e_simulator and the reaction_analyzer will be launched. It will automatically start
After the command, the `e2e_simulator` and the `reaction_analyzer` will be launched. It will automatically start
to test. After the test is completed, the results will be stored in the `output_file_path` you defined.

### Custom Test Environment

If you want to run the reaction analyzer with your custom test environment, you need to redefine some of the parameters.
The parameters you need to redefine are `initialization_pose`, `entity_params`, `goal_pose`, and `topic_publisher`
parameters.
For `initialization_pose`, `entity_params`, `goal_pose`, you can upload your `.osm` map file into
the [scenario editor](https://scenario.ci.tier4.jp/scenario_editor/) to define the position of the position parameters.
You can define the positions with respect to you desired root. Firstly, add EGO vehicle from edit/add entity/Ego to map.
Set destination to EGO vehicle and add another dummy object in same way. The dummy object represents the object spawn
suddenly in the reaction analyzer test. After you set up the positions in the map, we should get the positions of these
entities in the map frame. To achieve this, you should convert the positions to map frame by changing Map/Coordinate to
World and Map/Orientation to Euler. 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.

For the `topic_publisher` parameters, you need to record the rosbags from the AWSIM. After opened your AWSIM
**If you want to run the reaction analyzer with your custom test environment, you need to redefine some of the parameters.
The parameters you need to redefine are `initialization_pose`, `entity_params`, `goal_pose`, and `topic_publisher` (
for `perception_planning` mode) parameters.**

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

**After you set up the positions in the map, we should get the positions of these entities in the map frame. To achieve this:**

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

- 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.

**For the `topic_publisher` parameters, you need to record the rosbags from the AWSIM. After opened your AWSIM
environment, you should record two different rosbags. However, the environment should be static and the position of the
vehicle should be same. Firstly, record a rosbag in empty environment. After that, record another rosbag in the same
environment except add an object in front of the vehicle. 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.
vehicle should be same.**

- Record a rosbag in empty environment (without an obstacle in front of the vehicle).
- After that, record another rosbag in the same environment except add an object in front of the vehicle.

**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.**

## Parameters

@@ -136,3 +145,43 @@ the `path_bag_without_object` and `path_bag_with_object` parameters with the pat
| `reaction_params.search_zero_vel_params.max_looking_distance` | double | [m] Maximum looking distance for zero velocity on trajectory |
| `reaction_params.search_entity_params.search_radius` | double | [m] Searching radius for spawned entity. Distance between ego pose and entity pose. |
| `reaction_chain` | struct | List of the nodes with their topics and topic's message types. |

## Limitations

- Reaction analyzer has some limitation like `PublisherMessageType`, `SubscriberMessageType` and `ReactionType`. It is
currently supporting following list:

- **Publisher Message Types:**

- `sensor_msgs/msg/PointCloud2`
- `sensor_msgs/msg/CameraInfo`
- `sensor_msgs/msg/Image`
- `geometry_msgs/msg/PoseWithCovarianceStamped`
- `sensor_msgs/msg/Imu`
- `autoware_auto_vehicle_msgs/msg/ControlModeReport`
- `autoware_auto_vehicle_msgs/msg/GearReport`
- `autoware_auto_vehicle_msgs/msg/HazardLightsReport`
- `autoware_auto_vehicle_msgs/msg/SteeringReport`
- `autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport`
- `autoware_auto_vehicle_msgs/msg/VelocityReport`

- **Subscriber Message Types:**

- `sensor_msgs/msg/PointCloud2`
- `autoware_auto_perception_msgs/msg/DetectedObjects`
- `autoware_auto_perception_msgs/msg/TrackedObjects`
- `autoware_auto_msgs/msg/PredictedObject`
- `autoware_auto_planning_msgs/msg/Trajectory`
- `autoware_auto_control_msgs/msg/AckermannControlCommand`

- **Reaction Types:**
- `FIRST_BRAKE`
- `SEARCH_ZERO_VEL`
- `SEARCH_ENTITY`

## Future improvements

- The reaction analyzer can be improved by adding more reaction types. Currently, it is supporting only `FIRST_BRAKE`,
`SEARCH_ZERO_VEL`, and `SEARCH_ENTITY` reaction types. It can be extended by adding more reaction types for each of
the
message types.
30 changes: 15 additions & 15 deletions tools/reaction_analyzer/include/topic_publisher.hpp
View file
Original file line number Diff line number Diff line change
@@ -54,18 +54,18 @@ using nav_msgs::msg::Odometry;
using sensor_msgs::msg::PointCloud2;

enum class PublisherMessageType {
Unknown = 0,
CameraInfo = 1,
Image = 2,
PointCloud2 = 3,
PoseWithCovarianceStamped = 4,
Imu = 5,
ControlModeReport = 6,
GearReport = 7,
HazardLightsReport = 8,
SteeringReport = 9,
TurnIndicatorsReport = 10,
VelocityReport = 11,
UNKNOWN = 0,
CAMERA_INFO = 1,
IMAGE = 2,
POINTCLOUD2 = 3,
POSE_WITH_COVARIANCE_STAMPED = 4,
IMU = 5,
CONTROL_MODE_REPORT = 6,
GEAR_REPORT = 7,
HAZARD_LIGHTS_REPORT = 8,
STEERING_REPORT = 9,
TURN_INDICATORS_REPORT = 10,
VELOCITY_REPORT = 11,
};

struct TopicPublisherParams
@@ -77,9 +77,9 @@ struct TopicPublisherParams
};

enum class PointcloudPublisherType {
AsyncPublisher = 0, // Asynchronous publisher
SyncHeaderSyncPublisher = 1, // Synchronous publisher with header synchronization
AsyncHeaderSyncPublisher = 2, // Asynchronous publisher with header synchronization
ASYNC_PUBLISHER = 0, // Asynchronous publisher
SYNC_HEADER_SYNC_PUBLISHER = 1, // Synchronous publisher with header synchronization
ASYNC_HEADER_SYNC_PUBLISHER = 2, // Asynchronous publisher with header synchronization
};

/**
32 changes: 16 additions & 16 deletions tools/reaction_analyzer/param/reaction_analyzer.param.yaml
View file
Original file line number Diff line number Diff line change
@@ -53,64 +53,64 @@
obstacle_stop_planner:
topic_name: /planning/scenario_planning/lane_driving/trajectory
time_debug_topic_name: /planning/scenario_planning/lane_driving/trajectory/debug/published_time
message_type: autoware_auto_planning_msgs::msg::Trajectory
message_type: autoware_auto_planning_msgs/msg/Trajectory
scenario_selector:
topic_name: /planning/scenario_planning/scenario_selector/trajectory
time_debug_topic_name: /planning/scenario_planning/scenario_selector/trajectory/debug/published_time
message_type: autoware_auto_planning_msgs::msg::Trajectory
message_type: autoware_auto_planning_msgs/msg/Trajectory
motion_velocity_smoother:
topic_name: /planning/scenario_planning/motion_velocity_smoother/trajectory
time_debug_topic_name: /planning/scenario_planning/motion_velocity_smoother/trajectory/debug/published_time
message_type: autoware_auto_planning_msgs::msg::Trajectory
message_type: autoware_auto_planning_msgs/msg/Trajectory
planning_validator:
topic_name: /planning/scenario_planning/trajectory
time_debug_topic_name: /planning/scenario_planning/trajectory/debug/published_time
message_type: autoware_auto_planning_msgs::msg::Trajectory
message_type: autoware_auto_planning_msgs/msg/Trajectory
# trajectory_follower:
# topic_name: /control/trajectory_follower/control_cmd
# time_debug_topic_name: /control/trajectory_follower/control_cmd/debug/published_time
# message_type: autoware_auto_control_msgs::msg::AckermannControlCommand
# message_type: autoware_auto_control_msgs/msg/AckermannControlCommand
# vehicle_cmd_gate:
# topic_name: /control/command/control_cmd
# time_debug_topic_name: /control/command/control_cmd/debug/published_time
# message_type: autoware_auto_control_msgs::msg::AckermannControlCommand
# message_type: autoware_auto_control_msgs/msg/AckermannControlCommand
common_ground_filter:
topic_name: /perception/obstacle_segmentation/single_frame/pointcloud_raw
time_debug_topic_name: /perception/obstacle_segmentation/single_frame/pointcloud_raw/debug/published_time
message_type: sensor_msgs::msg::PointCloud2
message_type: sensor_msgs/msg/PointCloud2
occupancy_grid_map_outlier:
topic_name: /perception/obstacle_segmentation/pointcloud
time_debug_topic_name: /perception/obstacle_segmentation/pointcloud/debug/published_time
message_type: sensor_msgs::msg::PointCloud2
message_type: sensor_msgs/msg/PointCloud2
multi_object_tracker:
topic_name: /perception/object_recognition/tracking/near_objects
time_debug_topic_name: /perception/object_recognition/tracking/near_objects/debug/published_time
message_type: autoware_auto_perception_msgs::msg::TrackedObjects
message_type: autoware_auto_perception_msgs/msg/TrackedObjects
lidar_centerpoint:
topic_name: /perception/object_recognition/detection/centerpoint/objects
time_debug_topic_name: /perception/object_recognition/detection/centerpoint/objects/debug/published_time
message_type: autoware_auto_perception_msgs::msg::DetectedObjects
message_type: autoware_auto_perception_msgs/msg/DetectedObjects
obstacle_pointcloud_based_validator:
topic_name: /perception/object_recognition/detection/centerpoint/validation/objects
time_debug_topic_name: /perception/object_recognition/detection/centerpoint/validation/objects/debug/published_time
message_type: autoware_auto_perception_msgs::msg::DetectedObjects
message_type: autoware_auto_perception_msgs/msg/DetectedObjects
decorative_tracker_merger:
topic_name: /perception/object_recognition/tracking/objects
time_debug_topic_name: /perception/object_recognition/tracking/objects/debug/published_time
message_type: autoware_auto_perception_msgs::msg::TrackedObjects
message_type: autoware_auto_perception_msgs/msg/TrackedObjects
detected_object_feature_remover:
topic_name: /perception/object_recognition/detection/clustering/objects
time_debug_topic_name: /perception/object_recognition/detection/clustering/objects/debug/published_time
message_type: autoware_auto_perception_msgs::msg::DetectedObjects
message_type: autoware_auto_perception_msgs/msg/DetectedObjects
detection_by_tracker:
topic_name: /perception/object_recognition/detection/detection_by_tracker/objects
time_debug_topic_name: /perception/object_recognition/detection/detection_by_tracker/objects/debug/published_time
message_type: autoware_auto_perception_msgs::msg::DetectedObjects
message_type: autoware_auto_perception_msgs/msg/DetectedObjects
object_lanelet_filter:
topic_name: /perception/object_recognition/detection/objects
time_debug_topic_name: /perception/object_recognition/detection/objects/debug/published_time
message_type: autoware_auto_perception_msgs::msg::DetectedObjects
message_type: autoware_auto_perception_msgs/msg/DetectedObjects
map_based_prediction:
topic_name: /perception/object_recognition/objects
time_debug_topic_name: /perception/object_recognition/objects/debug/published_time
message_type: autoware_auto_perception_msgs::msg::PredictedObjects
message_type: autoware_auto_perception_msgs/msg/PredictedObjects
2 changes: 0 additions & 2 deletions tools/reaction_analyzer/src/reaction_analyzer_node.cpp
View file
Original file line number Diff line number Diff line change
@@ -162,7 +162,6 @@ void ReactionAnalyzerNode::onTimer()
mutex_.unlock();

// Init the test environment

if (!test_environment_init_time_) {
initEgoForTest(initialization_state_ptr, route_state_ptr, operation_mode_ptr);
return;
@@ -176,7 +175,6 @@ void ReactionAnalyzerNode::onTimer()

if (message_buffers) {
// if reacted, calculate the results

calculateResults(message_buffers.value(), spawn_cmd_time.value());
reset();
}
15 changes: 7 additions & 8 deletions tools/reaction_analyzer/src/subscriber.cpp
View file
Original file line number Diff line number Diff line change
@@ -38,17 +38,17 @@ SubscriberBase::SubscriberBase(
void SubscriberBase::initReactionChainsAndParams()
{
auto stringToMessageType = [](const std::string & input) {
if (input == "autoware_auto_control_msgs::msg::AckermannControlCommand") {
if (input == "autoware_auto_control_msgs/msg/AckermannControlCommand") {
return SubscriberMessageType::ACKERMANN_CONTROL_COMMAND;
} else if (input == "autoware_auto_planning_msgs::msg::Trajectory") {
} else if (input == "autoware_auto_planning_msgs/msg/Trajectory") {
return SubscriberMessageType::TRAJECTORY;
} else if (input == "sensor_msgs::msg::PointCloud2") {
} else if (input == "sensor_msgs/msg/PointCloud2") {
return SubscriberMessageType::POINTCLOUD2;
} else if (input == "autoware_auto_perception_msgs::msg::PredictedObjects") {
} else if (input == "autoware_auto_perception_msgs/msg/PredictedObjects") {
return SubscriberMessageType::PREDICTED_OBJECTS;
} else if (input == "autoware_auto_perception_msgs::msg::DetectedObjects") {
} else if (input == "autoware_auto_perception_msgs/msg/DetectedObjects") {
return SubscriberMessageType::DETECTED_OBJECTS;
} else if (input == "autoware_auto_perception_msgs::msg::TrackedObjects") {
} else if (input == "autoware_auto_perception_msgs/msg/TrackedObjects") {
return SubscriberMessageType::TRACKED_OBJECTS;
} else {
return SubscriberMessageType::UNKNOWN;
@@ -448,7 +448,6 @@ bool SubscriberBase::initSubscribers()
break;
}
}
std::cout << "---------- size: " << subscriber_variables_map_.size() << std::endl;
return true;
}

Check warning on line 452 in tools/reaction_analyzer/src/subscriber.cpp

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Complex Method 

SubscriberBase::initSubscribers has a cyclomatic complexity of 17, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

@@ -522,7 +521,6 @@ void SubscriberBase::controlCommandOutputCallback(

const auto brake_idx = findFirstBrakeIdx(cmd_buffer.first);
if (brake_idx) {
RCLCPP_INFO(node_->get_logger(), "%s reacted without published time", node_name.c_str());
const auto brake_cmd = cmd_buffer.first.at(brake_idx.value());

// TODO(brkay54): update here if message_filters package add the support for the messages which
@@ -554,6 +552,7 @@ void SubscriberBase::controlCommandOutputCallback(
}
} else {
cmd_buffer.second = brake_cmd;
RCLCPP_INFO(node_->get_logger(), "%s reacted without published time", node_name.c_str());
}
}
}

Check warning on line 558 in tools/reaction_analyzer/src/subscriber.cpp

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Complex Method 

SubscriberBase::controlCommandOutputCallback has a cyclomatic complexity of 9, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

Check warning on line 558 in tools/reaction_analyzer/src/subscriber.cpp

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Deep, Nested Complexity 

SubscriberBase::controlCommandOutputCallback has a nested complexity depth of 5, threshold = 4. This function contains deeply nested logic such as if statements and/or loops. The deeper the nesting, the lower the code health.
88 changes: 44 additions & 44 deletions tools/reaction_analyzer/src/topic_publisher.cpp
View file
Original file line number Diff line number Diff line change
@@ -40,11 +40,11 @@

// set pointcloud publisher type
if (topic_publisher_params_.pointcloud_publisher_type == "sync_header_sync_publish") {
pointcloud_publisher_type_ = PointcloudPublisherType::SyncHeaderSyncPublisher;
pointcloud_publisher_type_ = PointcloudPublisherType::SYNC_HEADER_SYNC_PUBLISHER;
} else if (topic_publisher_params_.pointcloud_publisher_type == "async_header_sync_publish") {
pointcloud_publisher_type_ = PointcloudPublisherType::AsyncHeaderSyncPublisher;
pointcloud_publisher_type_ = PointcloudPublisherType::ASYNC_HEADER_SYNC_PUBLISHER;
} else if (topic_publisher_params_.pointcloud_publisher_type == "async_publish") {
pointcloud_publisher_type_ = PointcloudPublisherType::AsyncPublisher;
pointcloud_publisher_type_ = PointcloudPublisherType::ASYNC_PUBLISHER;
} else {
RCLCPP_ERROR(node_->get_logger(), "Invalid pointcloud_publisher_type");
rclcpp::shutdown();
@@ -54,56 +54,56 @@
initRosbagPublishers();
}

void TopicPublisher::pointcloudMessagesSyncPublisher(const PointcloudPublisherType type)
{
const auto current_time = node_->now();
const bool is_object_spawned = spawn_object_cmd_;

switch (type) {
case PointcloudPublisherType::SyncHeaderSyncPublisher: {
case PointcloudPublisherType::SYNC_HEADER_SYNC_PUBLISHER: {
PublisherVarAccessor accessor;
for (const auto & publisher_var_pair : lidar_pub_variable_pair_map_) {
accessor.publishWithCurrentTime(
*publisher_var_pair.second.first, current_time, is_object_spawned);
accessor.publishWithCurrentTime(
*publisher_var_pair.second.second, current_time, is_object_spawned);
}
if (is_object_spawned && !is_object_spawned_message_published) {
is_object_spawned_message_published = true;
mutex_.lock();
spawn_cmd_time_ = node_->now();
mutex_.unlock();
}
break;
}
case PointcloudPublisherType::AsyncHeaderSyncPublisher: {
case PointcloudPublisherType::ASYNC_HEADER_SYNC_PUBLISHER: {
PublisherVarAccessor accessor;
const auto period_pointcloud_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(topic_publisher_params_.pointcloud_publisher_period));
const auto phase_dif = period_pointcloud_ns / lidar_pub_variable_pair_map_.size();

size_t counter = 0;
for (const auto & publisher_var_pair : lidar_pub_variable_pair_map_) {
const auto header_time =
current_time - std::chrono::nanoseconds(counter * phase_dif.count());
accessor.publishWithCurrentTime(
*publisher_var_pair.second.first, header_time, is_object_spawned);
accessor.publishWithCurrentTime(
*publisher_var_pair.second.second, header_time, is_object_spawned);
counter++;
}
if (is_object_spawned && !is_object_spawned_message_published) {
is_object_spawned_message_published = true;
mutex_.lock();
spawn_cmd_time_ = node_->now();
mutex_.unlock();
}
break;
}
default:
break;
}
}

Check warning on line 106 in tools/reaction_analyzer/src/topic_publisher.cpp

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Complex Method 

TopicPublisher::pointcloudMessagesSyncPublisher has a cyclomatic complexity of 10, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

void TopicPublisher::pointcloudMessagesAsyncPublisher(
const std::pair<
@@ -143,737 +143,737 @@
is_object_spawned_message_published = false;
}

void TopicPublisher::initRosbagPublishers()
{
auto string_to_publisher_message_type = [](const std::string & input) {
if (input == "sensor_msgs/msg/PointCloud2") {
return PublisherMessageType::PointCloud2;
return PublisherMessageType::POINTCLOUD2;
} else if (input == "sensor_msgs/msg/CameraInfo") {
return PublisherMessageType::CameraInfo;
return PublisherMessageType::CAMERA_INFO;
} else if (input == "sensor_msgs/msg/Image") {
return PublisherMessageType::Image;
return PublisherMessageType::IMAGE;
} else if (input == "geometry_msgs/msg/PoseWithCovarianceStamped") {
return PublisherMessageType::PoseWithCovarianceStamped;
return PublisherMessageType::POSE_WITH_COVARIANCE_STAMPED;
} else if (input == "sensor_msgs/msg/Imu") {
return PublisherMessageType::Imu;
return PublisherMessageType::IMU;
} else if (input == "autoware_auto_vehicle_msgs/msg/ControlModeReport") {
return PublisherMessageType::ControlModeReport;
return PublisherMessageType::CONTROL_MODE_REPORT;
} else if (input == "autoware_auto_vehicle_msgs/msg/GearReport") {
return PublisherMessageType::GearReport;
return PublisherMessageType::GEAR_REPORT;
} else if (input == "autoware_auto_vehicle_msgs/msg/HazardLightsReport") {
return PublisherMessageType::HazardLightsReport;
return PublisherMessageType::HAZARD_LIGHTS_REPORT;
} else if (input == "autoware_auto_vehicle_msgs/msg/SteeringReport") {
return PublisherMessageType::SteeringReport;
return PublisherMessageType::STEERING_REPORT;
} else if (input == "autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport") {
return PublisherMessageType::TurnIndicatorsReport;
return PublisherMessageType::TURN_INDICATORS_REPORT;
} else if (input == "autoware_auto_vehicle_msgs/msg/VelocityReport") {
return PublisherMessageType::VelocityReport;
return PublisherMessageType::VELOCITY_REPORT;
} else {
return PublisherMessageType::Unknown;
return PublisherMessageType::UNKNOWN;
}
};

rosbag2_cpp::Reader reader;

// read the messages without object
{
try {
reader.open(topic_publisher_params_.path_bag_without_object);
} catch (const std::exception & e) {
RCLCPP_ERROR_STREAM(node_->get_logger(), "Error opening bag file: " << e.what());
rclcpp::shutdown();
return;
}

const auto & topics = reader.get_metadata().topics_with_message_count;
auto getMessageTypeForTopic = [&topics, &string_to_publisher_message_type](
const std::string & topicName) -> PublisherMessageType {
auto it = std::find_if(topics.begin(), topics.end(), [&topicName](const auto & topic) {
return topic.topic_metadata.name == topicName;
});

if (it != topics.end()) {
return string_to_publisher_message_type(
it->topic_metadata.type); // Return the message type if found
} else {
return PublisherMessageType::Unknown; //
return PublisherMessageType::UNKNOWN; //
}
};
std::unordered_map<std::string, std::vector<rclcpp::Time>> timestamps_per_topic;
while (reader.has_next()) {
auto bag_message = reader.read_next();

const auto current_topic = bag_message->topic_name;

const auto message_type = getMessageTypeForTopic(current_topic);
if (message_type == PublisherMessageType::Unknown) {
if (message_type == PublisherMessageType::UNKNOWN) {
RCLCPP_WARN(
node_->get_logger(), "Unknown message type for topic name: %s, skipping..",
current_topic.c_str());
continue;
}

// Record timestamp
timestamps_per_topic[current_topic].emplace_back(bag_message->time_stamp);
// Deserialize and store the first message as a sample
if (timestamps_per_topic[current_topic].size() == 1) {
switch (message_type) {
case PublisherMessageType::PointCloud2: {
case PublisherMessageType::POINTCLOUD2: {
rclcpp::Serialization<PointCloud2> serialization;
auto & publisher_var = topic_publisher_map_[current_topic];
if (!std::holds_alternative<PublisherVariables<PointCloud2>>(publisher_var)) {
publisher_var = PublisherVariables<PointCloud2>();
}
std::get<PublisherVariables<PointCloud2>>(publisher_var).empty_area_message =
std::make_shared<PointCloud2>();
rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data);
serialization.deserialize_message(
&extracted_serialized_msg,
&(*std::get<PublisherVariables<PointCloud2>>(publisher_var).empty_area_message));
break;
}
case PublisherMessageType::CameraInfo: {
case PublisherMessageType::CAMERA_INFO: {
rclcpp::Serialization<sensor_msgs::msg::CameraInfo> serialization;

auto & publisher_var = topic_publisher_map_[current_topic];
if (!std::holds_alternative<PublisherVariables<sensor_msgs::msg::CameraInfo>>(
publisher_var)) {
publisher_var = PublisherVariables<sensor_msgs::msg::CameraInfo>();
}
std::get<PublisherVariables<sensor_msgs::msg::CameraInfo>>(publisher_var)
.empty_area_message = std::make_shared<sensor_msgs::msg::CameraInfo>();
rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data);
serialization.deserialize_message(
&extracted_serialized_msg,
&(*std::get<PublisherVariables<sensor_msgs::msg::CameraInfo>>(publisher_var)
.empty_area_message));
break;
}
case PublisherMessageType::Image: {
case PublisherMessageType::IMAGE: {
rclcpp::Serialization<sensor_msgs::msg::Image> serialization;

auto & publisher_var = topic_publisher_map_[current_topic];
if (!std::holds_alternative<PublisherVariables<sensor_msgs::msg::Image>>(
publisher_var)) {
publisher_var = PublisherVariables<sensor_msgs::msg::Image>();
}
std::get<PublisherVariables<sensor_msgs::msg::Image>>(publisher_var)
.empty_area_message = std::make_shared<sensor_msgs::msg::Image>();
rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data);
serialization.deserialize_message(
&extracted_serialized_msg,
&(*std::get<PublisherVariables<sensor_msgs::msg::Image>>(publisher_var)
.empty_area_message));
break;
}
case PublisherMessageType::PoseWithCovarianceStamped: {
case PublisherMessageType::POSE_WITH_COVARIANCE_STAMPED: {
rclcpp::Serialization<geometry_msgs::msg::PoseWithCovarianceStamped> serialization;
auto & publisher_var = topic_publisher_map_[current_topic];
if (!std::holds_alternative<
PublisherVariables<geometry_msgs::msg::PoseWithCovarianceStamped>>(
publisher_var)) {
publisher_var = PublisherVariables<geometry_msgs::msg::PoseWithCovarianceStamped>();
}
std::get<PublisherVariables<geometry_msgs::msg::PoseWithCovarianceStamped>>(
publisher_var)
.empty_area_message =
std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();
rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data);
serialization.deserialize_message(
&extracted_serialized_msg,
&(*std::get<PublisherVariables<geometry_msgs::msg::PoseWithCovarianceStamped>>(
publisher_var)
.empty_area_message));
break;
}
case PublisherMessageType::Imu: {
case PublisherMessageType::IMU: {
rclcpp::Serialization<sensor_msgs::msg::Imu> serialization;

auto & publisher_var = topic_publisher_map_[current_topic];
if (!std::holds_alternative<PublisherVariables<sensor_msgs::msg::Imu>>(publisher_var)) {
publisher_var = PublisherVariables<sensor_msgs::msg::Imu>();
}
std::get<PublisherVariables<sensor_msgs::msg::Imu>>(publisher_var).empty_area_message =
std::make_shared<sensor_msgs::msg::Imu>();
rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data);
serialization.deserialize_message(
&extracted_serialized_msg,
&(*std::get<PublisherVariables<sensor_msgs::msg::Imu>>(publisher_var)
.empty_area_message));
break;
}
case PublisherMessageType::ControlModeReport: {
case PublisherMessageType::CONTROL_MODE_REPORT: {
rclcpp::Serialization<autoware_auto_vehicle_msgs::msg::ControlModeReport> serialization;

auto & publisher_var = topic_publisher_map_[current_topic];
if (!std::holds_alternative<
PublisherVariables<autoware_auto_vehicle_msgs::msg::ControlModeReport>>(
publisher_var)) {
publisher_var =
PublisherVariables<autoware_auto_vehicle_msgs::msg::ControlModeReport>();
}
std::get<PublisherVariables<autoware_auto_vehicle_msgs::msg::ControlModeReport>>(
publisher_var)
.empty_area_message =
std::make_shared<autoware_auto_vehicle_msgs::msg::ControlModeReport>();
rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data);
serialization.deserialize_message(
&extracted_serialized_msg,
&(*std::get<PublisherVariables<autoware_auto_vehicle_msgs::msg::ControlModeReport>>(
publisher_var)
.empty_area_message));
break;
}
case PublisherMessageType::GearReport: {
case PublisherMessageType::GEAR_REPORT: {
rclcpp::Serialization<autoware_auto_vehicle_msgs::msg::GearReport> serialization;
auto & publisher_var = topic_publisher_map_[current_topic];
if (!std::holds_alternative<
PublisherVariables<autoware_auto_vehicle_msgs::msg::GearReport>>(publisher_var)) {
publisher_var = PublisherVariables<autoware_auto_vehicle_msgs::msg::GearReport>();
}
std::get<PublisherVariables<autoware_auto_vehicle_msgs::msg::GearReport>>(publisher_var)
.empty_area_message = std::make_shared<autoware_auto_vehicle_msgs::msg::GearReport>();
rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data);
serialization.deserialize_message(
&extracted_serialized_msg,
&(*std::get<PublisherVariables<autoware_auto_vehicle_msgs::msg::GearReport>>(
publisher_var)
.empty_area_message));
break;
}
case PublisherMessageType::HazardLightsReport: {
case PublisherMessageType::HAZARD_LIGHTS_REPORT: {
rclcpp::Serialization<autoware_auto_vehicle_msgs::msg::HazardLightsReport>
serialization;
auto & publisher_var = topic_publisher_map_[current_topic];
if (!std::holds_alternative<
PublisherVariables<autoware_auto_vehicle_msgs::msg::HazardLightsReport>>(
publisher_var)) {
publisher_var =
PublisherVariables<autoware_auto_vehicle_msgs::msg::HazardLightsReport>();
}
std::get<PublisherVariables<autoware_auto_vehicle_msgs::msg::HazardLightsReport>>(
publisher_var)
.empty_area_message =
std::make_shared<autoware_auto_vehicle_msgs::msg::HazardLightsReport>();
rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data);
serialization.deserialize_message(
&extracted_serialized_msg,
&(*std::get<PublisherVariables<autoware_auto_vehicle_msgs::msg::HazardLightsReport>>(
publisher_var)
.empty_area_message));
break;
}
case PublisherMessageType::SteeringReport: {
case PublisherMessageType::STEERING_REPORT: {
rclcpp::Serialization<autoware_auto_vehicle_msgs::msg::SteeringReport> serialization;
auto & publisher_var = topic_publisher_map_[current_topic];
if (!std::holds_alternative<
PublisherVariables<autoware_auto_vehicle_msgs::msg::SteeringReport>>(
publisher_var)) {
publisher_var = PublisherVariables<autoware_auto_vehicle_msgs::msg::SteeringReport>();
}
std::get<PublisherVariables<autoware_auto_vehicle_msgs::msg::SteeringReport>>(
publisher_var)
.empty_area_message =
std::make_shared<autoware_auto_vehicle_msgs::msg::SteeringReport>();
rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data);
serialization.deserialize_message(
&extracted_serialized_msg,
&(*std::get<PublisherVariables<autoware_auto_vehicle_msgs::msg::SteeringReport>>(
publisher_var)
.empty_area_message));
break;
}
case PublisherMessageType::TurnIndicatorsReport: {
case PublisherMessageType::TURN_INDICATORS_REPORT: {
rclcpp::Serialization<autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport>
serialization;
auto & publisher_var = topic_publisher_map_[current_topic];
if (!std::holds_alternative<
PublisherVariables<autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport>>(
publisher_var)) {
publisher_var =
PublisherVariables<autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport>();
}
std::get<PublisherVariables<autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport>>(
publisher_var)
.empty_area_message =
std::make_shared<autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport>();
rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data);
serialization.deserialize_message(
&extracted_serialized_msg,
&(*std::get<
PublisherVariables<autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport>>(
publisher_var)
.empty_area_message));
break;
}
case PublisherMessageType::VelocityReport: {
case PublisherMessageType::VELOCITY_REPORT: {
rclcpp::Serialization<autoware_auto_vehicle_msgs::msg::VelocityReport> serialization;
auto & publisher_var = topic_publisher_map_[current_topic];
if (!std::holds_alternative<
PublisherVariables<autoware_auto_vehicle_msgs::msg::VelocityReport>>(
publisher_var)) {
publisher_var = PublisherVariables<autoware_auto_vehicle_msgs::msg::VelocityReport>();
}
std::get<PublisherVariables<autoware_auto_vehicle_msgs::msg::VelocityReport>>(
publisher_var)
.empty_area_message =
std::make_shared<autoware_auto_vehicle_msgs::msg::VelocityReport>();
rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data);
serialization.deserialize_message(
&extracted_serialized_msg,
&(*std::get<PublisherVariables<autoware_auto_vehicle_msgs::msg::VelocityReport>>(
publisher_var)
.empty_area_message));
break;
}
default:
RCLCPP_WARN(
node_->get_logger(), "Unknown message type for topic name: %s, skipping..",
current_topic.c_str());
break;
}
}
}

// set frequencies of the publishers
// After collecting all timestamps for each topic
for (auto & topic_pair : timestamps_per_topic) {
auto & timestamps = topic_pair.second;

// Sort the timestamps
std::sort(timestamps.begin(), timestamps.end());

// Then proceed with the frequency calculation
std::string topic_name = topic_pair.first;
if (timestamps.size() > 1) {
int64_t total_time_diff_ns = 0;

// Accumulate the differences in nanoseconds
for (size_t i = 1; i < timestamps.size(); ++i) {
total_time_diff_ns += (timestamps[i] - timestamps[i - 1]).nanoseconds();
}

// Convert to double for the division to get the average period in nanoseconds
double period_ns =
static_cast<double>(total_time_diff_ns) / static_cast<double>(timestamps.size() - 1);

PublisherVariablesVariant & publisherVar = topic_publisher_map_[topic_name];
PublisherVarAccessor accessor;

std::visit([&](auto & var) { accessor.setPeriod(var, period_ns); }, publisherVar);
}
}
reader.close();
}

// read messages with object
{
try {
reader.open(topic_publisher_params_.path_bag_with_object);
} catch (const std::exception & e) {
RCLCPP_ERROR_STREAM(node_->get_logger(), "Error opening bag file: " << e.what());
rclcpp::shutdown();
return;
}

const auto & topics = reader.get_metadata().topics_with_message_count;

auto getMessageTypeForTopic = [&topics, &string_to_publisher_message_type](
const std::string & topicName) -> PublisherMessageType {
auto it = std::find_if(topics.begin(), topics.end(), [&topicName](const auto & topic) {
return topic.topic_metadata.name == topicName;
});

if (it != topics.end()) {
return string_to_publisher_message_type(
it->topic_metadata.type); // Return the message type if found
} else {
return PublisherMessageType::Unknown;
return PublisherMessageType::UNKNOWN;
}
};

while (reader.has_next()) {
auto bag_message = reader.read_next();
const auto current_topic = bag_message->topic_name;

const auto message_type = getMessageTypeForTopic(current_topic);
if (message_type == PublisherMessageType::Unknown) {
if (message_type == PublisherMessageType::UNKNOWN) {
RCLCPP_WARN(
node_->get_logger(), "Unknown message type for topic name: %s, skipping..",
current_topic.c_str());
continue;
}
switch (message_type) {
case PublisherMessageType::PointCloud2: {
case PublisherMessageType::POINTCLOUD2: {
rclcpp::Serialization<PointCloud2> serialization;
auto & publisher_var = topic_publisher_map_[current_topic];
if (!std::holds_alternative<PublisherVariables<PointCloud2>>(publisher_var)) {
RCLCPP_ERROR_STREAM(
node_->get_logger(), "Variant couldn't found in the topic named: " << current_topic);
rclcpp::shutdown();
}
auto & object_spawned_message =
std::get<PublisherVariables<PointCloud2>>(publisher_var).object_spawned_message;
if (!object_spawned_message) {
object_spawned_message = std::make_shared<PointCloud2>();
rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data);
serialization.deserialize_message(
&extracted_serialized_msg, &(*object_spawned_message));
}
break;
}
case PublisherMessageType::CameraInfo: {
case PublisherMessageType::CAMERA_INFO: {
rclcpp::Serialization<sensor_msgs::msg::CameraInfo> serialization;
auto & publisher_var = topic_publisher_map_[current_topic];
if (!std::holds_alternative<PublisherVariables<sensor_msgs::msg::CameraInfo>>(
publisher_var)) {
RCLCPP_ERROR_STREAM(
node_->get_logger(), "Variant couldn't found in the topic named: " << current_topic);
rclcpp::shutdown();
}
auto & object_spawned_message =
std::get<PublisherVariables<sensor_msgs::msg::CameraInfo>>(publisher_var)
.object_spawned_message;
if (!object_spawned_message) {
object_spawned_message = std::make_shared<sensor_msgs::msg::CameraInfo>();
rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data);
serialization.deserialize_message(
&extracted_serialized_msg, &(*object_spawned_message));
}
break;
}
case PublisherMessageType::Image: {
case PublisherMessageType::IMAGE: {
rclcpp::Serialization<sensor_msgs::msg::Image> serialization;
auto & publisher_var = topic_publisher_map_[current_topic];
if (!std::holds_alternative<PublisherVariables<sensor_msgs::msg::Image>>(publisher_var)) {
RCLCPP_ERROR_STREAM(
node_->get_logger(), "Variant couldn't found in the topic named: " << current_topic);
rclcpp::shutdown();
}
auto & object_spawned_message =
std::get<PublisherVariables<sensor_msgs::msg::Image>>(publisher_var)
.object_spawned_message;
if (!object_spawned_message) {
object_spawned_message = std::make_shared<sensor_msgs::msg::Image>();
rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data);
serialization.deserialize_message(
&extracted_serialized_msg, &(*object_spawned_message));
}
break;
}
case PublisherMessageType::PoseWithCovarianceStamped: {
case PublisherMessageType::POSE_WITH_COVARIANCE_STAMPED: {
rclcpp::Serialization<geometry_msgs::msg::PoseWithCovarianceStamped> serialization;
auto & publisher_var = topic_publisher_map_[current_topic];
if (!std::holds_alternative<
PublisherVariables<geometry_msgs::msg::PoseWithCovarianceStamped>>(publisher_var)) {
RCLCPP_ERROR_STREAM(
node_->get_logger(), "Variant couldn't found in the topic named: " << current_topic);
rclcpp::shutdown();
}
auto & object_spawned_message =
std::get<PublisherVariables<geometry_msgs::msg::PoseWithCovarianceStamped>>(
publisher_var)
.object_spawned_message;
if (!object_spawned_message) {
object_spawned_message =
std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();
rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data);
serialization.deserialize_message(
&extracted_serialized_msg, &(*object_spawned_message));
}
break;
}
case PublisherMessageType::Imu: {
case PublisherMessageType::IMU: {
rclcpp::Serialization<sensor_msgs::msg::Imu> serialization;
auto & publisher_var = topic_publisher_map_[current_topic];
if (!std::holds_alternative<PublisherVariables<sensor_msgs::msg::Imu>>(publisher_var)) {
RCLCPP_ERROR_STREAM(
node_->get_logger(), "Variant couldn't found in the topic named: " << current_topic);
rclcpp::shutdown();
}
auto & object_spawned_message =
std::get<PublisherVariables<sensor_msgs::msg::Imu>>(publisher_var)
.object_spawned_message;
if (!object_spawned_message) {
object_spawned_message = std::make_shared<sensor_msgs::msg::Imu>();
rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data);
serialization.deserialize_message(
&extracted_serialized_msg, &(*object_spawned_message));
}
break;
}
case PublisherMessageType::ControlModeReport: {
case PublisherMessageType::CONTROL_MODE_REPORT: {
rclcpp::Serialization<autoware_auto_vehicle_msgs::msg::ControlModeReport> serialization;
auto & publisher_var = topic_publisher_map_[current_topic];
if (!std::holds_alternative<
PublisherVariables<autoware_auto_vehicle_msgs::msg::ControlModeReport>>(
publisher_var)) {
RCLCPP_ERROR_STREAM(
node_->get_logger(), "Variant couldn't found in the topic named: " << current_topic);
rclcpp::shutdown();
}
auto & object_spawned_message =
std::get<PublisherVariables<autoware_auto_vehicle_msgs::msg::ControlModeReport>>(
publisher_var)
.object_spawned_message;
if (!object_spawned_message) {
object_spawned_message =
std::make_shared<autoware_auto_vehicle_msgs::msg::ControlModeReport>();
rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data);
serialization.deserialize_message(
&extracted_serialized_msg, &(*object_spawned_message));
}
break;
}
case PublisherMessageType::GearReport: {
case PublisherMessageType::GEAR_REPORT: {
rclcpp::Serialization<autoware_auto_vehicle_msgs::msg::GearReport> serialization;
auto & publisher_var = topic_publisher_map_[current_topic];
if (!std::holds_alternative<
PublisherVariables<autoware_auto_vehicle_msgs::msg::GearReport>>(publisher_var)) {
RCLCPP_ERROR_STREAM(
node_->get_logger(), "Variant couldn't found in the topic named: " << current_topic);
rclcpp::shutdown();
}
auto & object_spawned_message =
std::get<PublisherVariables<autoware_auto_vehicle_msgs::msg::GearReport>>(publisher_var)
.object_spawned_message;
if (!object_spawned_message) {
object_spawned_message =
std::make_shared<autoware_auto_vehicle_msgs::msg::GearReport>();
rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data);
serialization.deserialize_message(
&extracted_serialized_msg, &(*object_spawned_message));
}
break;
}
case PublisherMessageType::HazardLightsReport: {
case PublisherMessageType::HAZARD_LIGHTS_REPORT: {
rclcpp::Serialization<autoware_auto_vehicle_msgs::msg::HazardLightsReport> serialization;
auto & publisher_var = topic_publisher_map_[current_topic];
if (!std::holds_alternative<
PublisherVariables<autoware_auto_vehicle_msgs::msg::HazardLightsReport>>(
publisher_var)) {
RCLCPP_ERROR_STREAM(
node_->get_logger(), "Variant couldn't found in the topic named: " << current_topic);
rclcpp::shutdown();
}
auto & object_spawned_message =
std::get<PublisherVariables<autoware_auto_vehicle_msgs::msg::HazardLightsReport>>(
publisher_var)
.object_spawned_message;
if (!object_spawned_message) {
object_spawned_message =
std::make_shared<autoware_auto_vehicle_msgs::msg::HazardLightsReport>();
rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data);
serialization.deserialize_message(
&extracted_serialized_msg, &(*object_spawned_message));
}
break;
}
case PublisherMessageType::SteeringReport: {
case PublisherMessageType::STEERING_REPORT: {
rclcpp::Serialization<autoware_auto_vehicle_msgs::msg::SteeringReport> serialization;
auto & publisher_var = topic_publisher_map_[current_topic];
if (!std::holds_alternative<
PublisherVariables<autoware_auto_vehicle_msgs::msg::SteeringReport>>(
publisher_var)) {
RCLCPP_ERROR_STREAM(
node_->get_logger(), "Variant couldn't found in the topic named: " << current_topic);
rclcpp::shutdown();
}
auto & object_spawned_message =
std::get<PublisherVariables<autoware_auto_vehicle_msgs::msg::SteeringReport>>(
publisher_var)
.object_spawned_message;
if (!object_spawned_message) {
object_spawned_message =
std::make_shared<autoware_auto_vehicle_msgs::msg::SteeringReport>();
rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data);
serialization.deserialize_message(
&extracted_serialized_msg, &(*object_spawned_message));
}
break;
}
case PublisherMessageType::TurnIndicatorsReport: {
case PublisherMessageType::TURN_INDICATORS_REPORT: {
rclcpp::Serialization<autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport>
serialization;
auto & publisher_var = topic_publisher_map_[current_topic];
if (!std::holds_alternative<
PublisherVariables<autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport>>(
publisher_var)) {
RCLCPP_ERROR_STREAM(
node_->get_logger(), "Variant couldn't found in the topic named: " << current_topic);
rclcpp::shutdown();
}
auto & object_spawned_message =
std::get<PublisherVariables<autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport>>(
publisher_var)
.object_spawned_message;
if (!object_spawned_message) {
object_spawned_message =
std::make_shared<autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport>();
rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data);
serialization.deserialize_message(
&extracted_serialized_msg, &(*object_spawned_message));
}
break;
}
case PublisherMessageType::VelocityReport: {
case PublisherMessageType::VELOCITY_REPORT: {
rclcpp::Serialization<autoware_auto_vehicle_msgs::msg::VelocityReport> serialization;
auto & publisher_var = topic_publisher_map_[current_topic];
if (!std::holds_alternative<
PublisherVariables<autoware_auto_vehicle_msgs::msg::VelocityReport>>(
publisher_var)) {
RCLCPP_ERROR_STREAM(
node_->get_logger(), "Variant couldn't found in the topic named: " << current_topic);
rclcpp::shutdown();
}
auto & object_spawned_message =
std::get<PublisherVariables<autoware_auto_vehicle_msgs::msg::VelocityReport>>(
publisher_var)
.object_spawned_message;
if (!object_spawned_message) {
object_spawned_message =
std::make_shared<autoware_auto_vehicle_msgs::msg::VelocityReport>();
rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data);
serialization.deserialize_message(
&extracted_serialized_msg, &(*object_spawned_message));
}
break;
}
default:
RCLCPP_WARN(
node_->get_logger(), "Unknown message type for topic name: %s, skipping..",
current_topic.c_str());
break;
}
}
reader.close();
}

// check messages are correctly initialized or not from rosbags
for (const auto & [topic_name, variant] : topic_publisher_map_) {
PublisherVarAccessor accessor;
auto empty_area_message =
std::visit([&](const auto & var) { return accessor.getEmptyAreaMessage(var); }, variant);
auto object_spawned_message =
std::visit([&](const auto & var) { return accessor.getObjectSpawnedMessage(var); }, variant);

if (!empty_area_message) {
RCLCPP_ERROR_STREAM(
node_->get_logger(),
"Empty area message couldn't found in the topic named: " << topic_name);
rclcpp::shutdown();
} else if (!object_spawned_message) {
RCLCPP_ERROR_STREAM(
node_->get_logger(),
"Object spawned message couldn't found in the topic named: " << topic_name);
rclcpp::shutdown();
}
}

std::unordered_map<std::string, PublisherVariables<PointCloud2>>
pointcloud_variables_map; // temp map for pointcloud publishers

// initialize timers and message publishers
for (auto & [topic_name, variant] : topic_publisher_map_) {
PublisherVarAccessor accessor;
const auto & topic_ref = topic_name;
const auto period_ns = std::chrono::duration<double, std::nano>(
std::visit([&](const auto & var) { return accessor.getPeriod(var); }, variant));

// Dynamically create the correct publisher type based on the topic
std::visit(
[&](auto & var) {
using MessageType = typename decltype(var.empty_area_message)::element_type;

// Check if the MessageType is PointCloud2
if constexpr (
std::is_same_v<MessageType, sensor_msgs::msg::PointCloud2> ||
std::is_same_v<MessageType, sensor_msgs::msg::Image>) {
// For PointCloud2, use rclcpp::SensorDataQoS
var.publisher = node_->create_publisher<MessageType>(topic_ref, rclcpp::SensorDataQoS());
} else {
// For other message types, use the QoS setting depth of 1
var.publisher = node_->create_publisher<MessageType>(topic_ref, rclcpp::QoS(1));
}
},
variant);

// Conditionally create the timer based on the message type, if message type is not PointCloud2
std::visit(
[&](auto & var) {
using MessageType = typename decltype(var.empty_area_message)::element_type;

if constexpr (!std::is_same_v<MessageType, sensor_msgs::msg::PointCloud2>) {
var.timer = node_->create_wall_timer(
period_ns, [this, topic_ref]() { this->genericMessagePublisher(topic_ref); });
} else {
// For PointCloud2, Store the variables in a temporary map
pointcloud_variables_map[topic_ref] = var;
}
},
variant);
}

// Set the point cloud publisher timers
if (pointcloud_variables_map.empty()) {
RCLCPP_ERROR(node_->get_logger(), "No pointcloud publishers found!");
rclcpp::shutdown();
return;
}

// Arrange the PointCloud2 variables w.r.t. the lidars' name
for (auto & [topic_name, pointcloud_variant] : pointcloud_variables_map) {
const auto lidar_name = split(topic_name, '/').at(3);

if (lidar_pub_variable_pair_map_.find(lidar_name) == lidar_pub_variable_pair_map_.end()) {
lidar_pub_variable_pair_map_[lidar_name] = std::make_pair(
std::make_shared<PublisherVariables<PointCloud2>>(pointcloud_variant), nullptr);
} else {
if (lidar_pub_variable_pair_map_[lidar_name].second) {
RCLCPP_ERROR_STREAM(
node_->get_logger(),
"Lidar name: " << lidar_name << " is already used by another pointcloud publisher");
rclcpp::shutdown();
}
lidar_pub_variable_pair_map_[lidar_name].second =
std::make_shared<PublisherVariables<PointCloud2>>(pointcloud_variant);
}
}

// Create the timer(s) to publish PointCloud2 Messages
const auto period_pointcloud_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(topic_publisher_params_.pointcloud_publisher_period));

if (pointcloud_publisher_type_ != PointcloudPublisherType::AsyncPublisher) {
if (pointcloud_publisher_type_ != PointcloudPublisherType::ASYNC_PUBLISHER) {
// Create 1 timer to publish all PointCloud2 messages
pointcloud_sync_publish_timer_ = node_->create_wall_timer(period_pointcloud_ns, [this]() {
this->pointcloudMessagesSyncPublisher(this->pointcloud_publisher_type_);
});

} else {
// Create multiple timers which will run with a phase difference
const auto phase_dif = period_pointcloud_ns / lidar_pub_variable_pair_map_.size();

// Create a timer to delay the timer which will be created for each lidar topics
auto one_shot_timer = node_->create_wall_timer(phase_dif, [this, period_pointcloud_ns]() {
for (const auto & [lidar_name, publisher_var_pair] : lidar_pub_variable_pair_map_) {
if (
pointcloud_publish_timers_map_.find(lidar_name) == pointcloud_publish_timers_map_.end()) {
// Create the periodic timer
auto periodic_timer =
node_->create_wall_timer(period_pointcloud_ns, [this, publisher_var_pair]() {
this->pointcloudMessagesAsyncPublisher(publisher_var_pair);
});
// Store the periodic timer to keep it alive
pointcloud_publish_timers_map_[lidar_name] = periodic_timer;
return;
}
}
// close the timer
one_shot_timer_shared_ptr_->cancel();
});

one_shot_timer_shared_ptr_ = one_shot_timer; // Store a weak pointer to the timer
}
}

Check warning on line 878 in tools/reaction_analyzer/src/topic_publisher.cpp

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Complex Method 

TopicPublisher::initRosbagPublishers has a cyclomatic complexity of 95, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

Check warning on line 878 in tools/reaction_analyzer/src/topic_publisher.cpp

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Bumpy Road Ahead 

TopicPublisher::initRosbagPublishers has 10 blocks with nested conditional logic. Any nesting of 2 or deeper is considered. Threshold is one single, nested block per function. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.

Check warning on line 878 in tools/reaction_analyzer/src/topic_publisher.cpp

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Deep, Nested Complexity 

TopicPublisher::initRosbagPublishers has a nested complexity depth of 4, threshold = 4. This function contains deeply nested logic such as if statements and/or loops. The deeper the nesting, the lower the code health.
} // namespace reaction_analyzer::topic_publisher

0 comments on commit 70dcaf6

Please sign in to comment.