diff --git a/tools/reaction_analyzer/CMakeLists.txt b/tools/reaction_analyzer/CMakeLists.txt
index e05dbf0c3b55d..b5b5be4d4ad0c 100644
--- a/tools/reaction_analyzer/CMakeLists.txt
+++ b/tools/reaction_analyzer/CMakeLists.txt
@@ -10,8 +10,12 @@ find_package(Eigen3 REQUIRED)
ament_auto_add_library(reaction_analyzer SHARED
include/reaction_analyzer_node.hpp
src/reaction_analyzer_node.cpp
+ include/subscriber.hpp
+ src/subscriber.cpp
include/topic_publisher.hpp
src/topic_publisher.cpp
+ include/utils.hpp
+ src/utils.cpp
)
target_include_directories(reaction_analyzer
diff --git a/tools/reaction_analyzer/README.md b/tools/reaction_analyzer/README.md
index 3a2257c38bfc7..8ada7254e5021 100644
--- a/tools/reaction_analyzer/README.md
+++ b/tools/reaction_analyzer/README.md
@@ -112,26 +112,27 @@ the `path_bag_without_object` and `path_bag_with_object` parameters with the pat
## Parameters
-| Name | Type | Description |
-| ------------------------------------------------------------------ | ------ | --------------------------------------------------------------------------------------------------------------------------------------------- |
-| `timer_period` | double | [s] Period for the main processing timer. |
-| `published_time_expire_duration` | double | [s] Time horizon of the PublishedTime message vector. |
-| `test_iteration` | int | Number of iterations for the test. |
-| `output_file_path` | string | Directory path where test results and statictics 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. |
-| `spawned_pointcloud_sampling_distance` | double | [m] Sampling distance for point clouds of spawned objects. Only valid `planning_control` mode. |
-| `dummy_perception_publisher_period` | double | [s] Publishing period for the dummy perception data. Only valid `planning_control` mode. |
-| `first_brake_params.debug_control_commands` | bool | Debug publish flag. |
-| `first_brake_params.control_cmd_buffer_time_interval` | double | [s] Time interval for buffering control commands. |
-| `first_brake_params.min_number_descending_order_control_cmd` | int | Minimum number of control commands in descending order for triggering brake. |
-| `first_brake_params.min_jerk_for_brake_cmd` | double | [m/s³] Minimum jerk value for issuing a brake command. |
-| `initialization_pose` | struct | Initial pose of the vehicle, containing `x`, `y`, `z`, `roll`, `pitch`, and `yaw` fields. Only valid `planning_control` mode. |
-| `entity_params` | struct | Parameters for entities (e.g., obstacles), containing `x`, `y`, `z`, `roll`, `pitch`, `yaw`, `x_dimension`, `y_dimension`, and `z_dimension`. |
-| `goal_pose` | struct | Goal pose of the vehicle, containing `x`, `y`, `z`, `roll`, `pitch`, and `yaw` fields. |
-| `topic_publisher.path_bag_without_object` | string | Path to the ROS bag file without objects. Only valid `perception_planning` mode. |
-| `topic_publisher.path_bag_with_object` | string | Path to the ROS bag file with objects. Only valid `perception_planning` mode. |
-| `topic_publisher.pointcloud_publisher.pointcloud_publisher_type` | string | Defines how the PointCloud2 messages are going to be published. Modes explained above. |
-| `topic_publisher.pointcloud_publisher.pointcloud_publisher_period` | double | [s] Publishing period of the PointCloud2 messages. |
-| `reaction_chain` | struct | List of the nodes with their topics and topic's message types. |
+| Name | Type | Description |
+| ---------------------------------------------------------------------------- | ------ | --------------------------------------------------------------------------------------------------------------------------------------------- |
+| `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. |
+| `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. |
+| `spawned_pointcloud_sampling_distance` | double | [m] Sampling distance for point clouds of spawned objects. Only valid `planning_control` mode. |
+| `dummy_perception_publisher_period` | double | [s] Publishing period for the dummy perception data. Only valid `planning_control` mode. |
+| `initialization_pose` | struct | Initial pose of the vehicle, containing `x`, `y`, `z`, `roll`, `pitch`, and `yaw` fields. Only valid `planning_control` mode. |
+| `entity_params` | struct | Parameters for entities (e.g., obstacles), containing `x`, `y`, `z`, `roll`, `pitch`, `yaw`, `x_dimension`, `y_dimension`, and `z_dimension`. |
+| `goal_pose` | struct | Goal pose of the vehicle, containing `x`, `y`, `z`, `roll`, `pitch`, and `yaw` fields. |
+| `topic_publisher.path_bag_without_object` | string | Path to the ROS bag file without objects. Only valid `perception_planning` mode. |
+| `topic_publisher.path_bag_with_object` | string | Path to the ROS bag file with objects. Only valid `perception_planning` mode. |
+| `topic_publisher.pointcloud_publisher.pointcloud_publisher_type` | string | Defines how the PointCloud2 messages are going to be published. Modes explained above. |
+| `topic_publisher.pointcloud_publisher.pointcloud_publisher_period` | double | [s] Publishing period of the PointCloud2 messages. |
+| `reaction_params.first_brake_params.debug_control_commands` | bool | Debug publish flag. |
+| `reaction_params.first_brake_params.control_cmd_buffer_time_interval` | double | [s] Time interval for buffering control commands. |
+| `reaction_params.first_brake_params.min_number_descending_order_control_cmd` | int | Minimum number of control commands in descending order for triggering brake. |
+| `reaction_params.first_brake_params.min_jerk_for_brake_cmd` | double | [m/s³] Minimum jerk value for issuing a brake command. |
+| `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. |
diff --git a/tools/reaction_analyzer/include/reaction_analyzer_node.hpp b/tools/reaction_analyzer/include/reaction_analyzer_node.hpp
index 1ab03e8bd8dab..81c6f75abaa92 100644
--- a/tools/reaction_analyzer/include/reaction_analyzer_node.hpp
+++ b/tools/reaction_analyzer/include/reaction_analyzer_node.hpp
@@ -15,30 +15,30 @@
#ifndef REACTION_ANALYZER_NODE_HPP_
#define REACTION_ANALYZER_NODE_HPP_
-#include <motion_utils/trajectory/trajectory.hpp>
+#include "tf2/transform_datatypes.h"
+
+#include <pcl/impl/point_types.hpp>
+#include <pcl_ros/transforms.hpp>
#include <rclcpp/rclcpp.hpp>
-#include <rosbag2_cpp/reader.hpp>
+#include <subscriber.hpp>
+#include <tf2_eigen/tf2_eigen/tf2_eigen.hpp>
#include <tier4_autoware_utils/geometry/geometry.hpp>
#include <tier4_autoware_utils/math/unit_conversion.hpp>
#include <topic_publisher.hpp>
+#include <utils.hpp>
#include <autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>
#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
#include <autoware_adapi_v1_msgs/msg/route_state.hpp>
#include <autoware_adapi_v1_msgs/srv/change_operation_mode.hpp>
-#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
-#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
-#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
-#include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
-#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
-#include <autoware_internal_msgs/msg/published_time.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
-#include <message_filters/cache.h>
-#include <message_filters/subscriber.h>
-#include <message_filters/sync_policies/exact_time.h>
-#include <message_filters/synchronizer.h>
+#include <boost/uuid/string_generator.hpp>
+#include <boost/uuid/uuid.hpp>
+#include <boost/uuid/uuid_generators.hpp>
+#include <boost/uuid/uuid_io.hpp>
+
#include <pcl/common/distances.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
@@ -56,61 +56,18 @@ using autoware_adapi_v1_msgs::msg::LocalizationInitializationState;
using autoware_adapi_v1_msgs::msg::OperationModeState;
using autoware_adapi_v1_msgs::msg::RouteState;
using autoware_adapi_v1_msgs::srv::ChangeOperationMode;
-using autoware_auto_control_msgs::msg::AckermannControlCommand;
-using autoware_auto_perception_msgs::msg::DetectedObject;
-using autoware_auto_perception_msgs::msg::DetectedObjects;
using autoware_auto_perception_msgs::msg::PredictedObject;
using autoware_auto_perception_msgs::msg::PredictedObjects;
-using autoware_auto_perception_msgs::msg::TrackedObject;
-using autoware_auto_perception_msgs::msg::TrackedObjects;
-using autoware_auto_planning_msgs::msg::Trajectory;
-using autoware_internal_msgs::msg::PublishedTime;
using geometry_msgs::msg::Pose;
using nav_msgs::msg::Odometry;
using sensor_msgs::msg::PointCloud2;
-// Buffers to be used to store subscribed messages
-using ControlCommandBuffer =
- std::pair<std::vector<AckermannControlCommand>, std::optional<AckermannControlCommand>>;
-using TrajectoryBuffer = std::optional<Trajectory>;
-using PointCloud2Buffer = std::optional<PointCloud2>;
-using PredictedObjectsBuffer = std::optional<PredictedObjects>;
-using DetectedObjectsBuffer = std::optional<DetectedObjects>;
-using TrackedObjectsBuffer = std::optional<TrackedObjects>;
-
-// Variant to store different types of buffers
-using BufferVariant = std::variant<
- ControlCommandBuffer, TrajectoryBuffer, PointCloud2Buffer, PredictedObjectsBuffer,
- DetectedObjectsBuffer, TrackedObjectsBuffer>;
-
-// The supported message types
-enum class SubscriberMessageType {
- Unknown = 0,
- AckermannControlCommand = 1,
- Trajectory = 2,
- PointCloud2 = 3,
- DetectedObjects = 4,
- PredictedObjects = 5,
- TrackedObjects = 6,
-};
-
// The running mode of the node
enum class RunningMode {
PerceptionPlanning = 0,
PlanningControl = 1,
};
-// The configuration of the topic to be subscribed which are defined in reaction_chain
-struct TopicConfig
-{
- std::string node_name;
- std::string topic_address;
- std::string time_debug_topic_address;
- SubscriberMessageType message_type;
-};
-
-using ChainModules = std::vector<TopicConfig>;
-
struct PoseParams
{
double x;
@@ -138,68 +95,25 @@ struct NodeParams
{
std::string running_mode;
double timer_period;
- double published_time_expire_duration;
std::string output_file_path;
size_t test_iteration;
- double object_search_radius_offset;
double spawn_time_after_init;
double spawn_distance_threshold;
double spawned_pointcloud_sampling_distance;
double dummy_perception_publisher_period;
- bool debug_control_commands;
- double control_cmd_buffer_time_interval;
- double min_jerk_for_brake_cmd;
- size_t min_number_descending_order_control_cmd;
PoseParams initial_pose;
PoseParams goal_pose;
EntityParams entity_params;
};
-// class PublishedTimeSubscriber
-//{
-// public:
-// PublishedTimeSubscriber(const std::string & topic_name, rclcpp::Node * node) : node_(node)
-// {
-// // Initialize subscriber with cache
-// message_subscriber_ =
-// std::make_shared<message_filters::Subscriber<PublishedTime>>(node, topic_name);
-// cache_ = std::make_shared<message_filters::Cache<PublishedTime>>(
-// *message_subscriber_, 10); // Cache size of 100 messages
-// }
-//
-// std::optional<PublishedTime> getPublishedTime(const std_msgs::msg::Header & header)
-// {
-// for (const auto & msg : cache_->getInterval(rclcpp::Time(0), node_->now())) {
-// if (msg->header.stamp == header.stamp && msg->header.frame_id == header.frame_id) {
-// return *msg;
-// }
-// }
-// return std::nullopt; // Return an empty optional if no match is found
-// }
-//
-// std::optional<PublishedTime> getPublishedTime(const rclcpp::Time & stamp)
-// {
-// for (const auto & msg : cache_->getInterval(rclcpp::Time(0), node_->now())) {
-// if (msg->header.stamp == stamp) {
-// return *msg;
-// }
-// }
-// return std::nullopt; // Return an empty optional if no match is found
-// }
-//
-// private:
-// std::shared_ptr<message_filters::Subscriber<PublishedTime>> message_subscriber_;
-// std::shared_ptr<message_filters::Cache<PublishedTime>> cache_;
-// rclcpp::Node * node_;
-// };
-
class ReactionAnalyzerNode : public rclcpp::Node
{
public:
explicit ReactionAnalyzerNode(rclcpp::NodeOptions options);
-
~ReactionAnalyzerNode() = default;
+ Odometry::ConstSharedPtr odometry_;
+
private:
std::mutex mutex_;
RunningMode node_running_mode_;
@@ -211,7 +125,6 @@ class ReactionAnalyzerNode : public rclcpp::Node
geometry_msgs::msg::Pose entity_pose_;
geometry_msgs::msg::PoseWithCovarianceStamped init_pose_;
geometry_msgs::msg::PoseStamped goal_pose_;
- double entity_search_radius_;
// Subscribers
rclcpp::Subscription<Odometry>::SharedPtr sub_kinematics_;
@@ -230,39 +143,17 @@ class ReactionAnalyzerNode : public rclcpp::Node
tf2_ros::TransformListener tf_listener_{tf_buffer_};
// Variables
- std::vector<rclcpp::SubscriptionBase::SharedPtr> subscribers_;
- std::unordered_map<std::string, BufferVariant> message_buffers_;
- std::unique_ptr<message_filters::Subscriber<PublishedTime>> time_debug_sub_;
- std::unordered_map<std::string, std::vector<PublishedTime>> published_time_vector_map_;
- // std::unordered_map<std::string, PublishedTimeSubscriber> published_time_subscriber_map_;
-
std::unordered_map<std::string, std::vector<double>> test_results_;
-
std::optional<rclcpp::Time> last_test_environment_init_request_time_;
std::optional<rclcpp::Time> test_environment_init_time_;
std::optional<rclcpp::Time> spawn_cmd_time_;
std::atomic<bool> spawn_object_cmd_{false};
std::atomic<bool> is_object_spawned_message_published_{false};
- bool is_output_printed_{false};
bool is_vehicle_initialized_{false};
bool is_route_set_{false};
size_t test_iteration_count_{0};
// Functions
- rclcpp::SubscriptionOptions createSubscriptionOptions();
-
- bool searchPointcloudNearEntity(const pcl::PointCloud<pcl::PointXYZ> & pcl_pointcloud);
-
- bool searchPredictedObjectsNearEntity(const PredictedObjects & predicted_objects);
-
- bool searchDetectedObjectsNearEntity(const DetectedObjects & detected_objects);
-
- bool searchTrackedObjectsNearEntity(const TrackedObjects & tracked_objects);
-
- bool loadChainModules();
-
- bool initSubscribers(const reaction_analyzer::ChainModules & modules);
-
void initAnalyzerVariables();
void initPointcloud();
@@ -274,27 +165,16 @@ class ReactionAnalyzerNode : public rclcpp::Node
const RouteState::ConstSharedPtr & route_state_ptr,
const OperationModeState::ConstSharedPtr & operation_mode_ptr);
- void pushPublishedTime(
- std::vector<PublishedTime> & published_time_vec, const PublishedTime & published_time);
-
- void setControlCommandToBuffer(
- std::vector<AckermannControlCommand> & buffer, const AckermannControlCommand & cmd);
-
- std::optional<size_t> findFirstBrakeIdx(
- const std::vector<AckermannControlCommand> & cmd_array,
- const std::optional<rclcpp::Time> & spawn_cmd_time);
+ void callOperationModeServiceWithoutResponse();
void spawnObstacle(const geometry_msgs::msg::Point & ego_pose);
void calculateResults(
- const std::unordered_map<std::string, BufferVariant> & message_buffers,
- const std::unordered_map<std::string, std::vector<PublishedTime>> & published_time_vector_map,
+ const std::unordered_map<std::string, subscriber::BufferVariant> & message_buffers,
const rclcpp::Time & spawn_cmd_time);
void onTimer();
- bool allReacted(const std::unordered_map<std::string, BufferVariant> & message_buffers);
-
void dummyPerceptionPublisher();
void reset();
@@ -310,52 +190,6 @@ class ReactionAnalyzerNode : public rclcpp::Node
void operationModeCallback(OperationModeState::ConstSharedPtr msg_ptr);
- // Callbacks for modules are subscribed
- void controlCommandOutputCallback(
- const std::string & node_name, const AckermannControlCommand::ConstSharedPtr & msg_ptr);
-
- // void controlCommandOutputCallback(
- // const std::string & node_name, const AckermannControlCommand::ConstSharedPtr & msg_ptr,
- // const PublishedTime::ConstSharedPtr & published_time_ptr);
-
- void trajectoryOutputCallback(
- const std::string & node_name, const Trajectory::ConstSharedPtr & msg_ptr);
-
- // void trajectoryOutputCallback(
- // const std::string & node_name, const Trajectory::ConstSharedPtr & msg_ptr,
- // const PublishedTime::ConstSharedPtr & published_time_ptr);
-
- void pointcloud2OutputCallback(
- const std::string & node_name, const PointCloud2::ConstSharedPtr & msg_ptr);
-
- // void pointcloud2OutputCallback(
- // const std::string & node_name, const PointCloud2::ConstSharedPtr & msg_ptr,
- // const PublishedTime::ConstSharedPtr & published_time_ptr);
-
- void predictedObjectsOutputCallback(
- const std::string & node_name, const PredictedObjects::ConstSharedPtr & msg_ptr);
-
- // void predictedObjectsOutputCallback(
- // const std::string & node_name, const PredictedObjects::ConstSharedPtr & msg_ptr,
- // const PublishedTime::ConstSharedPtr & published_time_ptr);
-
- void detectedObjectsOutputCallback(
- const std::string & node_name, const DetectedObjects::ConstSharedPtr & msg_ptr);
-
- // void detectedObjectsOutputCallback(
- // const std::string & node_name, const DetectedObjects::ConstSharedPtr & msg_ptr,
- // const PublishedTime::ConstSharedPtr & published_time_ptr);
-
- void trackedObjectsOutputCallback(
- const std::string & node_name, const TrackedObjects::ConstSharedPtr & msg_ptr);
-
- // void trackedObjectsOutputCallback(
- // const std::string & node_name, const TrackedObjects::ConstSharedPtr & msg_ptr,
- // const PublishedTime::ConstSharedPtr & published_time_ptr);
-
- void publishedTimeOutputCallback(
- const std::string & node_name, const PublishedTime::ConstSharedPtr & msg_ptr);
-
// Timer
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::TimerBase::SharedPtr dummy_perception_timer_;
@@ -363,18 +197,15 @@ class ReactionAnalyzerNode : public rclcpp::Node
// Client
rclcpp::Client<ChangeOperationMode>::SharedPtr client_change_to_autonomous_;
- void callOperationModeServiceWithoutResponse();
-
// Pointers
std::shared_ptr<topic_publisher::TopicPublisher> topic_publisher_ptr_;
+ std::unique_ptr<subscriber::SubscriberBase> subscriber_ptr_;
PointCloud2::SharedPtr entity_pointcloud_ptr_;
PredictedObjects::SharedPtr predicted_objects_ptr_;
- Odometry::ConstSharedPtr odometry_;
LocalizationInitializationState::ConstSharedPtr initialization_state_ptr_;
RouteState::ConstSharedPtr current_route_state_ptr_;
OperationModeState::ConstSharedPtr operation_mode_ptr_;
};
-
} // namespace reaction_analyzer
#endif // REACTION_ANALYZER_NODE_HPP_
diff --git a/tools/reaction_analyzer/include/subscriber.hpp b/tools/reaction_analyzer/include/subscriber.hpp
new file mode 100644
index 0000000000000..5d9eba0643e1d
--- /dev/null
+++ b/tools/reaction_analyzer/include/subscriber.hpp
@@ -0,0 +1,248 @@
+// Copyright 2024 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SUBSCRIBER_HPP_
+#define SUBSCRIBER_HPP_
+#include <motion_utils/trajectory/trajectory.hpp>
+#include <pcl/impl/point_types.hpp>
+#include <pcl_ros/transforms.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <tf2_eigen/tf2_eigen/tf2_eigen.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+#include <tier4_autoware_utils/math/unit_conversion.hpp>
+#include <utils.hpp>
+
+#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
+#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
+#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
+#include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
+#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+#include <autoware_internal_msgs/msg/published_time.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <message_filters/cache.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/synchronizer.h>
+#include <pcl/common/distances.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <memory>
+#include <mutex>
+#include <utility>
+#include <variant>
+
+namespace reaction_analyzer::subscriber
+{
+using autoware_auto_control_msgs::msg::AckermannControlCommand;
+using autoware_auto_perception_msgs::msg::DetectedObject;
+using autoware_auto_perception_msgs::msg::DetectedObjects;
+using autoware_auto_perception_msgs::msg::PredictedObject;
+using autoware_auto_perception_msgs::msg::PredictedObjects;
+using autoware_auto_perception_msgs::msg::TrackedObject;
+using autoware_auto_perception_msgs::msg::TrackedObjects;
+using autoware_auto_planning_msgs::msg::Trajectory;
+using autoware_internal_msgs::msg::PublishedTime;
+using geometry_msgs::msg::Pose;
+using nav_msgs::msg::Odometry;
+using sensor_msgs::msg::PointCloud2;
+
+// Buffers to be used to store subscribed messages
+using ControlCommandBuffer =
+ std::pair<std::vector<AckermannControlCommand>, std::optional<AckermannControlCommand>>;
+using TrajectoryBuffer = std::optional<Trajectory>;
+using PointCloud2Buffer = std::optional<PointCloud2>;
+using PredictedObjectsBuffer = std::optional<PredictedObjects>;
+using DetectedObjectsBuffer = std::optional<DetectedObjects>;
+using TrackedObjectsBuffer = std::optional<TrackedObjects>;
+
+// Variant to store different types of buffers
+using BufferVariant = std::variant<
+ ControlCommandBuffer, TrajectoryBuffer, PointCloud2Buffer, PredictedObjectsBuffer,
+ DetectedObjectsBuffer, TrackedObjectsBuffer>;
+
+template <typename MessageType>
+struct SubscriberVariables
+{
+ using ExactTimePolicy = message_filters::sync_policies::ExactTime<MessageType, PublishedTime>;
+
+ std::unique_ptr<message_filters::Subscriber<MessageType>> sub1_;
+ std::unique_ptr<message_filters::Subscriber<PublishedTime>> sub2_;
+ std::unique_ptr<message_filters::Synchronizer<ExactTimePolicy>> synchronizer_;
+ // tmp: only for the messages who don't have header e.g. AckermannControlCommand
+ std::unique_ptr<message_filters::Cache<PublishedTime>> cache_;
+};
+
+// Variant to create subscribers for different message types
+using SubscriberVariablesVariant = std::variant<
+ SubscriberVariables<PointCloud2>, SubscriberVariables<DetectedObjects>,
+ SubscriberVariables<TrackedObjects>, SubscriberVariables<PredictedObjects>,
+ SubscriberVariables<Trajectory>, SubscriberVariables<AckermannControlCommand>>;
+
+// The supported message types
+enum class SubscriberMessageType {
+ UNKNOWN = 0,
+ ACKERMANN_CONTROL_COMMAND = 1,
+ TRAJECTORY = 2,
+ POINTCLOUD2 = 3,
+ DETECTED_OBJECTS = 4,
+ PREDICTED_OBJECTS = 5,
+ TRACKED_OBJECTS = 6,
+};
+
+// Reaction Types
+// TODO: For the future work, we can add more reaction types e.g. first shifted trajectory.
+enum class ReactionType {
+ UNKNOWN = 0,
+ FIRST_BRAKE = 1,
+ SEARCH_ZERO_VEL = 2,
+ SEARCH_ENTITY = 3,
+};
+
+// The configuration of the topic to be subscribed which are defined in reaction_chain
+struct TopicConfig
+{
+ std::string node_name;
+ std::string topic_address;
+ std::string time_debug_topic_address;
+ SubscriberMessageType message_type;
+};
+
+// Place for the reaction functions' parameter configuration
+struct FirstBrakeParams
+{
+ bool debug_control_commands;
+ double control_cmd_buffer_time_interval;
+ size_t min_number_descending_order_control_cmd;
+ double min_jerk_for_brake_cmd;
+};
+
+struct SearchZeroVelParams
+{
+ double max_looking_distance;
+};
+
+struct SearchEntityParams
+{
+ double search_radius;
+};
+
+// Place for the store the reaction parameter configuration (currently only for first brake)
+struct ReactionParams
+{
+ FirstBrakeParams first_brake_params;
+ SearchZeroVelParams search_zero_vel_params;
+ SearchEntityParams search_entity_params;
+};
+
+using ChainModules = std::vector<TopicConfig>;
+
+class SubscriberBase
+{
+public:
+ explicit SubscriberBase(
+ rclcpp::Node * node, Odometry::ConstSharedPtr & odometry, geometry_msgs::msg::Pose entity_pose,
+ std::atomic<bool> & spawn_object_cmd);
+
+ ~SubscriberBase() = default;
+
+ std::optional<std::unordered_map<std::string, BufferVariant>> getMessageBuffersMap();
+ void reset();
+
+private:
+ std::mutex mutex_;
+
+ rclcpp::Node * node_;
+ Odometry::ConstSharedPtr odometry_;
+ geometry_msgs::msg::Pose entity_pose_;
+ std::atomic<bool> & spawn_object_cmd_;
+
+ // Variables to be initialized in constructor
+ ChainModules chain_modules_;
+ ReactionParams reaction_params_{};
+
+ // Variants
+ std::unordered_map<std::string, SubscriberVariablesVariant> subscriber_variables_map_;
+ std::unordered_map<std::string, BufferVariant> message_buffers_;
+
+ // tf
+ std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+ std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+ // Functions
+ void initReactionChainsAndParams();
+
+ bool initSubscribers();
+
+ bool searchPointcloudNearEntity(const pcl::PointCloud<pcl::PointXYZ> & pcl_pointcloud);
+
+ bool searchPredictedObjectsNearEntity(const PredictedObjects & predicted_objects);
+
+ bool searchDetectedObjectsNearEntity(const DetectedObjects & detected_objects);
+
+ bool searchTrackedObjectsNearEntity(const TrackedObjects & tracked_objects);
+
+ void setControlCommandToBuffer(
+ std::vector<AckermannControlCommand> & buffer, const AckermannControlCommand & cmd);
+
+ std::optional<size_t> findFirstBrakeIdx(const std::vector<AckermannControlCommand> & cmd_array);
+
+ // Callbacks for modules are subscribed
+ void controlCommandOutputCallback(
+ const std::string & node_name, const AckermannControlCommand::ConstSharedPtr & msg_ptr);
+
+ void trajectoryOutputCallback(
+ const std::string & node_name, const Trajectory::ConstSharedPtr & msg_ptr);
+
+ void trajectoryOutputCallback(
+ const std::string & node_name, const Trajectory::ConstSharedPtr & msg_ptr,
+ const PublishedTime::ConstSharedPtr & published_time_ptr);
+
+ void pointcloud2OutputCallback(
+ const std::string & node_name, const PointCloud2::ConstSharedPtr & msg_ptr);
+
+ void pointcloud2OutputCallback(
+ const std::string & node_name, const PointCloud2::ConstSharedPtr & msg_ptr,
+ const PublishedTime::ConstSharedPtr & published_time_ptr);
+
+ void predictedObjectsOutputCallback(
+ const std::string & node_name, const PredictedObjects::ConstSharedPtr & msg_ptr);
+
+ void predictedObjectsOutputCallback(
+ const std::string & node_name, const PredictedObjects::ConstSharedPtr & msg_ptr,
+ const PublishedTime::ConstSharedPtr & published_time_ptr);
+
+ void detectedObjectsOutputCallback(
+ const std::string & node_name, const DetectedObjects::ConstSharedPtr & msg_ptr);
+
+ void detectedObjectsOutputCallback(
+ const std::string & node_name, const DetectedObjects::ConstSharedPtr & msg_ptr,
+ const PublishedTime::ConstSharedPtr & published_time_ptr);
+
+ void trackedObjectsOutputCallback(
+ const std::string & node_name, const TrackedObjects::ConstSharedPtr & msg_ptr);
+
+ void trackedObjectsOutputCallback(
+ const std::string & node_name, const TrackedObjects::ConstSharedPtr & msg_ptr,
+ const PublishedTime::ConstSharedPtr & published_time_ptr);
+};
+
+} // namespace reaction_analyzer::subscriber
+
+#endif // SUBSCRIBER_HPP_
diff --git a/tools/reaction_analyzer/include/topic_publisher.hpp b/tools/reaction_analyzer/include/topic_publisher.hpp
index 19f0f6ce74d85..939abab591d84 100644
--- a/tools/reaction_analyzer/include/topic_publisher.hpp
+++ b/tools/reaction_analyzer/include/topic_publisher.hpp
@@ -17,6 +17,7 @@
#include <rclcpp/rclcpp.hpp>
#include <rosbag2_cpp/reader.hpp>
+#include <utils.hpp>
#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
@@ -69,18 +70,21 @@ enum class PublisherMessageType {
struct TopicPublisherParams
{
- std::string path_bag_without_object;
- std::string path_bag_with_object;
- std::string pointcloud_publisher_type;
- double pointcloud_publisher_period;
+ std::string path_bag_without_object; // Path to the bag file without object
+ std::string path_bag_with_object; // Path to the bag file with object
+ std::string pointcloud_publisher_type; // Type of the pointcloud publisher
+ double pointcloud_publisher_period; // Period of the pointcloud publisher
};
enum class PointcloudPublisherType {
- AsyncPublisher = 0,
- SyncHeaderSyncPublisher = 1,
- AsyncHeaderSyncPublisher = 2,
+ AsyncPublisher = 0, // Asynchronous publisher
+ SyncHeaderSyncPublisher = 1, // Synchronous publisher with header synchronization
+ AsyncHeaderSyncPublisher = 2, // Asynchronous publisher with header synchronization
};
+/**
+ * @brief Message type template struct for the variables of the Publisher.
+ */
template <typename MessageType>
struct PublisherVariables
{
@@ -91,8 +95,12 @@ struct PublisherVariables
rclcpp::TimerBase::SharedPtr timer;
};
+/**
+ * @brief Struct for accessing the variables of the Publisher.
+ */
struct PublisherVarAccessor
{
+ // Template struct to check if a type has a header member.
template <typename T, typename = std::void_t<>>
struct has_header : std::false_type
{
@@ -103,6 +111,7 @@ struct PublisherVarAccessor
{
};
+ // Template struct to check if a type has a stamp member.
template <typename T, typename = std::void_t<>>
struct has_stamp : std::false_type
{
@@ -133,28 +142,24 @@ struct PublisherVarAccessor
publisherVar.publisher->publish(std::move(msg_to_be_published));
}
- // Set Period
template <typename T>
void setPeriod(T & publisherVar, double newPeriod)
{
publisherVar.period_ns = newPeriod;
}
- // Get Period
template <typename T>
double getPeriod(const T & publisherVar) const
{
return publisherVar.period_ns;
}
- // Get Empty Area Message
template <typename T>
std::shared_ptr<void> getEmptyAreaMessage(const T & publisherVar) const
{
return std::static_pointer_cast<void>(publisherVar.empty_area_message);
}
- // Get Object Spawned Message
template <typename T>
std::shared_ptr<void> getObjectSpawnedMessage(const T & publisherVar) const
{
@@ -174,6 +179,10 @@ using PublisherVariablesVariant = std::variant<
PublisherVariables<autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport>,
PublisherVariables<autoware_auto_vehicle_msgs::msg::VelocityReport>>;
+using LidarOutputPair = std::pair<
+ std::shared_ptr<PublisherVariables<PointCloud2>>,
+ std::shared_ptr<PublisherVariables<PointCloud2>>>;
+
class TopicPublisher
{
public:
@@ -187,7 +196,7 @@ class TopicPublisher
private:
std::mutex mutex_;
- // Initialize
+ // Initialized variables
rclcpp::Node * node_;
std::atomic<bool> & spawn_object_cmd_;
std::optional<rclcpp::Time> & spawn_cmd_time_;
@@ -207,10 +216,7 @@ class TopicPublisher
// Variables
PointcloudPublisherType pointcloud_publisher_type_;
std::unordered_map<std::string, PublisherVariablesVariant> topic_publisher_map_;
- std::unordered_map<
- std::string, std::pair<
- std::shared_ptr<PublisherVariables<PointCloud2>>,
- std::shared_ptr<PublisherVariables<PointCloud2>>>>
+ std::unordered_map<std::string, LidarOutputPair>
lidar_pub_variable_pair_map_; // used to publish pointcloud_raw and pointcloud_raw_ex
bool is_object_spawned_message_published{false};
std::shared_ptr<rclcpp::TimerBase> one_shot_timer_shared_ptr_;
@@ -219,7 +225,6 @@ class TopicPublisher
std::unordered_map<std::string, rclcpp::TimerBase::SharedPtr> pointcloud_publish_timers_map_;
rclcpp::TimerBase::SharedPtr pointcloud_sync_publish_timer_;
};
-
} // namespace reaction_analyzer::topic_publisher
#endif // TOPIC_PUBLISHER_HPP_
diff --git a/tools/reaction_analyzer/include/utils.hpp b/tools/reaction_analyzer/include/utils.hpp
new file mode 100644
index 0000000000000..70fc39c73906d
--- /dev/null
+++ b/tools/reaction_analyzer/include/utils.hpp
@@ -0,0 +1,75 @@
+// Copyright 2024 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef UTILS_HPP_
+#define UTILS_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+
+#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+
+#include <string>
+
+// The reaction_analyzer namespace contains utility functions for the Reaction Analyzer tool.
+namespace reaction_analyzer
+{
+using autoware_auto_planning_msgs::msg::Trajectory;
+using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+
+/**
+ * @brief Sorts the test results by their median value.
+ *
+ * @param test_results An unordered map containing the test results.
+ * @return A vector of tuples. Each tuple contains a string (the test name), a vector of doubles
+ * (the test results), and a double (the median value).
+ */
+std::vector<std::tuple<std::string, std::vector<double>, double>> sortResultsByMedian(
+ const std::unordered_map<std::string, std::vector<double>> test_results);
+
+/**
+ * @brief Creates a subscription option with a new callback group.
+ *
+ * @param node A pointer to the node for which the subscription options are being created.
+ * @return The created SubscriptionOptions.
+ */
+rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node);
+
+/**
+ * @brief Splits a string by a given delimiter.
+ *
+ * @param str The string to be split.
+ * @param delimiter The delimiter to split the string by.
+ * @return A vector of strings, each of which is a segment of the original string split by the
+ * delimiter.
+ */
+std::vector<std::string> split(const std::string & str, char delimiter);
+
+/**
+ * @brief Get the index of the trajectory point that is a certain distance away from the current
+ * point.
+ *
+ * This function iterates over the trajectory from the current point and returns the index of the
+ * first point that is at least the specified distance away.
+ *
+ * @param traj The trajectory to search.
+ * @param curr_id The index of the current point in the trajectory.
+ * @param distance The distance to search for a point.
+ * @return The index of the point in the trajectory that is at least the specified distance away
+ * from the current point.
+ */
+size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, const double distance);
+} // namespace reaction_analyzer
+
+#endif // UTILS_HPP_
diff --git a/tools/reaction_analyzer/package.xml b/tools/reaction_analyzer/package.xml
index faf5797d00433..05081eac01751 100644
--- a/tools/reaction_analyzer/package.xml
+++ b/tools/reaction_analyzer/package.xml
@@ -3,7 +3,7 @@
<package format="3">
<name>reaction_analyzer</name>
<version>1.0.0</version>
- <description>Nodes to measure reaction times of the nodes</description>
+ <description>Analyzer that measures reaction times of the nodes</description>
<maintainer email="berkay@leodrive.ai">Berkay Karaman</maintainer>
diff --git a/tools/reaction_analyzer/param/reaction_analyzer.param.yaml b/tools/reaction_analyzer/param/reaction_analyzer.param.yaml
index 37ef3bba86e38..082cdaf864265 100644
--- a/tools/reaction_analyzer/param/reaction_analyzer.param.yaml
+++ b/tools/reaction_analyzer/param/reaction_analyzer.param.yaml
@@ -1,19 +1,13 @@
/**:
ros__parameters:
timer_period: 0.033 # s
- published_time_expire_duration: 5.0 # s
test_iteration: 15
output_file_path: /home/berkay/projects/reaction_analyzer_results/
object_search_radius_offset: 0.1 # m
- spawn_time_after_init: 5.0 # s
- spawn_distance_threshold: 15.0 # m
+ spawn_time_after_init: 5.0 # s for perception_planning mode
+ spawn_distance_threshold: 15.0 # m # for planning_control mode
spawned_pointcloud_sampling_distance: 0.1
dummy_perception_publisher_period: 0.1 # s
- first_brake_params:
- debug_control_commands: false
- control_cmd_buffer_time_interval: 1.0 # s
- min_number_descending_order_control_cmd: 3
- min_jerk_for_brake_cmd: 0.3 # m/s^3
initialization_pose:
x: 81247.9609375
y: 49828.87890625
@@ -44,68 +38,79 @@
pointcloud_publisher:
pointcloud_publisher_type: "sync_header_sync_publish" # "async_header_sync_publish", "sync_header_sync_publish" or "async_publish"
pointcloud_publisher_period: 0.1 # s
- reaction_chain:
-# 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
-# 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
-# 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
-# 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
-# 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
-# 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
-# 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
-# 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
-# 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
-# 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
- 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
- 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
- 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
- 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
- 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
- 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
+ subscriber:
+ reaction_params:
+ first_brake_params:
+ debug_control_commands: false
+ control_cmd_buffer_time_interval: 1.0 # s
+ min_number_descending_order_control_cmd: 3
+ min_jerk_for_brake_cmd: 0.3 # m/s^3
+ search_zero_vel_params:
+ max_looking_distance: 15.0 # m
+ search_entity_params:
+ search_radius: 5.0 # m
+ reaction_chain:
+ 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
+ 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
+ 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
+ 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
+# 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
+# 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
+ 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
+ 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
+ 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
+ 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
+ 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
+ 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
+ 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
+ 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
+ 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
+ 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
diff --git a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp
index aa6cad7ee7623..95ac106375897 100644
--- a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp
+++ b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp
@@ -14,244 +14,12 @@
#include "reaction_analyzer_node.hpp"
-#include "tf2/transform_datatypes.h"
-
-#include <pcl/impl/point_types.hpp>
-#include <pcl_ros/transforms.hpp>
-#include <tf2_eigen/tf2_eigen/tf2_eigen.hpp>
-
-#include <boost/uuid/string_generator.hpp>
-#include <boost/uuid/uuid.hpp>
-#include <boost/uuid/uuid_generators.hpp>
-#include <boost/uuid/uuid_io.hpp>
-
-#include <message_filters/sync_policies/approximate_time.h>
-
-#include <algorithm>
+#include <functional>
#include <memory>
namespace reaction_analyzer
{
-// Callbacks
-
-void ReactionAnalyzerNode::publishedTimeOutputCallback(
- const std::string & node_name, const PublishedTime::ConstSharedPtr & msg_ptr)
-{
- std::lock_guard<std::mutex> lock(mutex_);
- auto & published_time_vector = published_time_vector_map_[node_name];
- pushPublishedTime(published_time_vector, *msg_ptr);
-}
-
-void ReactionAnalyzerNode::controlCommandOutputCallback(
- const std::string & node_name, const AckermannControlCommand::ConstSharedPtr & msg_ptr)
-{
- std::lock_guard<std::mutex> lock(mutex_);
- auto & variant = message_buffers_[node_name];
- const auto is_spawned = spawn_cmd_time_;
- if (!std::holds_alternative<ControlCommandBuffer>(variant)) {
- ControlCommandBuffer buffer(std::vector<AckermannControlCommand>{*msg_ptr}, std::nullopt);
- variant = buffer;
- }
-
- if (std::get<ControlCommandBuffer>(variant).second) {
- // reacted
- return;
- }
- setControlCommandToBuffer(std::get<ControlCommandBuffer>(variant).first, *msg_ptr);
- const auto brake_idx =
- findFirstBrakeIdx(std::get<ControlCommandBuffer>(variant).first, is_spawned);
- if (brake_idx) {
- std::get<ControlCommandBuffer>(variant).second =
- std::get<ControlCommandBuffer>(variant).first.at(brake_idx.value());
- RCLCPP_INFO(this->get_logger(), "Reacted %s", node_name.c_str());
- }
-}
-
-void ReactionAnalyzerNode::trajectoryOutputCallback(
- const std::string & node_name, const Trajectory::ConstSharedPtr & msg_ptr)
-{
- mutex_.lock();
- auto variant = message_buffers_[node_name];
- const auto current_odometry_ptr = odometry_;
- const auto is_spawned = spawn_cmd_time_;
- if (!std::holds_alternative<TrajectoryBuffer>(variant)) {
- TrajectoryBuffer buffer(std::nullopt);
- variant = buffer;
- message_buffers_[node_name] = variant;
- }
- mutex_.unlock();
-
- if (!current_odometry_ptr || !is_spawned || std::get<TrajectoryBuffer>(variant).has_value()) {
- return;
- }
-
- const auto nearest_seg_idx = motion_utils::findNearestSegmentIndex(
- msg_ptr->points, current_odometry_ptr->pose.pose.position);
-
- const auto nearest_objects_seg_idx =
- motion_utils::findNearestIndex(msg_ptr->points, entity_pose_.position);
-
- const auto zero_vel_idx = motion_utils::searchZeroVelocityIndex(
- msg_ptr->points, nearest_seg_idx, nearest_objects_seg_idx);
-
- if (zero_vel_idx) {
- std::get<TrajectoryBuffer>(variant) = *msg_ptr;
- RCLCPP_INFO(this->get_logger(), "Reacted %s", node_name.c_str());
- mutex_.lock();
- message_buffers_[node_name] = variant;
- mutex_.unlock();
- }
-}
-
-void ReactionAnalyzerNode::pointcloud2OutputCallback(
- const std::string & node_name, const PointCloud2::ConstSharedPtr & msg_ptr)
-{
- mutex_.lock();
- auto variant = message_buffers_[node_name];
- const auto is_spawned = spawn_cmd_time_;
- if (!std::holds_alternative<PointCloud2Buffer>(variant)) {
- PointCloud2Buffer buffer(std::nullopt);
- variant = buffer;
- message_buffers_[node_name] = variant;
- }
- mutex_.unlock();
-
- if (!is_spawned || std::get<PointCloud2Buffer>(variant).has_value()) {
- return;
- }
-
- // transform pointcloud
- geometry_msgs::msg::TransformStamped transform_stamped{};
- try {
- transform_stamped = tf_buffer_.lookupTransform(
- "map", msg_ptr->header.frame_id, this->now(), rclcpp::Duration::from_seconds(0.5));
- } catch (tf2::TransformException & ex) {
- RCLCPP_ERROR_STREAM(
- get_logger(), "Failed to look up transform from " << msg_ptr->header.frame_id << " to map");
- return;
- }
-
- // transform by using eigen matrix
- PointCloud2 transformed_points{};
- const Eigen::Matrix4f affine_matrix =
- tf2::transformToEigen(transform_stamped.transform).matrix().cast<float>();
- pcl_ros::transformPointCloud(affine_matrix, *msg_ptr, transformed_points);
-
- pcl::PointCloud<pcl::PointXYZ> pcl_pointcloud;
- pcl::fromROSMsg(transformed_points, pcl_pointcloud);
-
- if (searchPointcloudNearEntity(pcl_pointcloud)) {
- std::get<PointCloud2Buffer>(variant) = *msg_ptr;
- RCLCPP_INFO(this->get_logger(), "Reacted %s", node_name.c_str());
- mutex_.lock();
- message_buffers_[node_name] = variant;
- mutex_.unlock();
- }
-}
-
-void ReactionAnalyzerNode::predictedObjectsOutputCallback(
- const std::string & node_name, const PredictedObjects::ConstSharedPtr & msg_ptr)
-{
- mutex_.lock();
- auto variant = message_buffers_[node_name];
- const auto is_spawned = spawn_cmd_time_;
- if (!std::holds_alternative<PredictedObjectsBuffer>(variant)) {
- PredictedObjectsBuffer buffer(std::nullopt);
- variant = buffer;
- message_buffers_[node_name] = variant;
- }
- mutex_.unlock();
-
- if (
- !is_spawned || msg_ptr->objects.empty() ||
- std::get<PredictedObjectsBuffer>(variant).has_value()) {
- return;
- }
-
- if (searchPredictedObjectsNearEntity(*msg_ptr)) {
- std::get<PredictedObjectsBuffer>(variant) = *msg_ptr;
- RCLCPP_INFO(this->get_logger(), "Reacted %s", node_name.c_str());
-
- mutex_.lock();
- message_buffers_[node_name] = variant;
- mutex_.unlock();
- }
-}
-
-void ReactionAnalyzerNode::detectedObjectsOutputCallback(
- const std::string & node_name, const DetectedObjects::ConstSharedPtr & msg_ptr)
-{
- mutex_.lock();
- auto variant = message_buffers_[node_name];
- const auto is_spawned = spawn_cmd_time_;
- if (!std::holds_alternative<DetectedObjectsBuffer>(variant)) {
- DetectedObjectsBuffer buffer(std::nullopt);
- variant = buffer;
- message_buffers_[node_name] = variant;
- }
- mutex_.unlock();
- if (
- !is_spawned || msg_ptr->objects.empty() ||
- std::get<DetectedObjectsBuffer>(variant).has_value()) {
- return;
- }
-
- // transform objects
- geometry_msgs::msg::TransformStamped transform_stamped{};
- try {
- transform_stamped = tf_buffer_.lookupTransform(
- "map", msg_ptr->header.frame_id, this->now(), rclcpp::Duration::from_seconds(0.5));
- } catch (tf2::TransformException & ex) {
- RCLCPP_ERROR_STREAM(
- get_logger(), "Failed to look up transform from " << msg_ptr->header.frame_id << " to map");
- return;
- }
-
- DetectedObjects output_objs;
- output_objs = *msg_ptr;
- for (auto & obj : output_objs.objects) {
- geometry_msgs::msg::PoseStamped output_stamped, input_stamped;
- input_stamped.pose = obj.kinematics.pose_with_covariance.pose;
- tf2::doTransform(input_stamped, output_stamped, transform_stamped);
- obj.kinematics.pose_with_covariance.pose = output_stamped.pose;
- }
- if (searchDetectedObjectsNearEntity(output_objs)) {
- std::get<DetectedObjectsBuffer>(variant) = *msg_ptr;
- RCLCPP_INFO(this->get_logger(), "Reacted %s", node_name.c_str());
- mutex_.lock();
- message_buffers_[node_name] = variant;
- mutex_.unlock();
- }
-}
-
-void ReactionAnalyzerNode::trackedObjectsOutputCallback(
- const std::string & node_name, const TrackedObjects::ConstSharedPtr & msg_ptr)
-{
- mutex_.lock();
- auto variant = message_buffers_[node_name];
- const auto is_spawned = spawn_cmd_time_;
- if (!std::holds_alternative<TrackedObjectsBuffer>(variant)) {
- TrackedObjectsBuffer buffer(std::nullopt);
- variant = buffer;
- message_buffers_[node_name] = variant;
- }
- mutex_.unlock();
- if (
- !is_spawned || msg_ptr->objects.empty() ||
- std::get<TrackedObjectsBuffer>(variant).has_value()) {
- return;
- }
-
- if (searchTrackedObjectsNearEntity(*msg_ptr)) {
- std::get<TrackedObjectsBuffer>(variant) = *msg_ptr;
- RCLCPP_INFO(this->get_logger(), "Reacted %s", node_name.c_str());
- mutex_.lock();
- message_buffers_[node_name] = variant;
- mutex_.unlock();
- }
-}
-
void ReactionAnalyzerNode::operationModeCallback(OperationModeState::ConstSharedPtr msg_ptr)
{
std::lock_guard<std::mutex> lock(mutex_);
@@ -267,7 +35,7 @@ void ReactionAnalyzerNode::routeStateCallback(RouteState::ConstSharedPtr msg_ptr
void ReactionAnalyzerNode::vehiclePoseCallback(Odometry::ConstSharedPtr msg_ptr)
{
std::lock_guard<std::mutex> lock(mutex_);
- odometry_ = std::move(msg_ptr);
+ odometry_ = msg_ptr;
}
void ReactionAnalyzerNode::initializationStateCallback(
@@ -296,25 +64,14 @@ ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions options)
node_params_.timer_period = get_parameter("timer_period").as_double();
node_params_.test_iteration = get_parameter("test_iteration").as_int();
- node_params_.published_time_expire_duration =
- get_parameter("published_time_expire_duration").as_double();
node_params_.output_file_path = get_parameter("output_file_path").as_string();
- node_params_.object_search_radius_offset =
- get_parameter("object_search_radius_offset").as_double();
+
node_params_.spawn_time_after_init = get_parameter("spawn_time_after_init").as_double();
node_params_.spawn_distance_threshold = get_parameter("spawn_distance_threshold").as_double();
node_params_.spawned_pointcloud_sampling_distance =
get_parameter("spawned_pointcloud_sampling_distance").as_double();
node_params_.dummy_perception_publisher_period =
get_parameter("dummy_perception_publisher_period").as_double();
- node_params_.debug_control_commands =
- get_parameter("first_brake_params.debug_control_commands").as_bool();
- node_params_.control_cmd_buffer_time_interval =
- get_parameter("first_brake_params.control_cmd_buffer_time_interval").as_double();
- node_params_.min_number_descending_order_control_cmd =
- get_parameter("first_brake_params.min_number_descending_order_control_cmd").as_int();
- node_params_.min_jerk_for_brake_cmd =
- get_parameter("first_brake_params.min_jerk_for_brake_cmd").as_double();
// Position parameters
node_params_.initial_pose.x = get_parameter("initialization_pose.x").as_double();
@@ -341,31 +98,26 @@ ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions options)
node_params_.entity_params.y_l = get_parameter("entity_params.y_dimension").as_double();
node_params_.entity_params.z_l = get_parameter("entity_params.z_dimension").as_double();
- // initialize the reaction chain
- if (!loadChainModules()) {
- RCLCPP_ERROR(
- get_logger(), "Modules in chain are invalid. Node couldn't be initialized. Failed.");
- return;
- }
-
- initAnalyzerVariables();
-
sub_kinematics_ = create_subscription<Odometry>(
"input/kinematics", 1, std::bind(&ReactionAnalyzerNode::vehiclePoseCallback, this, _1),
- createSubscriptionOptions());
+ createSubscriptionOptions(this));
sub_localization_init_state_ = create_subscription<LocalizationInitializationState>(
"input/localization_initialization_state", rclcpp::QoS(1).transient_local(),
std::bind(&ReactionAnalyzerNode::initializationStateCallback, this, _1),
- createSubscriptionOptions());
+ createSubscriptionOptions(this));
sub_route_state_ = create_subscription<RouteState>(
"input/routing_state", rclcpp::QoS{1}.transient_local(),
- std::bind(&ReactionAnalyzerNode::routeStateCallback, this, _1), createSubscriptionOptions());
+ std::bind(&ReactionAnalyzerNode::routeStateCallback, this, _1),
+ createSubscriptionOptions(this));
sub_operation_mode_ = create_subscription<OperationModeState>(
"input/operation_mode_state", rclcpp::QoS{1}.transient_local(),
- std::bind(&ReactionAnalyzerNode::operationModeCallback, this, _1), createSubscriptionOptions());
+ std::bind(&ReactionAnalyzerNode::operationModeCallback, this, _1),
+ createSubscriptionOptions(this));
pub_goal_pose_ = create_publisher<geometry_msgs::msg::PoseStamped>("output/goal", rclcpp::QoS(1));
+ initAnalyzerVariables();
+
if (node_running_mode_ == RunningMode::PlanningControl) {
pub_initial_pose_ = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
"output/initialpose", rclcpp::QoS(1));
@@ -388,6 +140,11 @@ ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions options)
std::make_shared<topic_publisher::TopicPublisher>(this, spawn_object_cmd_, spawn_cmd_time_);
}
+ // Load the subscriber to listen the topics for reactions
+ odometry_ = std::make_shared<Odometry>(); // initialize the odometry before init the subscriber
+ subscriber_ptr_ =
+ std::make_unique<subscriber::SubscriberBase>(this, odometry_, entity_pose_, spawn_object_cmd_);
+
const auto period_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(node_params_.timer_period));
timer_ = rclcpp::create_timer(
@@ -401,9 +158,7 @@ void ReactionAnalyzerNode::onTimer()
const auto initialization_state_ptr = initialization_state_ptr_;
const auto route_state_ptr = current_route_state_ptr_;
const auto operation_mode_ptr = operation_mode_ptr_;
- const auto message_buffers = message_buffers_;
const auto spawn_cmd_time = spawn_cmd_time_;
- const auto published_time_vector_map = published_time_vector_map_;
mutex_.unlock();
// Init the test environment
@@ -417,11 +172,12 @@ void ReactionAnalyzerNode::onTimer()
if (!spawn_cmd_time) return;
- if (!allReacted(message_buffers)) return;
+ const auto message_buffers = subscriber_ptr_->getMessageBuffersMap();
- if (!is_output_printed_) {
- calculateResults(message_buffers, published_time_vector_map, spawn_cmd_time.value());
- } else {
+ if (message_buffers) {
+ // if reacted, calculate the results
+
+ calculateResults(message_buffers.value(), spawn_cmd_time.value());
reset();
}
}
@@ -449,7 +205,7 @@ void ReactionAnalyzerNode::dummyPerceptionPublisher()
geometry_msgs::msg::TransformStamped transform_stamped{};
try {
transform_stamped = tf_buffer_.lookupTransform(
- "base_link", "map", this->now(), rclcpp::Duration::from_seconds(0.5));
+ "base_link", "map", this->now(), rclcpp::Duration::from_seconds(0.1));
} catch (tf2::TransformException & ex) {
RCLCPP_ERROR_STREAM(get_logger(), "Failed to look up transform from map to base_link");
return;
@@ -501,58 +257,8 @@ void ReactionAnalyzerNode::spawnObstacle(const geometry_msgs::msg::Point & ego_p
}
}
-bool ReactionAnalyzerNode::allReacted(
- const std::unordered_map<std::string, BufferVariant> & message_buffers)
-{
- bool all_reacted = true;
- for (const auto & [key, variant] : message_buffers) {
- if (auto * control_message = std::get_if<ControlCommandBuffer>(&variant)) {
- if (!control_message->second) {
- all_reacted = false;
- }
- } else if (auto * planning_message = std::get_if<TrajectoryBuffer>(&variant)) {
- if (!planning_message->has_value()) {
- all_reacted = false;
- }
- } else if (auto * pointcloud_message = std::get_if<PointCloud2Buffer>(&variant)) {
- if (!pointcloud_message->has_value()) {
- all_reacted = false;
- }
- } else if (auto * predicted_objects_message = std::get_if<PredictedObjectsBuffer>(&variant)) {
- if (!predicted_objects_message->has_value()) {
- all_reacted = false;
- }
- } else if (auto * detected_objects_message = std::get_if<DetectedObjectsBuffer>(&variant)) {
- if (!detected_objects_message->has_value()) {
- all_reacted = false;
- }
- } else if (auto * tracked_objects_message = std::get_if<TrackedObjectsBuffer>(&variant)) {
- if (!tracked_objects_message->has_value()) {
- all_reacted = false;
- }
- }
- }
- return all_reacted;
-}
-
-std::optional<size_t> findConjugatePublishedTimeIdx(
- const std::vector<PublishedTime> & published_time_vector, const rclcpp::Time & time)
-{
- auto it = std::find_if(
- published_time_vector.begin(), published_time_vector.end(),
- [&time](const PublishedTime & timeInVector) { return timeInVector.header.stamp == time; });
-
- if (it != published_time_vector.end()) {
- return std::optional<int>(
- std::distance(published_time_vector.begin(), it)); // Return the index of the found time
- } else {
- return std::nullopt;
- }
-}
-
void ReactionAnalyzerNode::calculateResults(
- const std::unordered_map<std::string, BufferVariant> & message_buffers,
- const std::unordered_map<std::string, std::vector<PublishedTime>> & published_time_vector_map,
+ const std::unordered_map<std::string, subscriber::BufferVariant> & message_buffers,
const rclcpp::Time & spawn_cmd_time)
{
auto createDurationMs = [](const rclcpp::Time & start_time, const rclcpp::Time & end_time) {
@@ -562,123 +268,35 @@ void ReactionAnalyzerNode::calculateResults(
for (const auto & [key, variant] : message_buffers) {
rclcpp::Time reaction_time;
- if (auto * control_message = std::get_if<ControlCommandBuffer>(&variant)) {
+ if (auto * control_message = std::get_if<subscriber::ControlCommandBuffer>(&variant)) {
if (control_message->second) {
- auto it = published_time_vector_map.find(key);
- if (it != published_time_vector_map.end()) {
- const auto & published_time_vec = it->second;
- const auto idx =
- findConjugatePublishedTimeIdx(published_time_vec, control_message->second->stamp);
- if (idx) {
- reaction_time = rclcpp::Time(published_time_vec.at(idx.value()).published_stamp);
- } else {
- RCLCPP_ERROR(
- this->get_logger(), "Published time for %s node is not found", key.c_str());
-
- reaction_time = control_message->second->stamp;
- }
- } else {
- // It might do not have a published time debug topic
- reaction_time = control_message->second->stamp;
- }
+ reaction_time = control_message->second->stamp;
}
- } else if (auto * planning_message = std::get_if<TrajectoryBuffer>(&variant)) {
+ } else if (auto * planning_message = std::get_if<subscriber::TrajectoryBuffer>(&variant)) {
if (planning_message->has_value()) {
- auto it = published_time_vector_map.find(key);
- if (it != published_time_vector_map.end()) {
- const auto & published_time_vec = it->second;
- const auto idx = findConjugatePublishedTimeIdx(
- published_time_vec, planning_message->value().header.stamp);
- if (idx) {
- reaction_time = rclcpp::Time(published_time_vec.at(idx.value()).published_stamp);
- } else {
- RCLCPP_ERROR(
- this->get_logger(), "Published time for %s node is not found", key.c_str());
-
- reaction_time = planning_message->value().header.stamp;
- }
- } else {
- reaction_time = planning_message->value().header.stamp;
- }
+ reaction_time = planning_message->value().header.stamp;
}
- } else if (auto * pointcloud_message = std::get_if<PointCloud2Buffer>(&variant)) {
+ } else if (auto * pointcloud_message = std::get_if<subscriber::PointCloud2Buffer>(&variant)) {
if (pointcloud_message->has_value()) {
- auto it = published_time_vector_map.find(key);
- if (it != published_time_vector_map.end()) {
- const auto & published_time_vec = it->second;
- const auto idx = findConjugatePublishedTimeIdx(
- published_time_vec, pointcloud_message->value().header.stamp);
- if (idx) {
- reaction_time = rclcpp::Time(published_time_vec.at(idx.value()).published_stamp);
- } else {
- RCLCPP_ERROR(
- this->get_logger(), "Published time for %s node is not found", key.c_str());
-
- reaction_time = pointcloud_message->value().header.stamp;
- }
- } else {
- reaction_time = pointcloud_message->value().header.stamp;
- }
+ reaction_time = pointcloud_message->value().header.stamp;
}
- } else if (auto * predicted_objects_message = std::get_if<PredictedObjectsBuffer>(&variant)) {
+ } else if (
+ auto * predicted_objects_message =
+ std::get_if<subscriber::PredictedObjectsBuffer>(&variant)) {
if (predicted_objects_message->has_value()) {
- auto it = published_time_vector_map.find(key);
- if (it != published_time_vector_map.end()) {
- const auto & published_time_vec = it->second;
- const auto idx = findConjugatePublishedTimeIdx(
- published_time_vec, predicted_objects_message->value().header.stamp);
- if (idx) {
- reaction_time = rclcpp::Time(published_time_vec.at(idx.value()).published_stamp);
- } else {
- RCLCPP_ERROR(
- this->get_logger(), "Published time for %s node is not found", key.c_str());
-
- reaction_time = predicted_objects_message->value().header.stamp;
- }
- } else {
- reaction_time = predicted_objects_message->value().header.stamp;
- }
+ reaction_time = predicted_objects_message->value().header.stamp;
}
- } else if (auto * detected_objects_message = std::get_if<DetectedObjectsBuffer>(&variant)) {
+ } else if (
+ auto * detected_objects_message = std::get_if<subscriber::DetectedObjectsBuffer>(&variant)) {
if (detected_objects_message->has_value()) {
- auto it = published_time_vector_map.find(key);
- if (it != published_time_vector_map.end()) {
- const auto & published_time_vec = it->second;
- const auto idx = findConjugatePublishedTimeIdx(
- published_time_vec, detected_objects_message->value().header.stamp);
- if (idx) {
- reaction_time = rclcpp::Time(published_time_vec.at(idx.value()).published_stamp);
- } else {
- RCLCPP_ERROR(
- this->get_logger(), "Published time for %s node is not found", key.c_str());
-
- reaction_time = detected_objects_message->value().header.stamp;
- }
- } else {
- reaction_time = detected_objects_message->value().header.stamp;
- }
+ reaction_time = detected_objects_message->value().header.stamp;
}
- } else if (auto * tracked_objects_message = std::get_if<TrackedObjectsBuffer>(&variant)) {
+ } else if (
+ auto * tracked_objects_message = std::get_if<subscriber::TrackedObjectsBuffer>(&variant)) {
if (tracked_objects_message->has_value()) {
- auto it = published_time_vector_map.find(key);
- if (it != published_time_vector_map.end()) {
- const auto & published_time_vec = it->second;
- const auto idx = findConjugatePublishedTimeIdx(
- published_time_vec, tracked_objects_message->value().header.stamp);
- if (idx) {
- reaction_time = rclcpp::Time(published_time_vec.at(idx.value()).published_stamp);
- } else {
- RCLCPP_ERROR(
- this->get_logger(), "Published time for %s node is not found", key.c_str());
-
- reaction_time = tracked_objects_message->value().header.stamp;
- }
- } else {
- reaction_time = tracked_objects_message->value().header.stamp;
- }
+ reaction_time = tracked_objects_message->value().header.stamp;
}
}
-
const auto duration = createDurationMs(spawn_cmd_time, reaction_time);
RCLCPP_INFO(
@@ -686,61 +304,6 @@ void ReactionAnalyzerNode::calculateResults(
test_results_[key].emplace_back(duration);
}
test_iteration_count_++;
- is_output_printed_ = true;
-}
-
-bool ReactionAnalyzerNode::loadChainModules()
-{
- auto split = [](const std::string & str, const char delim) {
- std::vector<std::string> elems;
- std::stringstream ss(str);
- std::string item;
- while (std::getline(ss, item, delim)) {
- elems.push_back(item);
- }
- return elems;
- };
-
- auto stringToMessageType = [](const std::string & input) {
- if (input == "autoware_auto_control_msgs::msg::AckermannControlCommand") {
- return SubscriberMessageType::AckermannControlCommand;
- } else if (input == "autoware_auto_planning_msgs::msg::Trajectory") {
- return SubscriberMessageType::Trajectory;
- } else if (input == "sensor_msgs::msg::PointCloud2") {
- return SubscriberMessageType::PointCloud2;
- } else if (input == "autoware_auto_perception_msgs::msg::PredictedObjects") {
- return SubscriberMessageType::PredictedObjects;
- } else if (input == "autoware_auto_perception_msgs::msg::DetectedObjects") {
- return SubscriberMessageType::DetectedObjects;
- } else if (input == "autoware_auto_perception_msgs::msg::TrackedObjects") {
- return SubscriberMessageType::TrackedObjects;
- } else {
- return SubscriberMessageType::Unknown;
- }
- };
-
- // get the topic addresses and message types of the modules in chain
- const auto param_key = std::string("reaction_chain");
- const auto module_names = this->list_parameters({param_key}, 3).prefixes;
- ChainModules chain_modules;
- for (const auto & module_name : module_names) {
- const auto splitted_name = split(module_name, '.');
- TopicConfig tmp;
- tmp.node_name = splitted_name.back();
- tmp.topic_address = this->get_parameter(module_name + ".topic_name").as_string();
- tmp.time_debug_topic_address =
- this->get_parameter_or(module_name + ".time_debug_topic_name", std::string(""));
- tmp.message_type =
- stringToMessageType(this->get_parameter(module_name + ".message_type").as_string());
- if (tmp.message_type != SubscriberMessageType::Unknown) {
- chain_modules.emplace_back(tmp);
- } else {
- RCLCPP_WARN(
- this->get_logger(), "Unknown message type for module name: %s, skipping..",
- tmp.node_name.c_str());
- }
- }
- return (initSubscribers(chain_modules));
}
void ReactionAnalyzerNode::initAnalyzerVariables()
@@ -758,15 +321,6 @@ void ReactionAnalyzerNode::initAnalyzerVariables()
entity_pose_.orientation.set__z(entity_q_orientation.z());
entity_pose_.orientation.set__w(entity_q_orientation.w());
- // find minimum radius of sphere that encloses the entity
-
- entity_search_radius_ =
- std::sqrt(
- std::pow(node_params_.entity_params.x_l, 2) + std::pow(node_params_.entity_params.y_l, 2) +
- std::pow(node_params_.entity_params.z_l, 2)) /
- 2.0 +
- node_params_.object_search_radius_offset;
-
tf2::Quaternion goal_pose_q_orientation;
goal_pose_q_orientation.setRPY(
tier4_autoware_utils::deg2rad(node_params_.goal_pose.roll),
@@ -843,101 +397,6 @@ void ReactionAnalyzerNode::initPointcloud()
pcl::toROSMsg(point_cloud, *entity_pointcloud_ptr_);
}
-bool ReactionAnalyzerNode::initSubscribers(const reaction_analyzer::ChainModules & modules)
-{
- if (modules.empty()) {
- RCLCPP_ERROR(get_logger(), "No module to initialize, failed.");
- return false;
- }
- for (const auto & module : modules) {
- if (!module.time_debug_topic_address.empty()) {
- auto callback = [this, module](const PublishedTime::ConstSharedPtr & msg) {
- this->publishedTimeOutputCallback(module.node_name, msg);
- };
- auto subscriber = this->create_subscription<PublishedTime>(
- module.time_debug_topic_address, rclcpp::QoS(1), callback, createSubscriptionOptions());
- subscribers_.push_back(subscriber);
- published_time_vector_map_[module.node_name] = std::vector<PublishedTime>();
- } else {
- RCLCPP_WARN(
- this->get_logger(), "Time debug topic is not provided for module name: %s, skipping..",
- module.node_name.c_str());
- }
- switch (module.message_type) {
- case SubscriberMessageType::AckermannControlCommand: {
- auto callback = [this, module](const AckermannControlCommand::ConstSharedPtr & msg) {
- this->controlCommandOutputCallback(module.node_name, msg);
- };
- auto subscriber = this->create_subscription<AckermannControlCommand>(
- module.topic_address, rclcpp::QoS(1), callback, createSubscriptionOptions());
- subscribers_.push_back(subscriber);
- break;
- }
- case SubscriberMessageType::Trajectory: {
- auto callback = [this, module](const Trajectory::ConstSharedPtr & msg) {
- this->trajectoryOutputCallback(module.node_name, msg);
- };
- auto subscriber = this->create_subscription<Trajectory>(
- module.topic_address, rclcpp::QoS(1), callback, createSubscriptionOptions());
- subscribers_.push_back(subscriber);
- break;
- }
- case SubscriberMessageType::PointCloud2: {
- auto callback = [this, module](const PointCloud2::ConstSharedPtr & msg) {
- this->pointcloud2OutputCallback(module.node_name, msg);
- };
- auto subscriber = this->create_subscription<PointCloud2>(
- module.topic_address, rclcpp::SensorDataQoS(), callback, createSubscriptionOptions());
- subscribers_.push_back(subscriber);
- break;
- }
- case SubscriberMessageType::PredictedObjects: {
- auto callback = [this, module](const PredictedObjects::ConstSharedPtr & msg) {
- this->predictedObjectsOutputCallback(module.node_name, msg);
- };
- auto subscriber = this->create_subscription<PredictedObjects>(
- module.topic_address, rclcpp::QoS(1), callback, createSubscriptionOptions());
- subscribers_.push_back(subscriber);
- break;
- }
- case SubscriberMessageType::DetectedObjects: {
- auto callback = [this, module](const DetectedObjects::ConstSharedPtr & msg) {
- this->detectedObjectsOutputCallback(module.node_name, msg);
- };
- auto subscriber = this->create_subscription<DetectedObjects>(
- module.topic_address, rclcpp::QoS(1), callback, createSubscriptionOptions());
- subscribers_.push_back(subscriber);
- break;
- }
- case SubscriberMessageType::TrackedObjects: {
- auto callback = [this, module](const TrackedObjects::ConstSharedPtr & msg) {
- this->trackedObjectsOutputCallback(module.node_name, msg);
- };
- auto subscriber = this->create_subscription<TrackedObjects>(
- module.topic_address, rclcpp::QoS(1), callback, createSubscriptionOptions());
- subscribers_.push_back(subscriber);
- break;
- }
- case SubscriberMessageType::Unknown:
- RCLCPP_WARN(
- this->get_logger(), "Unknown message type for module name: %s, skipping..",
- module.node_name.c_str());
- break;
- default:
- RCLCPP_WARN(
- this->get_logger(), "Unknown message type for module name: %s, skipping..",
- module.node_name.c_str());
- break;
- }
- }
- if (subscribers_.empty()) {
- RCLCPP_ERROR(
- get_logger(), "Subscribers for modules are empty. Node couldn't be initialized. Failed");
- return false;
- }
- return true;
-}
-
void ReactionAnalyzerNode::initPredictedObjects()
{
auto generateUUIDMsg = [](const std::string & input) {
@@ -970,106 +429,6 @@ void ReactionAnalyzerNode::initPredictedObjects()
predicted_objects_ptr_ = std::make_shared<PredictedObjects>(pred_objects);
}
-void ReactionAnalyzerNode::setControlCommandToBuffer(
- std::vector<AckermannControlCommand> & buffer, const AckermannControlCommand & cmd)
-{
- const auto last_cmd_time = cmd.stamp;
- if (!buffer.empty()) {
- for (auto itr = buffer.begin(); itr != buffer.end();) {
- const auto expired = (rclcpp::Time(last_cmd_time) - rclcpp::Time(itr->stamp)).seconds() >
- node_params_.control_cmd_buffer_time_interval;
-
- if (expired) {
- itr = buffer.erase(itr);
- continue;
- }
-
- itr++;
- }
- }
- buffer.emplace_back(cmd);
-}
-
-void ReactionAnalyzerNode::pushPublishedTime(
- std::vector<PublishedTime> & published_time_vec, const PublishedTime & published_time)
-{
- published_time_vec.emplace_back(published_time);
- if (published_time_vec.size() > 1) {
- for (auto itr = published_time_vec.begin(); itr != published_time_vec.end();) {
- const auto expired =
- (rclcpp::Time(published_time.header.stamp) - rclcpp::Time(itr->header.stamp)).seconds() >
- node_params_.published_time_expire_duration;
-
- if (expired) {
- itr = published_time_vec.erase(itr);
- continue;
- }
-
- itr++;
- }
- }
-}
-
-std::optional<size_t> ReactionAnalyzerNode::findFirstBrakeIdx(
- const std::vector<AckermannControlCommand> & cmd_array,
- const std::optional<rclcpp::Time> & spawn_cmd_time)
-{
- if (
- cmd_array.size() < static_cast<size_t>(node_params_.min_number_descending_order_control_cmd) ||
- !spawn_cmd_time)
- return {};
-
- // wait for enough data after spawn_cmd_time
- if (
- rclcpp::Time(
- cmd_array.at(cmd_array.size() - node_params_.min_number_descending_order_control_cmd).stamp) <
- spawn_cmd_time)
- return {};
-
- for (size_t i = 0;
- i < cmd_array.size() - node_params_.min_number_descending_order_control_cmd + 1; ++i) {
- size_t decreased_cmd_counter = 1; // because # of the decreased cmd = iteration + 1
- for (size_t j = i; j < cmd_array.size() - 1; ++j) {
- const auto & cmd_first = cmd_array.at(j).longitudinal;
- const auto & cmd_second = cmd_array.at(j + 1).longitudinal;
- constexpr double jerk_time_epsilon = 0.001;
- const auto jerk =
- abs(cmd_second.acceleration - cmd_first.acceleration) /
- std::max(
- (rclcpp::Time(cmd_second.stamp) - rclcpp::Time(cmd_first.stamp)).seconds(),
- jerk_time_epsilon);
-
- if (
- (cmd_second.acceleration < cmd_first.acceleration) &&
- (jerk > node_params_.min_jerk_for_brake_cmd)) {
- decreased_cmd_counter++;
- } else {
- break;
- }
- }
- if (
- decreased_cmd_counter <
- static_cast<size_t>(node_params_.min_number_descending_order_control_cmd))
- continue;
- if (node_params_.debug_control_commands) {
- std::stringstream ss;
-
- // debug print to show the first brake command in the all control commands
- for (size_t k = 0; k < cmd_array.size(); ++k) {
- if (k == i + 1) {
- ss << "First Brake(" << cmd_array.at(k).longitudinal.acceleration << ") ";
- } else {
- ss << cmd_array.at(k).longitudinal.acceleration << " ";
- }
- }
-
- RCLCPP_INFO(this->get_logger(), "%s", ss.str().c_str());
- }
- return i + 1;
- }
- return {};
-}
-
void ReactionAnalyzerNode::initEgoForTest(
const LocalizationInitializationState::ConstSharedPtr & initialization_state_ptr,
const RouteState::ConstSharedPtr & route_state_ptr,
@@ -1130,17 +489,6 @@ void ReactionAnalyzerNode::initEgoForTest(
}
}
-rclcpp::SubscriptionOptions ReactionAnalyzerNode::createSubscriptionOptions()
-{
- rclcpp::CallbackGroup::SharedPtr callback_group =
- this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-
- auto sub_opt = rclcpp::SubscriptionOptions();
- sub_opt.callback_group = callback_group;
-
- return sub_opt;
-}
-
void ReactionAnalyzerNode::callOperationModeServiceWithoutResponse()
{
auto req = std::make_shared<ChangeOperationMode::Request>();
@@ -1160,71 +508,6 @@ void ReactionAnalyzerNode::callOperationModeServiceWithoutResponse()
});
}
-bool ReactionAnalyzerNode::searchPointcloudNearEntity(
- const pcl::PointCloud<pcl::PointXYZ> & pcl_pointcloud)
-{
- bool isAnyPointWithinRadius = std::any_of(
- pcl_pointcloud.points.begin(), pcl_pointcloud.points.end(), [this](const auto & point) {
- return tier4_autoware_utils::calcDistance3d(entity_pose_.position, point) <=
- entity_search_radius_;
- });
-
- if (isAnyPointWithinRadius) {
- return true;
- }
- return false;
-}
-
-bool ReactionAnalyzerNode::searchPredictedObjectsNearEntity(
- const PredictedObjects & predicted_objects)
-{
- bool isAnyObjectWithinRadius = std::any_of(
- predicted_objects.objects.begin(), predicted_objects.objects.end(),
- [this](const PredictedObject & object) {
- return tier4_autoware_utils::calcDistance3d(
- entity_pose_.position,
- object.kinematics.initial_pose_with_covariance.pose.position) <=
- entity_search_radius_;
- });
-
- if (isAnyObjectWithinRadius) {
- return true;
- }
- return false;
-}
-
-bool ReactionAnalyzerNode::searchDetectedObjectsNearEntity(const DetectedObjects & detected_objects)
-{
- bool isAnyObjectWithinRadius = std::any_of(
- detected_objects.objects.begin(), detected_objects.objects.end(),
- [this](const DetectedObject & object) {
- return tier4_autoware_utils::calcDistance3d(
- entity_pose_.position, object.kinematics.pose_with_covariance.pose.position) <=
- entity_search_radius_;
- });
-
- if (isAnyObjectWithinRadius) {
- return true;
- }
- return false;
-}
-
-bool ReactionAnalyzerNode::searchTrackedObjectsNearEntity(const TrackedObjects & tracked_objects)
-{
- bool isAnyObjectWithinRadius = std::any_of(
- tracked_objects.objects.begin(), tracked_objects.objects.end(),
- [this](const TrackedObject & object) {
- return tier4_autoware_utils::calcDistance3d(
- entity_pose_.position, object.kinematics.pose_with_covariance.pose.position) <=
- entity_search_radius_;
- });
-
- if (isAnyObjectWithinRadius) {
- return true;
- }
- return false;
-}
-
void ReactionAnalyzerNode::reset()
{
if (test_iteration_count_ >= node_params_.test_iteration) {
@@ -1238,53 +521,20 @@ void ReactionAnalyzerNode::reset()
test_environment_init_time_ = std::nullopt;
last_test_environment_init_request_time_ = std::nullopt;
spawn_object_cmd_ = false;
- is_output_printed_ = false;
is_object_spawned_message_published_ = false;
if (topic_publisher_ptr_) {
topic_publisher_ptr_->reset();
}
std::lock_guard<std::mutex> lock(mutex_);
- message_buffers_.clear();
spawn_cmd_time_ = std::nullopt;
- for (auto & [key, value] : published_time_vector_map_) {
- value.clear();
- }
+ subscriber_ptr_->reset();
RCLCPP_INFO(this->get_logger(), "Test - %zu is done, resetting..", test_iteration_count_);
}
void ReactionAnalyzerNode::writeResultsToFile()
{
// sort the results w.r.t the median value
-
- const auto sort_by_median =
- [this]() -> std::vector<std::tuple<std::string, std::vector<double>, double>> {
- std::vector<std::tuple<std::string, std::vector<double>, double>> sorted_data;
-
- for (const auto & pair : test_results_) {
- auto vec = pair.second;
-
- // Calculate the median
- std::sort(vec.begin(), vec.end());
- double median = 0.0;
- size_t size = vec.size();
- if (size % 2 == 0) {
- median = (vec[size / 2 - 1] + vec[size / 2]) / 2.0;
- } else {
- median = vec[size / 2];
- }
-
- sorted_data.emplace_back(pair.first, pair.second, median);
- }
-
- // Sort based on the computed median
- std::sort(sorted_data.begin(), sorted_data.end(), [](const auto & a, const auto & b) {
- return std::get<2>(a) < std::get<2>(b); // Change to > for descending order
- });
-
- return sorted_data;
- };
-
- const auto sorted_data_by_median = sort_by_median();
+ const auto sorted_data_by_median = sortResultsByMedian(test_results_);
// create csv file
auto now = std::chrono::system_clock::now();
diff --git a/tools/reaction_analyzer/src/subscriber.cpp b/tools/reaction_analyzer/src/subscriber.cpp
new file mode 100644
index 0000000000000..43fd05515fa0a
--- /dev/null
+++ b/tools/reaction_analyzer/src/subscriber.cpp
@@ -0,0 +1,1088 @@
+// Copyright 2024 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "subscriber.hpp"
+
+#include <algorithm>
+#include <memory>
+#include <utility>
+
+namespace reaction_analyzer::subscriber
+{
+
+SubscriberBase::SubscriberBase(
+ rclcpp::Node * node, Odometry::ConstSharedPtr & odometry,
+ const geometry_msgs::msg::Pose entity_pose, std::atomic<bool> & spawn_object_cmd)
+: node_(node), odometry_(odometry), entity_pose_(entity_pose), spawn_object_cmd_(spawn_object_cmd)
+{
+ // init tf
+ tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
+ tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+
+ // init reaction parameters and chain configuration
+ initReactionChainsAndParams();
+ initSubscribers();
+}
+
+void SubscriberBase::initReactionChainsAndParams()
+{
+ auto stringToMessageType = [](const std::string & input) {
+ if (input == "autoware_auto_control_msgs::msg::AckermannControlCommand") {
+ return SubscriberMessageType::ACKERMANN_CONTROL_COMMAND;
+ } else if (input == "autoware_auto_planning_msgs::msg::Trajectory") {
+ return SubscriberMessageType::TRAJECTORY;
+ } else if (input == "sensor_msgs::msg::PointCloud2") {
+ return SubscriberMessageType::POINTCLOUD2;
+ } else if (input == "autoware_auto_perception_msgs::msg::PredictedObjects") {
+ return SubscriberMessageType::PREDICTED_OBJECTS;
+ } else if (input == "autoware_auto_perception_msgs::msg::DetectedObjects") {
+ return SubscriberMessageType::DETECTED_OBJECTS;
+ } else if (input == "autoware_auto_perception_msgs::msg::TrackedObjects") {
+ return SubscriberMessageType::TRACKED_OBJECTS;
+ } else {
+ return SubscriberMessageType::UNKNOWN;
+ }
+ };
+
+ auto stringToReactionType = [](const std::string & input) {
+ if (input == "first_brake_params") {
+ return ReactionType::FIRST_BRAKE;
+ } else if (input == "search_zero_vel_params") {
+ return ReactionType::SEARCH_ZERO_VEL;
+ } else if (input == "search_entity_params") {
+ return ReactionType::SEARCH_ENTITY;
+ } else {
+ return ReactionType::UNKNOWN;
+ }
+ };
+
+ // Init Chains: get the topic addresses and message types of the modules in chain
+ {
+ const auto param_key = std::string("subscriber.reaction_chain");
+ const auto module_names = node_->list_parameters({param_key}, 3).prefixes;
+ for (const auto & module_name : module_names) {
+ const auto splitted_name = split(module_name, '.');
+ TopicConfig tmp;
+ tmp.node_name = splitted_name.back();
+ tmp.topic_address = node_->get_parameter(module_name + ".topic_name").as_string();
+ tmp.time_debug_topic_address =
+ node_->get_parameter_or(module_name + ".time_debug_topic_name", std::string(""));
+ tmp.message_type =
+ stringToMessageType(node_->get_parameter(module_name + ".message_type").as_string());
+ if (tmp.message_type != SubscriberMessageType::UNKNOWN) {
+ chain_modules_.emplace_back(tmp);
+ } else {
+ RCLCPP_WARN(
+ node_->get_logger(), "Unknown message type for module name: %s, skipping..",
+ tmp.node_name.c_str());
+ }
+ }
+ }
+
+ // Init Params: get the parameters for the reaction functions
+ {
+ const auto param_key = std::string("subscriber.reaction_params");
+ const auto module_names = node_->list_parameters({param_key}, 3).prefixes;
+ for (const auto & module_name : module_names) {
+ const auto splitted_name = split(module_name, '.');
+ const auto type = stringToReactionType(splitted_name.back());
+ switch (type) {
+ case ReactionType::FIRST_BRAKE: {
+ reaction_params_.first_brake_params.debug_control_commands =
+ node_->get_parameter(module_name + ".debug_control_commands").as_bool();
+ reaction_params_.first_brake_params.control_cmd_buffer_time_interval =
+ node_->get_parameter(module_name + ".control_cmd_buffer_time_interval").as_double();
+ reaction_params_.first_brake_params.min_number_descending_order_control_cmd =
+ node_->get_parameter(module_name + ".min_number_descending_order_control_cmd").as_int();
+ reaction_params_.first_brake_params.min_jerk_for_brake_cmd =
+ node_->get_parameter(module_name + ".min_jerk_for_brake_cmd").as_double();
+ RCLCPP_INFO_ONCE(
+ node_->get_logger(),
+ "First brake parameters are set: debug_control_commands %s, "
+ "control_cmd_buffer_time_interval %f, min_number_descending_order_control_cmd %zu, "
+ "min_jerk_for_brake_cmd %f",
+ reaction_params_.first_brake_params.debug_control_commands ? "true" : "false",
+ reaction_params_.first_brake_params.control_cmd_buffer_time_interval,
+ reaction_params_.first_brake_params.min_number_descending_order_control_cmd,
+ reaction_params_.first_brake_params.min_jerk_for_brake_cmd);
+ break;
+ }
+ case ReactionType::SEARCH_ZERO_VEL: {
+ reaction_params_.search_zero_vel_params.max_looking_distance =
+ node_->get_parameter(module_name + ".max_looking_distance").as_double();
+ RCLCPP_INFO_ONCE(
+ node_->get_logger(), "Search Zero Vel parameters are set: max_looking_distance %f",
+ reaction_params_.search_zero_vel_params.max_looking_distance);
+ break;
+ }
+ case ReactionType::SEARCH_ENTITY: {
+ reaction_params_.search_entity_params.search_radius =
+ node_->get_parameter(module_name + ".search_radius").as_double();
+ RCLCPP_INFO_ONCE(
+ node_->get_logger(), "Search Entity parameters are set: search_radius %f",
+ reaction_params_.search_entity_params.search_radius);
+ break;
+ }
+ default:
+ RCLCPP_WARN(
+ node_->get_logger(), "Unknown reaction type for module name: %s, skipping..",
+ splitted_name.back().c_str());
+ break;
+ }
+ }
+ }
+}
+
+bool SubscriberBase::initSubscribers()
+{
+ if (chain_modules_.empty()) {
+ RCLCPP_ERROR(node_->get_logger(), "No module to initialize subscribers, failed.");
+ return false;
+ }
+
+ for (const auto & module : chain_modules_) {
+ switch (module.message_type) {
+ case SubscriberMessageType::ACKERMANN_CONTROL_COMMAND: {
+ SubscriberVariables<AckermannControlCommand> subscriber_variable;
+
+ if (!module.time_debug_topic_address.empty()) {
+ // If not empty, user should define a time debug topic
+ // NOTE: Because message_filters package does not support the messages without headers, we
+ // can not use the synchronizer. After we reacted, we are going to use the cache
+ // of the both PublishedTime and AckermannControlCommand subscribers to find the messages
+ // which have same header time.
+
+ std::function<void(const AckermannControlCommand::ConstSharedPtr &)> callback =
+ [this, module](const AckermannControlCommand::ConstSharedPtr & ptr) {
+ this->controlCommandOutputCallback(module.node_name, ptr);
+ };
+ subscriber_variable.sub1_ =
+ std::make_unique<message_filters::Subscriber<AckermannControlCommand>>(
+ node_, module.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(),
+ createSubscriptionOptions(node_));
+
+ subscriber_variable.sub1_->registerCallback(std::bind(callback, std::placeholders::_1));
+
+ subscriber_variable.sub2_ = std::make_unique<message_filters::Subscriber<PublishedTime>>(
+ node_, module.time_debug_topic_address, rclcpp::QoS(1).get_rmw_qos_profile(),
+ createSubscriptionOptions(node_));
+ constexpr int cache_size = 5;
+ subscriber_variable.cache_ = std::make_unique<message_filters::Cache<PublishedTime>>(
+ *subscriber_variable.sub2_, cache_size);
+
+ } else {
+ std::function<void(const AckermannControlCommand::ConstSharedPtr &)> callback =
+ [this, module](const AckermannControlCommand::ConstSharedPtr & ptr) {
+ this->controlCommandOutputCallback(module.node_name, ptr);
+ };
+
+ subscriber_variable.sub1_ =
+ std::make_unique<message_filters::Subscriber<AckermannControlCommand>>(
+ node_, module.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(),
+ createSubscriptionOptions(node_));
+
+ subscriber_variable.sub1_->registerCallback(std::bind(callback, std::placeholders::_1));
+ RCLCPP_WARN(
+ node_->get_logger(),
+ "PublishedTime will not be used for node name: %s. Header timestamp will be used for "
+ "calculations",
+ module.node_name.c_str());
+ }
+ // set the variable to map with the topic address
+ subscriber_variables_map_[module.node_name] = std::move(subscriber_variable);
+ break;
+ }
+ case SubscriberMessageType::TRAJECTORY: {
+ SubscriberVariables<Trajectory> subscriber_variable;
+
+ if (!module.time_debug_topic_address.empty()) {
+ std::function<void(
+ const Trajectory::ConstSharedPtr &, const PublishedTime::ConstSharedPtr &)>
+ callback = [this, module](
+ const Trajectory::ConstSharedPtr & ptr,
+ const PublishedTime::ConstSharedPtr & published_time_ptr) {
+ this->trajectoryOutputCallback(module.node_name, ptr, published_time_ptr);
+ };
+
+ subscriber_variable.sub1_ = std::make_unique<message_filters::Subscriber<Trajectory>>(
+ node_, module.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(),
+ createSubscriptionOptions(node_));
+ subscriber_variable.sub2_ = std::make_unique<message_filters::Subscriber<PublishedTime>>(
+ node_, module.time_debug_topic_address, rclcpp::QoS(1).get_rmw_qos_profile(),
+ createSubscriptionOptions(node_));
+
+ subscriber_variable.synchronizer_ = std::make_unique<
+ message_filters::Synchronizer<SubscriberVariables<Trajectory>::ExactTimePolicy>>(
+ SubscriberVariables<Trajectory>::ExactTimePolicy(10), *subscriber_variable.sub1_,
+ *subscriber_variable.sub2_);
+
+ subscriber_variable.synchronizer_->registerCallback(
+ std::bind(callback, std::placeholders::_1, std::placeholders::_2));
+
+ } else {
+ std::function<void(const Trajectory::ConstSharedPtr &)> callback =
+ [this, module](const Trajectory::ConstSharedPtr & msg) {
+ this->trajectoryOutputCallback(module.node_name, msg);
+ };
+ subscriber_variable.sub1_ = std::make_unique<message_filters::Subscriber<Trajectory>>(
+ node_, module.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(),
+ createSubscriptionOptions(node_));
+
+ subscriber_variable.sub1_->registerCallback(std::bind(callback, std::placeholders::_1));
+ RCLCPP_WARN(
+ node_->get_logger(),
+ "PublishedTime will not be used for node name: %s. Header timestamp will be used for "
+ "calculations",
+ module.node_name.c_str());
+ }
+ subscriber_variables_map_[module.node_name] = std::move(subscriber_variable);
+ break;
+ }
+ case SubscriberMessageType::POINTCLOUD2: {
+ SubscriberVariables<PointCloud2> subscriber_variable;
+
+ if (!module.time_debug_topic_address.empty()) {
+ std::function<void(
+ const PointCloud2::ConstSharedPtr &, const PublishedTime::ConstSharedPtr &)>
+ callback = [this, module](
+ const PointCloud2::ConstSharedPtr & ptr,
+ const PublishedTime::ConstSharedPtr & published_time_ptr) {
+ this->pointcloud2OutputCallback(module.node_name, ptr, published_time_ptr);
+ };
+
+ subscriber_variable.sub1_ = std::make_unique<message_filters::Subscriber<PointCloud2>>(
+ node_, module.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(),
+ createSubscriptionOptions(node_));
+ subscriber_variable.sub2_ = std::make_unique<message_filters::Subscriber<PublishedTime>>(
+ node_, module.time_debug_topic_address, rclcpp::QoS(1).get_rmw_qos_profile(),
+ createSubscriptionOptions(node_));
+
+ subscriber_variable.synchronizer_ = std::make_unique<
+ message_filters::Synchronizer<SubscriberVariables<PointCloud2>::ExactTimePolicy>>(
+ SubscriberVariables<PointCloud2>::ExactTimePolicy(10), *subscriber_variable.sub1_,
+ *subscriber_variable.sub2_);
+
+ subscriber_variable.synchronizer_->registerCallback(
+ std::bind(callback, std::placeholders::_1, std::placeholders::_2));
+
+ } else {
+ std::function<void(const PointCloud2::ConstSharedPtr &)> callback =
+ [this, module](const PointCloud2::ConstSharedPtr & msg) {
+ this->pointcloud2OutputCallback(module.node_name, msg);
+ };
+ subscriber_variable.sub1_ = std::make_unique<message_filters::Subscriber<PointCloud2>>(
+ node_, module.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(),
+ createSubscriptionOptions(node_));
+
+ subscriber_variable.sub1_->registerCallback(std::bind(callback, std::placeholders::_1));
+ RCLCPP_WARN(
+ node_->get_logger(),
+ "PublishedTime will not be used for node name: %s. Header timestamp will be used for "
+ "calculations",
+ module.node_name.c_str());
+ }
+ subscriber_variables_map_[module.node_name] = std::move(subscriber_variable);
+ break;
+ }
+ case SubscriberMessageType::PREDICTED_OBJECTS: {
+ SubscriberVariables<PredictedObjects> subscriber_variable;
+
+ if (!module.time_debug_topic_address.empty()) {
+ std::function<void(
+ const PredictedObjects::ConstSharedPtr &, const PublishedTime::ConstSharedPtr &)>
+ callback = [this, module](
+ const PredictedObjects::ConstSharedPtr & ptr,
+ const PublishedTime::ConstSharedPtr & published_time_ptr) {
+ this->predictedObjectsOutputCallback(module.node_name, ptr, published_time_ptr);
+ };
+ subscriber_variable.sub1_ =
+ std::make_unique<message_filters::Subscriber<PredictedObjects>>(
+ node_, module.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(),
+ createSubscriptionOptions(node_));
+ subscriber_variable.sub2_ = std::make_unique<message_filters::Subscriber<PublishedTime>>(
+ node_, module.time_debug_topic_address, rclcpp::QoS(1).get_rmw_qos_profile(),
+ createSubscriptionOptions(node_));
+
+ subscriber_variable.synchronizer_ = std::make_unique<
+ message_filters::Synchronizer<SubscriberVariables<PredictedObjects>::ExactTimePolicy>>(
+ SubscriberVariables<PredictedObjects>::ExactTimePolicy(10), *subscriber_variable.sub1_,
+ *subscriber_variable.sub2_);
+
+ subscriber_variable.synchronizer_->registerCallback(
+ std::bind(callback, std::placeholders::_1, std::placeholders::_2));
+
+ } else {
+ std::function<void(const PredictedObjects::ConstSharedPtr &)> callback =
+ [this, module](const PredictedObjects::ConstSharedPtr & msg) {
+ this->predictedObjectsOutputCallback(module.node_name, msg);
+ };
+ subscriber_variable.sub1_ =
+ std::make_unique<message_filters::Subscriber<PredictedObjects>>(
+ node_, module.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(),
+ createSubscriptionOptions(node_));
+
+ subscriber_variable.sub1_->registerCallback(std::bind(callback, std::placeholders::_1));
+ RCLCPP_WARN(
+ node_->get_logger(),
+ "PublishedTime will not be used for node name: %s. Header timestamp will be used for "
+ "calculations",
+ module.node_name.c_str());
+ }
+ subscriber_variables_map_[module.node_name] = std::move(subscriber_variable);
+ break;
+ }
+ case SubscriberMessageType::DETECTED_OBJECTS: {
+ SubscriberVariables<DetectedObjects> subscriber_variable;
+
+ if (!module.time_debug_topic_address.empty()) {
+ std::function<void(
+ const DetectedObjects::ConstSharedPtr &, const PublishedTime::ConstSharedPtr &)>
+ callback = [this, module](
+ const DetectedObjects::ConstSharedPtr & ptr,
+ const PublishedTime::ConstSharedPtr & published_time_ptr) {
+ this->detectedObjectsOutputCallback(module.node_name, ptr, published_time_ptr);
+ };
+
+ subscriber_variable.sub1_ =
+ std::make_unique<message_filters::Subscriber<DetectedObjects>>(
+ node_, module.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(),
+ createSubscriptionOptions(node_));
+ subscriber_variable.sub2_ = std::make_unique<message_filters::Subscriber<PublishedTime>>(
+ node_, module.time_debug_topic_address, rclcpp::QoS(1).get_rmw_qos_profile(),
+ createSubscriptionOptions(node_));
+
+ subscriber_variable.synchronizer_ = std::make_unique<
+ message_filters::Synchronizer<SubscriberVariables<DetectedObjects>::ExactTimePolicy>>(
+ SubscriberVariables<DetectedObjects>::ExactTimePolicy(10), *subscriber_variable.sub1_,
+ *subscriber_variable.sub2_);
+
+ subscriber_variable.synchronizer_->registerCallback(
+ std::bind(callback, std::placeholders::_1, std::placeholders::_2));
+
+ } else {
+ std::function<void(const DetectedObjects::ConstSharedPtr &)> callback =
+ [this, module](const DetectedObjects::ConstSharedPtr & msg) {
+ this->detectedObjectsOutputCallback(module.node_name, msg);
+ };
+ subscriber_variable.sub1_ =
+ std::make_unique<message_filters::Subscriber<DetectedObjects>>(
+ node_, module.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(),
+ createSubscriptionOptions(node_));
+
+ subscriber_variable.sub1_->registerCallback(std::bind(callback, std::placeholders::_1));
+ RCLCPP_WARN(
+ node_->get_logger(),
+ "PublishedTime will not be used for node name: %s. Header timestamp will be used for "
+ "calculations",
+ module.node_name.c_str());
+ }
+ subscriber_variables_map_[module.node_name] = std::move(subscriber_variable);
+
+ break;
+ }
+ case SubscriberMessageType::TRACKED_OBJECTS: {
+ SubscriberVariables<TrackedObjects> subscriber_variable;
+ if (!module.time_debug_topic_address.empty()) {
+ std::function<void(
+ const TrackedObjects::ConstSharedPtr &, const PublishedTime::ConstSharedPtr &)>
+ callback = [this, module](
+ const TrackedObjects::ConstSharedPtr & ptr,
+ const PublishedTime::ConstSharedPtr & published_time_ptr) {
+ this->trackedObjectsOutputCallback(module.node_name, ptr, published_time_ptr);
+ };
+
+ subscriber_variable.sub1_ = std::make_unique<message_filters::Subscriber<TrackedObjects>>(
+ node_, module.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(),
+ createSubscriptionOptions(node_));
+ subscriber_variable.sub2_ = std::make_unique<message_filters::Subscriber<PublishedTime>>(
+ node_, module.time_debug_topic_address, rclcpp::QoS(1).get_rmw_qos_profile(),
+ createSubscriptionOptions(node_));
+
+ subscriber_variable.synchronizer_ = std::make_unique<
+ message_filters::Synchronizer<SubscriberVariables<TrackedObjects>::ExactTimePolicy>>(
+ SubscriberVariables<TrackedObjects>::ExactTimePolicy(10), *subscriber_variable.sub1_,
+ *subscriber_variable.sub2_);
+
+ subscriber_variable.synchronizer_->registerCallback(
+ std::bind(callback, std::placeholders::_1, std::placeholders::_2));
+
+ } else {
+ std::function<void(const TrackedObjects::ConstSharedPtr &)> callback =
+ [this, module](const TrackedObjects::ConstSharedPtr & msg) {
+ this->trackedObjectsOutputCallback(module.node_name, msg);
+ };
+ subscriber_variable.sub1_ = std::make_unique<message_filters::Subscriber<TrackedObjects>>(
+ node_, module.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(),
+ createSubscriptionOptions(node_));
+
+ subscriber_variable.sub1_->registerCallback(std::bind(callback, std::placeholders::_1));
+ RCLCPP_WARN(
+ node_->get_logger(),
+ "PublishedTime will not be used for node name: %s. Header timestamp will be used for "
+ "calculations",
+ module.node_name.c_str());
+ }
+ subscriber_variables_map_[module.node_name] = std::move(subscriber_variable);
+ break;
+ }
+ case SubscriberMessageType::UNKNOWN:
+ RCLCPP_WARN(
+ node_->get_logger(), "Unknown message type for module name: %s, skipping..",
+ module.node_name.c_str());
+ break;
+ default:
+ RCLCPP_WARN(
+ node_->get_logger(), "Unknown message type for module name: %s, skipping..",
+ module.node_name.c_str());
+ break;
+ }
+ }
+ std::cout << "---------- size: " << subscriber_variables_map_.size() << std::endl;
+ return true;
+}
+
+std::optional<std::unordered_map<std::string, BufferVariant>> SubscriberBase::getMessageBuffersMap()
+{
+ std::lock_guard<std::mutex> lock(mutex_);
+ if (message_buffers_.empty()) {
+ RCLCPP_ERROR(node_->get_logger(), "No message buffers are initialized.");
+ return std::nullopt;
+ }
+
+ // Check all reacted or not
+ bool all_reacted = true;
+ for (const auto & [key, variant] : message_buffers_) {
+ if (auto * control_message = std::get_if<ControlCommandBuffer>(&variant)) {
+ if (!control_message->second) {
+ all_reacted = false;
+ }
+ } else if (auto * planning_message = std::get_if<TrajectoryBuffer>(&variant)) {
+ if (!planning_message->has_value()) {
+ all_reacted = false;
+ }
+ } else if (auto * pointcloud_message = std::get_if<PointCloud2Buffer>(&variant)) {
+ if (!pointcloud_message->has_value()) {
+ all_reacted = false;
+ }
+ } else if (auto * predicted_objects_message = std::get_if<PredictedObjectsBuffer>(&variant)) {
+ if (!predicted_objects_message->has_value()) {
+ all_reacted = false;
+ }
+ } else if (auto * detected_objects_message = std::get_if<DetectedObjectsBuffer>(&variant)) {
+ if (!detected_objects_message->has_value()) {
+ all_reacted = false;
+ }
+ } else if (auto * tracked_objects_message = std::get_if<TrackedObjectsBuffer>(&variant)) {
+ if (!tracked_objects_message->has_value()) {
+ all_reacted = false;
+ }
+ }
+ }
+ if (!all_reacted) {
+ return std::nullopt;
+ }
+ return message_buffers_;
+}
+
+void SubscriberBase::reset()
+{
+ std::lock_guard<std::mutex> lock(mutex_);
+ message_buffers_.clear();
+}
+
+// Callbacks
+
+void SubscriberBase::controlCommandOutputCallback(
+ const std::string & node_name, const AckermannControlCommand::ConstSharedPtr & msg_ptr)
+{
+ std::lock_guard<std::mutex> lock(mutex_);
+ auto & variant = message_buffers_[node_name];
+ if (!std::holds_alternative<ControlCommandBuffer>(variant)) {
+ ControlCommandBuffer buffer(std::vector<AckermannControlCommand>{*msg_ptr}, std::nullopt);
+ variant = buffer;
+ }
+ auto & cmd_buffer = std::get<ControlCommandBuffer>(variant);
+ if (cmd_buffer.second) {
+ // reacted
+ return;
+ }
+ setControlCommandToBuffer(cmd_buffer.first, *msg_ptr);
+ if (!spawn_object_cmd_) return;
+
+ 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
+ // does not have header
+ const auto & subscriber_variant =
+ std::get<SubscriberVariables<AckermannControlCommand>>(subscriber_variables_map_[node_name]);
+
+ // check if the cache was initialized or not, if there is, use it to set the published time
+ if (subscriber_variant.cache_) {
+ // use cache to get the published time
+ const auto published_time_vec =
+ subscriber_variant.cache_->getSurroundingInterval(brake_cmd.stamp, node_->now());
+ if (!published_time_vec.empty()) {
+ for (const auto & published_time : published_time_vec) {
+ if (published_time->header.stamp == brake_cmd.stamp) {
+ cmd_buffer.second = brake_cmd;
+ cmd_buffer.second->stamp = published_time->published_stamp;
+ RCLCPP_INFO(node_->get_logger(), "%s reacted with published time", node_name.c_str());
+ return;
+ }
+ }
+ RCLCPP_ERROR(
+ node_->get_logger(), "Published time couldn't found for the node: %s", node_name.c_str());
+
+ } else {
+ RCLCPP_ERROR(
+ node_->get_logger(), "Published time vector is empty for the node: %s",
+ node_name.c_str());
+ }
+ } else {
+ cmd_buffer.second = brake_cmd;
+ }
+ }
+}
+
+void SubscriberBase::trajectoryOutputCallback(
+ const std::string & node_name, const Trajectory::ConstSharedPtr & msg_ptr)
+{
+ mutex_.lock();
+ auto variant = message_buffers_[node_name];
+ const auto current_odometry_ptr = odometry_;
+ if (!std::holds_alternative<TrajectoryBuffer>(variant)) {
+ TrajectoryBuffer buffer(std::nullopt);
+ variant = buffer;
+ message_buffers_[node_name] = variant;
+ }
+ mutex_.unlock();
+
+ if (
+ !current_odometry_ptr || !spawn_object_cmd_ ||
+ std::get<TrajectoryBuffer>(variant).has_value()) {
+ return;
+ }
+
+ const auto nearest_seg_idx = motion_utils::findNearestSegmentIndex(
+ msg_ptr->points, current_odometry_ptr->pose.pose.position);
+
+ const auto nearest_objects_seg_idx =
+ motion_utils::findNearestIndex(msg_ptr->points, entity_pose_.position);
+
+ const auto zero_vel_idx = motion_utils::searchZeroVelocityIndex(
+ msg_ptr->points, nearest_seg_idx, nearest_objects_seg_idx);
+
+ if (zero_vel_idx) {
+ std::get<TrajectoryBuffer>(variant) = *msg_ptr;
+ RCLCPP_INFO(node_->get_logger(), "%s reacted without published time", node_name.c_str());
+ mutex_.lock();
+ message_buffers_[node_name] = variant;
+ mutex_.unlock();
+ }
+}
+
+void SubscriberBase::trajectoryOutputCallback(
+ const std::string & node_name, const Trajectory::ConstSharedPtr & msg_ptr,
+ const PublishedTime::ConstSharedPtr & published_time_ptr)
+{
+ mutex_.lock();
+ auto variant = message_buffers_[node_name];
+ const auto current_odometry_ptr = odometry_;
+ if (!std::holds_alternative<TrajectoryBuffer>(variant)) {
+ TrajectoryBuffer buffer(std::nullopt);
+ variant = buffer;
+ message_buffers_[node_name] = variant;
+ }
+ mutex_.unlock();
+
+ if (
+ !current_odometry_ptr || !spawn_object_cmd_ ||
+ std::get<TrajectoryBuffer>(variant).has_value()) {
+ return;
+ }
+
+ const auto nearest_seg_idx = motion_utils::findNearestSegmentIndex(
+ msg_ptr->points, current_odometry_ptr->pose.pose.position);
+
+ // find the target index which we will search for zero velocity
+ auto tmp_target_idx = getIndexAfterDistance(
+ *msg_ptr, nearest_seg_idx, reaction_params_.search_zero_vel_params.max_looking_distance);
+ if (tmp_target_idx == msg_ptr->points.size() - 1) {
+ tmp_target_idx = msg_ptr->points.size() - 2; // Last trajectory points might be zero velocity
+ }
+ const auto target_idx = tmp_target_idx;
+ const auto zero_vel_idx =
+ motion_utils::searchZeroVelocityIndex(msg_ptr->points, nearest_seg_idx, target_idx);
+
+ if (zero_vel_idx) {
+ std::get<TrajectoryBuffer>(variant) = *msg_ptr;
+
+ // set published time
+ std::get<TrajectoryBuffer>(variant)->header.stamp = published_time_ptr->published_stamp;
+ RCLCPP_INFO(node_->get_logger(), "%s reacted with published time", node_name.c_str());
+ mutex_.lock();
+ message_buffers_[node_name] = variant;
+ mutex_.unlock();
+ }
+}
+
+void SubscriberBase::pointcloud2OutputCallback(
+ const std::string & node_name, const PointCloud2::ConstSharedPtr & msg_ptr)
+{
+ mutex_.lock();
+ auto variant = message_buffers_[node_name];
+ if (!std::holds_alternative<PointCloud2Buffer>(variant)) {
+ PointCloud2Buffer buffer(std::nullopt);
+ variant = buffer;
+ message_buffers_[node_name] = variant;
+ }
+ mutex_.unlock();
+
+ if (!spawn_object_cmd_ || std::get<PointCloud2Buffer>(variant).has_value()) {
+ return;
+ }
+
+ // transform pointcloud
+ geometry_msgs::msg::TransformStamped transform_stamped{};
+ try {
+ transform_stamped = tf_buffer_->lookupTransform(
+ "map", msg_ptr->header.frame_id, node_->now(), rclcpp::Duration::from_seconds(0.1));
+ } catch (tf2::TransformException & ex) {
+ RCLCPP_ERROR_STREAM(
+ node_->get_logger(),
+ "Failed to look up transform from " << msg_ptr->header.frame_id << " to map");
+ return;
+ }
+
+ // transform by using eigen matrix
+ PointCloud2 transformed_points{};
+ const Eigen::Matrix4f affine_matrix =
+ tf2::transformToEigen(transform_stamped.transform).matrix().cast<float>();
+ pcl_ros::transformPointCloud(affine_matrix, *msg_ptr, transformed_points);
+
+ pcl::PointCloud<pcl::PointXYZ> pcl_pointcloud;
+ pcl::fromROSMsg(transformed_points, pcl_pointcloud);
+
+ if (searchPointcloudNearEntity(pcl_pointcloud)) {
+ std::get<PointCloud2Buffer>(variant) = *msg_ptr;
+ RCLCPP_INFO(node_->get_logger(), "%s reacted without published time", node_name.c_str());
+ mutex_.lock();
+ message_buffers_[node_name] = variant;
+ mutex_.unlock();
+ }
+}
+void SubscriberBase::pointcloud2OutputCallback(
+ const std::string & node_name, const PointCloud2::ConstSharedPtr & msg_ptr,
+ const PublishedTime::ConstSharedPtr & published_time_ptr)
+{
+ mutex_.lock();
+ auto variant = message_buffers_[node_name];
+ if (!std::holds_alternative<PointCloud2Buffer>(variant)) {
+ PointCloud2Buffer buffer(std::nullopt);
+ variant = buffer;
+ message_buffers_[node_name] = variant;
+ }
+ mutex_.unlock();
+
+ if (!spawn_object_cmd_ || std::get<PointCloud2Buffer>(variant).has_value()) {
+ return;
+ }
+
+ // transform pointcloud
+ geometry_msgs::msg::TransformStamped transform_stamped{};
+ try {
+ transform_stamped = tf_buffer_->lookupTransform(
+ "map", msg_ptr->header.frame_id, node_->now(), rclcpp::Duration::from_seconds(0.1));
+ } catch (tf2::TransformException & ex) {
+ RCLCPP_ERROR_STREAM(
+ node_->get_logger(),
+ "Failed to look up transform from " << msg_ptr->header.frame_id << " to map");
+ return;
+ }
+
+ // transform by using eigen matrix
+ PointCloud2 transformed_points{};
+ const Eigen::Matrix4f affine_matrix =
+ tf2::transformToEigen(transform_stamped.transform).matrix().cast<float>();
+ pcl_ros::transformPointCloud(affine_matrix, *msg_ptr, transformed_points);
+
+ pcl::PointCloud<pcl::PointXYZ> pcl_pointcloud;
+ pcl::fromROSMsg(transformed_points, pcl_pointcloud);
+
+ if (searchPointcloudNearEntity(pcl_pointcloud)) {
+ std::get<PointCloud2Buffer>(variant) = *msg_ptr;
+ // set published time
+ std::get<PointCloud2Buffer>(variant)->header.stamp = published_time_ptr->published_stamp;
+ RCLCPP_INFO(node_->get_logger(), "%s reacted with published time", node_name.c_str());
+ mutex_.lock();
+ message_buffers_[node_name] = variant;
+ mutex_.unlock();
+ }
+}
+void SubscriberBase::predictedObjectsOutputCallback(
+ const std::string & node_name, const PredictedObjects::ConstSharedPtr & msg_ptr)
+{
+ mutex_.lock();
+ auto variant = message_buffers_[node_name];
+ if (!std::holds_alternative<PredictedObjectsBuffer>(variant)) {
+ PredictedObjectsBuffer buffer(std::nullopt);
+ variant = buffer;
+ message_buffers_[node_name] = variant;
+ }
+ mutex_.unlock();
+
+ if (
+ !spawn_object_cmd_ || msg_ptr->objects.empty() ||
+ std::get<PredictedObjectsBuffer>(variant).has_value()) {
+ return;
+ }
+
+ if (searchPredictedObjectsNearEntity(*msg_ptr)) {
+ std::get<PredictedObjectsBuffer>(variant) = *msg_ptr;
+ RCLCPP_INFO(node_->get_logger(), "%s reacted without published time", node_name.c_str());
+
+ mutex_.lock();
+ message_buffers_[node_name] = variant;
+ mutex_.unlock();
+ }
+}
+
+void SubscriberBase::predictedObjectsOutputCallback(
+ const std::string & node_name, const PredictedObjects::ConstSharedPtr & msg_ptr,
+ const PublishedTime::ConstSharedPtr & published_time_ptr)
+{
+ mutex_.lock();
+ auto variant = message_buffers_[node_name];
+ if (!std::holds_alternative<PredictedObjectsBuffer>(variant)) {
+ PredictedObjectsBuffer buffer(std::nullopt);
+ variant = buffer;
+ message_buffers_[node_name] = variant;
+ }
+ mutex_.unlock();
+
+ if (
+ !spawn_object_cmd_ || msg_ptr->objects.empty() ||
+ std::get<PredictedObjectsBuffer>(variant).has_value()) {
+ return;
+ }
+
+ if (searchPredictedObjectsNearEntity(*msg_ptr)) {
+ std::get<PredictedObjectsBuffer>(variant) = *msg_ptr;
+ RCLCPP_INFO(node_->get_logger(), "%s reacted with published time", node_name.c_str());
+
+ // set published time
+ std::get<PredictedObjectsBuffer>(variant)->header.stamp = published_time_ptr->published_stamp;
+ mutex_.lock();
+ message_buffers_[node_name] = variant;
+ mutex_.unlock();
+ }
+}
+
+void SubscriberBase::detectedObjectsOutputCallback(
+ const std::string & node_name, const DetectedObjects::ConstSharedPtr & msg_ptr)
+{
+ mutex_.lock();
+ auto variant = message_buffers_[node_name];
+ if (!std::holds_alternative<DetectedObjectsBuffer>(variant)) {
+ DetectedObjectsBuffer buffer(std::nullopt);
+ variant = buffer;
+ message_buffers_[node_name] = variant;
+ }
+ mutex_.unlock();
+ if (
+ !spawn_object_cmd_ || msg_ptr->objects.empty() ||
+ std::get<DetectedObjectsBuffer>(variant).has_value()) {
+ return;
+ }
+
+ // transform objects
+ geometry_msgs::msg::TransformStamped transform_stamped{};
+ try {
+ transform_stamped = tf_buffer_->lookupTransform(
+ "map", msg_ptr->header.frame_id, node_->now(), rclcpp::Duration::from_seconds(0.1));
+ } catch (tf2::TransformException & ex) {
+ RCLCPP_ERROR_STREAM(
+ node_->get_logger(),
+ "Failed to look up transform from " << msg_ptr->header.frame_id << " to map");
+ return;
+ }
+
+ DetectedObjects output_objs;
+ output_objs = *msg_ptr;
+ for (auto & obj : output_objs.objects) {
+ geometry_msgs::msg::PoseStamped output_stamped, input_stamped;
+ input_stamped.pose = obj.kinematics.pose_with_covariance.pose;
+ tf2::doTransform(input_stamped, output_stamped, transform_stamped);
+ obj.kinematics.pose_with_covariance.pose = output_stamped.pose;
+ }
+ if (searchDetectedObjectsNearEntity(output_objs)) {
+ std::get<DetectedObjectsBuffer>(variant) = *msg_ptr;
+ RCLCPP_INFO(node_->get_logger(), "%s reacted without published time", node_name.c_str());
+ mutex_.lock();
+ message_buffers_[node_name] = variant;
+ mutex_.unlock();
+ }
+}
+
+void SubscriberBase::detectedObjectsOutputCallback(
+ const std::string & node_name, const DetectedObjects::ConstSharedPtr & msg_ptr,
+ const PublishedTime::ConstSharedPtr & published_time_ptr)
+{
+ mutex_.lock();
+ auto variant = message_buffers_[node_name];
+ if (!std::holds_alternative<DetectedObjectsBuffer>(variant)) {
+ DetectedObjectsBuffer buffer(std::nullopt);
+ variant = buffer;
+ message_buffers_[node_name] = variant;
+ }
+ mutex_.unlock();
+ if (
+ !spawn_object_cmd_ || msg_ptr->objects.empty() ||
+ std::get<DetectedObjectsBuffer>(variant).has_value()) {
+ return;
+ }
+
+ // transform objects
+ geometry_msgs::msg::TransformStamped transform_stamped{};
+ try {
+ transform_stamped = tf_buffer_->lookupTransform(
+ "map", msg_ptr->header.frame_id, node_->now(), rclcpp::Duration::from_seconds(0.1));
+ } catch (tf2::TransformException & ex) {
+ RCLCPP_ERROR_STREAM(
+ node_->get_logger(),
+ "Failed to look up transform from " << msg_ptr->header.frame_id << " to map");
+ return;
+ }
+
+ DetectedObjects output_objs;
+ output_objs = *msg_ptr;
+ for (auto & obj : output_objs.objects) {
+ geometry_msgs::msg::PoseStamped output_stamped, input_stamped;
+ input_stamped.pose = obj.kinematics.pose_with_covariance.pose;
+ tf2::doTransform(input_stamped, output_stamped, transform_stamped);
+ obj.kinematics.pose_with_covariance.pose = output_stamped.pose;
+ }
+ if (searchDetectedObjectsNearEntity(output_objs)) {
+ std::get<DetectedObjectsBuffer>(variant) = *msg_ptr;
+ RCLCPP_INFO(node_->get_logger(), "%s reacted with published time", node_name.c_str());
+
+ // set published time
+ std::get<DetectedObjectsBuffer>(variant)->header.stamp = published_time_ptr->published_stamp;
+ mutex_.lock();
+ message_buffers_[node_name] = variant;
+ mutex_.unlock();
+ }
+}
+
+void SubscriberBase::trackedObjectsOutputCallback(
+ const std::string & node_name, const TrackedObjects::ConstSharedPtr & msg_ptr)
+{
+ mutex_.lock();
+ auto variant = message_buffers_[node_name];
+ if (!std::holds_alternative<TrackedObjectsBuffer>(variant)) {
+ TrackedObjectsBuffer buffer(std::nullopt);
+ variant = buffer;
+ message_buffers_[node_name] = variant;
+ }
+ mutex_.unlock();
+ if (
+ !spawn_object_cmd_ || msg_ptr->objects.empty() ||
+ std::get<TrackedObjectsBuffer>(variant).has_value()) {
+ return;
+ }
+
+ if (searchTrackedObjectsNearEntity(*msg_ptr)) {
+ std::get<TrackedObjectsBuffer>(variant) = *msg_ptr;
+ RCLCPP_INFO(node_->get_logger(), "Reacted %s", node_name.c_str());
+ mutex_.lock();
+ message_buffers_[node_name] = variant;
+ mutex_.unlock();
+ }
+}
+
+void SubscriberBase::trackedObjectsOutputCallback(
+ const std::string & node_name, const TrackedObjects::ConstSharedPtr & msg_ptr,
+ const PublishedTime::ConstSharedPtr & published_time_ptr)
+{
+ mutex_.lock();
+ auto variant = message_buffers_[node_name];
+ if (!std::holds_alternative<TrackedObjectsBuffer>(variant)) {
+ TrackedObjectsBuffer buffer(std::nullopt);
+ variant = buffer;
+ message_buffers_[node_name] = variant;
+ }
+ mutex_.unlock();
+ if (
+ !spawn_object_cmd_ || msg_ptr->objects.empty() ||
+ std::get<TrackedObjectsBuffer>(variant).has_value()) {
+ return;
+ }
+
+ if (searchTrackedObjectsNearEntity(*msg_ptr)) {
+ std::get<TrackedObjectsBuffer>(variant) = *msg_ptr;
+ RCLCPP_INFO(node_->get_logger(), "%s reacted with published time", node_name.c_str());
+ // set published time
+ std::get<TrackedObjectsBuffer>(variant)->header.stamp = published_time_ptr->published_stamp;
+ mutex_.lock();
+ message_buffers_[node_name] = variant;
+ mutex_.unlock();
+ }
+}
+
+bool SubscriberBase::searchPointcloudNearEntity(
+ const pcl::PointCloud<pcl::PointXYZ> & pcl_pointcloud)
+{
+ bool isAnyPointWithinRadius = std::any_of(
+ pcl_pointcloud.points.begin(), pcl_pointcloud.points.end(), [this](const auto & point) {
+ return tier4_autoware_utils::calcDistance3d(entity_pose_.position, point) <=
+ reaction_params_.search_entity_params.search_radius;
+ });
+
+ if (isAnyPointWithinRadius) {
+ return true;
+ }
+ return false;
+}
+
+bool SubscriberBase::searchPredictedObjectsNearEntity(const PredictedObjects & predicted_objects)
+{
+ bool isAnyObjectWithinRadius = std::any_of(
+ predicted_objects.objects.begin(), predicted_objects.objects.end(),
+ [this](const PredictedObject & object) {
+ return tier4_autoware_utils::calcDistance3d(
+ entity_pose_.position,
+ object.kinematics.initial_pose_with_covariance.pose.position) <=
+ reaction_params_.search_entity_params.search_radius;
+ });
+
+ if (isAnyObjectWithinRadius) {
+ return true;
+ }
+ return false;
+}
+
+bool SubscriberBase::searchDetectedObjectsNearEntity(const DetectedObjects & detected_objects)
+{
+ bool isAnyObjectWithinRadius = std::any_of(
+ detected_objects.objects.begin(), detected_objects.objects.end(),
+ [this](const DetectedObject & object) {
+ return tier4_autoware_utils::calcDistance3d(
+ entity_pose_.position, object.kinematics.pose_with_covariance.pose.position) <=
+ reaction_params_.search_entity_params.search_radius;
+ });
+
+ if (isAnyObjectWithinRadius) {
+ return true;
+ }
+ return false;
+}
+
+bool SubscriberBase::searchTrackedObjectsNearEntity(const TrackedObjects & tracked_objects)
+{
+ bool isAnyObjectWithinRadius = std::any_of(
+ tracked_objects.objects.begin(), tracked_objects.objects.end(),
+ [this](const TrackedObject & object) {
+ return tier4_autoware_utils::calcDistance3d(
+ entity_pose_.position, object.kinematics.pose_with_covariance.pose.position) <=
+ reaction_params_.search_entity_params.search_radius;
+ });
+
+ if (isAnyObjectWithinRadius) {
+ return true;
+ }
+ return false;
+}
+
+std::optional<size_t> SubscriberBase::findFirstBrakeIdx(
+ const std::vector<AckermannControlCommand> & cmd_array)
+{
+ if (
+ cmd_array.size() <
+ reaction_params_.first_brake_params.min_number_descending_order_control_cmd) {
+ RCLCPP_WARN_ONCE(
+ node_->get_logger(),
+ "Control command buffer size is less than the minimum required size for first brake check");
+ return {};
+ }
+
+ for (size_t i = 0;
+ i < cmd_array.size() -
+ reaction_params_.first_brake_params.min_number_descending_order_control_cmd + 1;
+ ++i) {
+ size_t decreased_cmd_counter = 1; // because # of the decreased cmd = iteration + 1
+ for (size_t j = i; j < cmd_array.size() - 1; ++j) {
+ const auto & cmd_first = cmd_array.at(j).longitudinal;
+ const auto & cmd_second = cmd_array.at(j + 1).longitudinal;
+ constexpr double jerk_time_epsilon = 0.001;
+ const auto jerk =
+ abs(cmd_second.acceleration - cmd_first.acceleration) /
+ std::max(
+ (rclcpp::Time(cmd_second.stamp) - rclcpp::Time(cmd_first.stamp)).seconds(),
+ jerk_time_epsilon);
+
+ if (
+ (cmd_second.acceleration < cmd_first.acceleration) &&
+ (jerk > reaction_params_.first_brake_params.min_jerk_for_brake_cmd)) {
+ decreased_cmd_counter++;
+ } else {
+ break;
+ }
+ }
+ if (
+ decreased_cmd_counter <
+ static_cast<size_t>(
+ reaction_params_.first_brake_params.min_number_descending_order_control_cmd))
+ continue;
+ if (reaction_params_.first_brake_params.debug_control_commands) {
+ std::stringstream ss;
+
+ // debug print to show the first brake command in the all control commands
+ for (size_t k = 0; k < cmd_array.size(); ++k) {
+ if (k == i + 1) {
+ ss << "First Brake(" << cmd_array.at(k).longitudinal.acceleration << ") ";
+ } else {
+ ss << cmd_array.at(k).longitudinal.acceleration << " ";
+ }
+ }
+
+ RCLCPP_INFO(node_->get_logger(), "%s", ss.str().c_str());
+ }
+ return i + 1;
+ }
+ return {};
+}
+
+void SubscriberBase::setControlCommandToBuffer(
+ std::vector<AckermannControlCommand> & buffer, const AckermannControlCommand & cmd)
+{
+ const auto last_cmd_time = cmd.stamp;
+ if (!buffer.empty()) {
+ for (auto itr = buffer.begin(); itr != buffer.end();) {
+ const auto expired = (rclcpp::Time(last_cmd_time) - rclcpp::Time(itr->stamp)).seconds() >
+ reaction_params_.first_brake_params.control_cmd_buffer_time_interval;
+
+ if (expired) {
+ itr = buffer.erase(itr);
+ continue;
+ }
+
+ itr++;
+ }
+ }
+ buffer.emplace_back(cmd);
+}
+} // namespace reaction_analyzer::subscriber
diff --git a/tools/reaction_analyzer/src/topic_publisher.cpp b/tools/reaction_analyzer/src/topic_publisher.cpp
index 6f2d27beb3e5d..079bc8fb057e5 100644
--- a/tools/reaction_analyzer/src/topic_publisher.cpp
+++ b/tools/reaction_analyzer/src/topic_publisher.cpp
@@ -145,17 +145,6 @@ void TopicPublisher::reset()
void TopicPublisher::initRosbagPublishers()
{
- // Necessary lambda functions for string manipulation and message type conversion
- auto split = [](const std::string & str, const char delim) {
- std::vector<std::string> elems;
- std::stringstream ss(str);
- std::string item;
- while (std::getline(ss, item, delim)) {
- elems.push_back(item);
- }
- return elems;
- };
-
auto string_to_publisher_message_type = [](const std::string & input) {
if (input == "sensor_msgs/msg/PointCloud2") {
return PublisherMessageType::PointCloud2;
@@ -223,6 +212,7 @@ void TopicPublisher::initRosbagPublishers()
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
@@ -498,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;
}
};
@@ -780,14 +770,13 @@ void TopicPublisher::initRosbagPublishers()
}
}
- // initialize timers and message publishers
-
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; // Create a reference to topic_name for capture
+ 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));
@@ -825,11 +814,7 @@ void TopicPublisher::initRosbagPublishers()
variant);
}
- // Set the pointcloud publisher
-
- const auto period_pointcloud_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
- std::chrono::duration<double>(topic_publisher_params_.pointcloud_publisher_period));
-
+ // Set the point cloud publisher timers
if (pointcloud_variables_map.empty()) {
RCLCPP_ERROR(node_->get_logger(), "No pointcloud publishers found!");
rclcpp::shutdown();
@@ -856,6 +841,9 @@ void TopicPublisher::initRosbagPublishers()
}
// 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) {
// Create 1 timer to publish all PointCloud2 messages
pointcloud_sync_publish_timer_ = node_->create_wall_timer(period_pointcloud_ns, [this]() {
@@ -888,5 +876,4 @@ void TopicPublisher::initRosbagPublishers()
one_shot_timer_shared_ptr_ = one_shot_timer; // Store a weak pointer to the timer
}
}
-
} // namespace reaction_analyzer::topic_publisher
diff --git a/tools/reaction_analyzer/src/utils.cpp b/tools/reaction_analyzer/src/utils.cpp
new file mode 100644
index 0000000000000..9b9775e479d14
--- /dev/null
+++ b/tools/reaction_analyzer/src/utils.cpp
@@ -0,0 +1,85 @@
+// Copyright 2024 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "utils.hpp"
+
+namespace reaction_analyzer
+{
+rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node)
+{
+ rclcpp::CallbackGroup::SharedPtr callback_group =
+ node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+
+ auto sub_opt = rclcpp::SubscriptionOptions();
+ sub_opt.callback_group = callback_group;
+
+ return sub_opt;
+}
+
+std::vector<std::tuple<std::string, std::vector<double>, double>> sortResultsByMedian(
+ const std::unordered_map<std::string, std::vector<double>> test_results)
+{
+ std::vector<std::tuple<std::string, std::vector<double>, double>> sorted_data;
+ for (const auto & pair : test_results) {
+ auto vec = pair.second;
+
+ // Calculate the median
+ std::sort(vec.begin(), vec.end());
+ double median = 0.0;
+ size_t size = vec.size();
+ if (size % 2 == 0) {
+ median = (vec[size / 2 - 1] + vec[size / 2]) / 2.0;
+ } else {
+ median = vec[size / 2];
+ }
+
+ sorted_data.emplace_back(pair.first, pair.second, median);
+ }
+
+ // Sort based on the computed median
+ std::sort(sorted_data.begin(), sorted_data.end(), [](const auto & a, const auto & b) {
+ return std::get<2>(a) < std::get<2>(b); // Change to > for descending order
+ });
+
+ return sorted_data;
+}
+
+std::vector<std::string> split(const std::string & str, char delimiter)
+{
+ std::vector<std::string> elements;
+ std::stringstream ss(str);
+ std::string item;
+ while (std::getline(ss, item, delimiter)) {
+ elements.push_back(item);
+ }
+ return elements;
+}
+
+size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, const double distance)
+{
+ // Get Current Trajectory Point
+ const TrajectoryPoint & curr_p = traj.points.at(curr_id);
+
+ size_t target_id = curr_id;
+ double current_distance = 0.0;
+ for (size_t traj_id = curr_id + 1; traj_id < traj.points.size(); ++traj_id) {
+ current_distance = tier4_autoware_utils::calcDistance3d(traj.points.at(traj_id), curr_p);
+ if (current_distance >= distance) {
+ break;
+ }
+ target_id = traj_id;
+ }
+ return target_id;
+}
+} // namespace reaction_analyzer