From 94ca733dca3196a62620eba1118f1348cb5885c4 Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Tue, 5 Mar 2024 15:30:11 +0300 Subject: [PATCH] feat: update style and readme Signed-off-by: Berkay Karaman --- tools/reaction_analyzer/README.md | 117 +++++++++++++----- .../include/topic_publisher.hpp | 30 ++--- .../param/reaction_analyzer.param.yaml | 32 ++--- .../src/reaction_analyzer_node.cpp | 2 - tools/reaction_analyzer/src/subscriber.cpp | 15 ++- .../reaction_analyzer/src/topic_publisher.cpp | 88 ++++++------- 6 files changed, 165 insertions(+), 119 deletions(-) diff --git a/tools/reaction_analyzer/README.md b/tools/reaction_analyzer/README.md index 8ada7254e5021..78f3ae11361ea 100644 --- a/tools/reaction_analyzer/README.md +++ b/tools/reaction_analyzer/README.md @@ -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 check out the following branches before launch the + reaction analyzer: For the `autoware_individual_params` repository, check out the + branch [here](https://github.com/brkay54/autoware_individual_params/tree/bk/reaction-analyzer-config). +- For the `awsim_sensor_kit_launch` repository, check out the + branch [here](https://github.com/brkay54/awsim_sensor_kit_launch/tree/bk/reaction-analyzer-config). +- After you check outed the branches, you can start to test with the following command: ```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 @@ -116,7 +125,7 @@ the `path_bag_without_object` and `path_bag_with_object` parameters with the pat | ---------------------------------------------------------------------------- | ------ | --------------------------------------------------------------------------------------------------------------------------------------------- | | `timer_period` | double | [s] Period for the main processing timer. | | `test_iteration` | int | Number of iterations for the test. | -| `output_file_path` | string | Directory path where test results and statictics will be stored. | +| `output_file_path` | string | Directory path where test results and statistics will be stored. | | `object_search_radius_offset` | double | [m] Additional radius added to the search area when looking for objects. | | `spawn_time_after_init` | double | [s] Time delay after initialization before spawning objects. Only valid `perception_planning` mode. | | `spawn_distance_threshold` | double | [m] Distance threshold for spawning objects. Only valid `planning_control` mode. | @@ -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. diff --git a/tools/reaction_analyzer/include/topic_publisher.hpp b/tools/reaction_analyzer/include/topic_publisher.hpp index 939abab591d84..dfab3c7f87e79 100644 --- a/tools/reaction_analyzer/include/topic_publisher.hpp +++ b/tools/reaction_analyzer/include/topic_publisher.hpp @@ -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 }; /** diff --git a/tools/reaction_analyzer/param/reaction_analyzer.param.yaml b/tools/reaction_analyzer/param/reaction_analyzer.param.yaml index 082cdaf864265..70e6ae69eac57 100644 --- a/tools/reaction_analyzer/param/reaction_analyzer.param.yaml +++ b/tools/reaction_analyzer/param/reaction_analyzer.param.yaml @@ -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 diff --git a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp index 95ac106375897..a8fbf13606f25 100644 --- a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp +++ b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp @@ -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(); } diff --git a/tools/reaction_analyzer/src/subscriber.cpp b/tools/reaction_analyzer/src/subscriber.cpp index 43fd05515fa0a..c29243519f599 100644 --- a/tools/reaction_analyzer/src/subscriber.cpp +++ b/tools/reaction_analyzer/src/subscriber.cpp @@ -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; } @@ -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()); } } } diff --git a/tools/reaction_analyzer/src/topic_publisher.cpp b/tools/reaction_analyzer/src/topic_publisher.cpp index 079bc8fb057e5..4ac1c11d6622b 100644 --- a/tools/reaction_analyzer/src/topic_publisher.cpp +++ b/tools/reaction_analyzer/src/topic_publisher.cpp @@ -40,11 +40,11 @@ TopicPublisher::TopicPublisher( // 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(); @@ -60,7 +60,7 @@ void TopicPublisher::pointcloudMessagesSyncPublisher(const PointcloudPublisherTy 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( @@ -76,7 +76,7 @@ void TopicPublisher::pointcloudMessagesSyncPublisher(const PointcloudPublisherTy } break; } - case PointcloudPublisherType::AsyncHeaderSyncPublisher: { + case PointcloudPublisherType::ASYNC_HEADER_SYNC_PUBLISHER: { PublisherVarAccessor accessor; const auto period_pointcloud_ns = std::chrono::duration_cast( std::chrono::duration(topic_publisher_params_.pointcloud_publisher_period)); @@ -147,29 +147,29 @@ 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; } }; @@ -196,7 +196,7 @@ void TopicPublisher::initRosbagPublishers() 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> timestamps_per_topic; @@ -206,7 +206,7 @@ void TopicPublisher::initRosbagPublishers() 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()); @@ -218,7 +218,7 @@ void TopicPublisher::initRosbagPublishers() // 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 serialization; auto & publisher_var = topic_publisher_map_[current_topic]; if (!std::holds_alternative>(publisher_var)) { @@ -232,7 +232,7 @@ void TopicPublisher::initRosbagPublishers() &(*std::get>(publisher_var).empty_area_message)); break; } - case PublisherMessageType::CameraInfo: { + case PublisherMessageType::CAMERA_INFO: { rclcpp::Serialization serialization; auto & publisher_var = topic_publisher_map_[current_topic]; @@ -249,7 +249,7 @@ void TopicPublisher::initRosbagPublishers() .empty_area_message)); break; } - case PublisherMessageType::Image: { + case PublisherMessageType::IMAGE: { rclcpp::Serialization serialization; auto & publisher_var = topic_publisher_map_[current_topic]; @@ -266,7 +266,7 @@ void TopicPublisher::initRosbagPublishers() .empty_area_message)); break; } - case PublisherMessageType::PoseWithCovarianceStamped: { + case PublisherMessageType::POSE_WITH_COVARIANCE_STAMPED: { rclcpp::Serialization serialization; auto & publisher_var = topic_publisher_map_[current_topic]; if (!std::holds_alternative< @@ -286,7 +286,7 @@ void TopicPublisher::initRosbagPublishers() .empty_area_message)); break; } - case PublisherMessageType::Imu: { + case PublisherMessageType::IMU: { rclcpp::Serialization serialization; auto & publisher_var = topic_publisher_map_[current_topic]; @@ -302,7 +302,7 @@ void TopicPublisher::initRosbagPublishers() .empty_area_message)); break; } - case PublisherMessageType::ControlModeReport: { + case PublisherMessageType::CONTROL_MODE_REPORT: { rclcpp::Serialization serialization; auto & publisher_var = topic_publisher_map_[current_topic]; @@ -324,7 +324,7 @@ void TopicPublisher::initRosbagPublishers() .empty_area_message)); break; } - case PublisherMessageType::GearReport: { + case PublisherMessageType::GEAR_REPORT: { rclcpp::Serialization serialization; auto & publisher_var = topic_publisher_map_[current_topic]; if (!std::holds_alternative< @@ -341,7 +341,7 @@ void TopicPublisher::initRosbagPublishers() .empty_area_message)); break; } - case PublisherMessageType::HazardLightsReport: { + case PublisherMessageType::HAZARD_LIGHTS_REPORT: { rclcpp::Serialization serialization; auto & publisher_var = topic_publisher_map_[current_topic]; @@ -363,7 +363,7 @@ void TopicPublisher::initRosbagPublishers() .empty_area_message)); break; } - case PublisherMessageType::SteeringReport: { + case PublisherMessageType::STEERING_REPORT: { rclcpp::Serialization serialization; auto & publisher_var = topic_publisher_map_[current_topic]; if (!std::holds_alternative< @@ -383,7 +383,7 @@ void TopicPublisher::initRosbagPublishers() .empty_area_message)); break; } - case PublisherMessageType::TurnIndicatorsReport: { + case PublisherMessageType::TURN_INDICATORS_REPORT: { rclcpp::Serialization serialization; auto & publisher_var = topic_publisher_map_[current_topic]; @@ -406,7 +406,7 @@ void TopicPublisher::initRosbagPublishers() .empty_area_message)); break; } - case PublisherMessageType::VelocityReport: { + case PublisherMessageType::VELOCITY_REPORT: { rclcpp::Serialization serialization; auto & publisher_var = topic_publisher_map_[current_topic]; if (!std::holds_alternative< @@ -488,7 +488,7 @@ void TopicPublisher::initRosbagPublishers() return string_to_publisher_message_type( it->topic_metadata.type); // Return the message type if found } else { - return PublisherMessageType::Unknown; + return PublisherMessageType::UNKNOWN; } }; @@ -497,14 +497,14 @@ void TopicPublisher::initRosbagPublishers() 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 serialization; auto & publisher_var = topic_publisher_map_[current_topic]; if (!std::holds_alternative>(publisher_var)) { @@ -522,7 +522,7 @@ void TopicPublisher::initRosbagPublishers() } break; } - case PublisherMessageType::CameraInfo: { + case PublisherMessageType::CAMERA_INFO: { rclcpp::Serialization serialization; auto & publisher_var = topic_publisher_map_[current_topic]; if (!std::holds_alternative>( @@ -542,7 +542,7 @@ void TopicPublisher::initRosbagPublishers() } break; } - case PublisherMessageType::Image: { + case PublisherMessageType::IMAGE: { rclcpp::Serialization serialization; auto & publisher_var = topic_publisher_map_[current_topic]; if (!std::holds_alternative>(publisher_var)) { @@ -561,7 +561,7 @@ void TopicPublisher::initRosbagPublishers() } break; } - case PublisherMessageType::PoseWithCovarianceStamped: { + case PublisherMessageType::POSE_WITH_COVARIANCE_STAMPED: { rclcpp::Serialization serialization; auto & publisher_var = topic_publisher_map_[current_topic]; if (!std::holds_alternative< @@ -583,7 +583,7 @@ void TopicPublisher::initRosbagPublishers() } break; } - case PublisherMessageType::Imu: { + case PublisherMessageType::IMU: { rclcpp::Serialization serialization; auto & publisher_var = topic_publisher_map_[current_topic]; if (!std::holds_alternative>(publisher_var)) { @@ -602,7 +602,7 @@ void TopicPublisher::initRosbagPublishers() } break; } - case PublisherMessageType::ControlModeReport: { + case PublisherMessageType::CONTROL_MODE_REPORT: { rclcpp::Serialization serialization; auto & publisher_var = topic_publisher_map_[current_topic]; if (!std::holds_alternative< @@ -625,7 +625,7 @@ void TopicPublisher::initRosbagPublishers() } break; } - case PublisherMessageType::GearReport: { + case PublisherMessageType::GEAR_REPORT: { rclcpp::Serialization serialization; auto & publisher_var = topic_publisher_map_[current_topic]; if (!std::holds_alternative< @@ -646,7 +646,7 @@ void TopicPublisher::initRosbagPublishers() } break; } - case PublisherMessageType::HazardLightsReport: { + case PublisherMessageType::HAZARD_LIGHTS_REPORT: { rclcpp::Serialization serialization; auto & publisher_var = topic_publisher_map_[current_topic]; if (!std::holds_alternative< @@ -669,7 +669,7 @@ void TopicPublisher::initRosbagPublishers() } break; } - case PublisherMessageType::SteeringReport: { + case PublisherMessageType::STEERING_REPORT: { rclcpp::Serialization serialization; auto & publisher_var = topic_publisher_map_[current_topic]; if (!std::holds_alternative< @@ -692,7 +692,7 @@ void TopicPublisher::initRosbagPublishers() } break; } - case PublisherMessageType::TurnIndicatorsReport: { + case PublisherMessageType::TURN_INDICATORS_REPORT: { rclcpp::Serialization serialization; auto & publisher_var = topic_publisher_map_[current_topic]; @@ -716,7 +716,7 @@ void TopicPublisher::initRosbagPublishers() } break; } - case PublisherMessageType::VelocityReport: { + case PublisherMessageType::VELOCITY_REPORT: { rclcpp::Serialization serialization; auto & publisher_var = topic_publisher_map_[current_topic]; if (!std::holds_alternative< @@ -844,7 +844,7 @@ void TopicPublisher::initRosbagPublishers() const auto period_pointcloud_ns = std::chrono::duration_cast( std::chrono::duration(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_);