Skip to content

Commit 516a2cc

Browse files
committed
fix: ci/cd
Signed-off-by: Berkay Karaman <brkay54@gmail.com>
1 parent 8397ddb commit 516a2cc

9 files changed

+641
-581
lines changed

tools/reaction_analyzer/CMakeLists.txt

-3
Original file line numberDiff line numberDiff line change
@@ -33,9 +33,6 @@ rclcpp_components_register_node(reaction_analyzer
3333
EXECUTABLE reaction_analyzer_exe
3434
)
3535

36-
if (BUILD_TESTING)
37-
endif ()
38-
3936
ament_auto_package(
4037
INSTALL_TO_SHARE
4138
param

tools/reaction_analyzer/include/reaction_analyzer_node.hpp

+10-3
Original file line numberDiff line numberDiff line change
@@ -66,8 +66,7 @@ struct NodeParams
6666
class ReactionAnalyzerNode : public rclcpp::Node
6767
{
6868
public:
69-
explicit ReactionAnalyzerNode(rclcpp::NodeOptions options);
70-
~ReactionAnalyzerNode() = default;
69+
explicit ReactionAnalyzerNode(rclcpp::NodeOptions node_options);
7170

7271
Odometry::ConstSharedPtr odometry_ptr_;
7372

@@ -107,12 +106,20 @@ class ReactionAnalyzerNode : public rclcpp::Node
107106

108107
// Functions
109108
void init_analyzer_variables();
110-
void init_ego(
109+
void init_test_env(
111110
const LocalizationInitializationState::ConstSharedPtr & initialization_state_ptr,
112111
const RouteState::ConstSharedPtr & route_state_ptr,
113112
const OperationModeState::ConstSharedPtr & operation_mode_ptr,
114113
const PoseStamped::ConstSharedPtr & ground_truth_pose_ptr,
115114
const Odometry::ConstSharedPtr & odometry_ptr);
115+
bool init_ego(
116+
const LocalizationInitializationState::ConstSharedPtr & initialization_state_ptr,
117+
const Odometry::ConstSharedPtr & odometry_ptr, const rclcpp::Time & current_time);
118+
bool set_route(
119+
const RouteState::ConstSharedPtr & route_state_ptr, const rclcpp::Time & current_time);
120+
bool check_ego_init_correctly(
121+
const PoseStamped::ConstSharedPtr & ground_truth_pose_ptr,
122+
const Odometry::ConstSharedPtr & odometry_ptr);
116123
void call_operation_mode_service_without_response();
117124
void spawn_obstacle(const geometry_msgs::msg::Point & ego_pose);
118125
void calculate_results(

tools/reaction_analyzer/include/subscriber.hpp

+7-30
Original file line numberDiff line numberDiff line change
@@ -77,25 +77,6 @@ using SubscriberVariablesVariant = std::variant<
7777
SubscriberVariables<TrackedObjects>, SubscriberVariables<PredictedObjects>,
7878
SubscriberVariables<Trajectory>, SubscriberVariables<AckermannControlCommand>>;
7979

80-
// The supported message types
81-
enum class SubscriberMessageType {
82-
UNKNOWN = 0,
83-
ACKERMANN_CONTROL_COMMAND = 1,
84-
TRAJECTORY = 2,
85-
POINTCLOUD2 = 3,
86-
DETECTED_OBJECTS = 4,
87-
PREDICTED_OBJECTS = 5,
88-
TRACKED_OBJECTS = 6,
89-
};
90-
91-
// Reaction Types
92-
enum class ReactionType {
93-
UNKNOWN = 0,
94-
FIRST_BRAKE = 1,
95-
SEARCH_ZERO_VEL = 2,
96-
SEARCH_ENTITY = 3,
97-
};
98-
9980
// The configuration of the topic to be subscribed which are defined in reaction_chain
10081
struct TopicConfig
10182
{
@@ -141,13 +122,7 @@ class SubscriberBase
141122
rclcpp::Node * node, Odometry::ConstSharedPtr & odometry, std::atomic<bool> & spawn_object_cmd,
142123
const EntityParams & entity_params);
143124

144-
// Instances of SubscriberBase cannot be copied
145-
SubscriberBase(const SubscriberBase &) = delete;
146-
SubscriberBase & operator=(const SubscriberBase &) = delete;
147-
148-
~SubscriberBase() = default;
149-
150-
std::optional<std::unordered_map<std::string, MessageBufferVariant>> getMessageBuffersMap();
125+
std::optional<std::unordered_map<std::string, MessageBufferVariant>> get_message_buffers_map();
151126
void reset();
152127

153128
private:
@@ -160,10 +135,10 @@ class SubscriberBase
160135
EntityParams entity_params_;
161136

162137
// Variables to be initialized in constructor
163-
ChainModules chain_modules_;
138+
ChainModules chain_modules_{};
164139
ReactionParams reaction_params_{};
165-
geometry_msgs::msg::Pose entity_pose_;
166-
double entity_search_radius_;
140+
geometry_msgs::msg::Pose entity_pose_{};
141+
double entity_search_radius_{0.0};
167142

168143
// Variants
169144
std::unordered_map<std::string, SubscriberVariablesVariant> subscriber_variables_map_;
@@ -176,10 +151,12 @@ class SubscriberBase
176151
// Functions
177152
void init_reaction_chains_and_params();
178153
bool init_subscribers();
154+
std::optional<SubscriberVariablesVariant> get_subscriber_variable(
155+
const TopicConfig & topic_config);
179156
std::optional<size_t> find_first_brake_idx(
180157
const std::vector<AckermannControlCommand> & cmd_array);
181158
void set_control_command_to_buffer(
182-
std::vector<AckermannControlCommand> & buffer, const AckermannControlCommand & cmd);
159+
std::vector<AckermannControlCommand> & buffer, const AckermannControlCommand & cmd) const;
183160

184161
// Callbacks for modules are subscribed
185162
void on_control_command(

tools/reaction_analyzer/include/topic_publisher.hpp

+11-28
Original file line numberDiff line numberDiff line change
@@ -55,23 +55,6 @@ using geometry_msgs::msg::Pose;
5555
using nav_msgs::msg::Odometry;
5656
using sensor_msgs::msg::PointCloud2;
5757

58-
enum class PublisherMessageType {
59-
UNKNOWN = 0,
60-
CAMERA_INFO = 1,
61-
IMAGE = 2,
62-
POINTCLOUD2 = 3,
63-
POSE_WITH_COVARIANCE_STAMPED = 4,
64-
POSE_STAMPED = 5,
65-
ODOMETRY = 6,
66-
IMU = 7,
67-
CONTROL_MODE_REPORT = 8,
68-
GEAR_REPORT = 9,
69-
HAZARD_LIGHTS_REPORT = 10,
70-
STEERING_REPORT = 11,
71-
TURN_INDICATORS_REPORT = 12,
72-
VELOCITY_REPORT = 13,
73-
};
74-
7558
struct TopicPublisherParams
7659
{
7760
double dummy_perception_publisher_period; // Only for planning_control mode
@@ -110,23 +93,23 @@ struct PublisherVarAccessor
11093
{
11194
// Template struct to check if a type has a header member.
11295
template <typename T, typename = std::void_t<>>
113-
struct has_header : std::false_type
96+
struct HasHeader : std::false_type
11497
{
11598
};
11699

117100
template <typename T>
118-
struct has_header<T, std::void_t<decltype(T::header)>> : std::true_type
101+
struct HasHeader<T, std::void_t<decltype(T::header)>> : std::true_type
119102
{
120103
};
121104

122105
// Template struct to check if a type has a stamp member.
123106
template <typename T, typename = std::void_t<>>
124-
struct has_stamp : std::false_type
107+
struct HasStamp : std::false_type
125108
{
126109
};
127110

128111
template <typename T>
129-
struct has_stamp<T, std::void_t<decltype(T::stamp)>> : std::true_type
112+
struct HasStamp<T, std::void_t<decltype(T::stamp)>> : std::true_type
130113
{
131114
};
132115

@@ -142,9 +125,9 @@ struct PublisherVarAccessor
142125
} else {
143126
*msg_to_be_published = *publisherVar.empty_area_message;
144127
}
145-
if constexpr (has_header<MessageType>::value) {
128+
if constexpr (HasHeader<MessageType>::value) {
146129
msg_to_be_published->header.stamp = current_time;
147-
} else if constexpr (has_stamp<MessageType>::value) {
130+
} else if constexpr (HasStamp<MessageType>::value) {
148131
msg_to_be_published->stamp = current_time;
149132
}
150133
publisherVar.publisher->publish(std::move(msg_to_be_published));
@@ -200,7 +183,6 @@ class TopicPublisher
200183
std::optional<rclcpp::Time> & spawn_cmd_time, const RunningMode & node_running_mode,
201184
const EntityParams & entity_params);
202185

203-
~TopicPublisher() = default;
204186
void reset();
205187

206188
private:
@@ -236,12 +218,13 @@ class TopicPublisher
236218
std::shared_ptr<rclcpp::TimerBase> one_shot_timer_shared_ptr_;
237219

238220
// Functions
239-
void set_message_to_variable_map(
221+
void set_message(
240222
const PublisherMessageType & message_type, const std::string & topic_name,
241223
rosbag2_storage::SerializedBagMessage & bag_message, const bool is_empty_area_message);
242-
void set_period_to_variable_map(
243-
const std::unordered_map<std::string, std::vector<rclcpp::Time>> & time_map);
244-
bool set_publishers_and_timers_to_variable_map();
224+
void set_period(const std::unordered_map<std::string, std::vector<rclcpp::Time>> & time_map);
225+
std::map<std::string, PublisherVariables<PointCloud2>> set_general_publishers_and_timers();
226+
void set_pointcloud_publishers_and_timers(
227+
const std::map<std::string, PublisherVariables<PointCloud2>> & pointcloud_variables_map);
245228
bool check_publishers_initialized_correctly();
246229
void init_rosbag_publishers();
247230
void pointcloud_messages_sync_publisher(const PointcloudPublisherType type);

tools/reaction_analyzer/include/utils.hpp

+67
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@
1818
#include <pcl/impl/point_types.hpp>
1919
#include <pcl_ros/transforms.hpp>
2020
#include <rclcpp/rclcpp.hpp>
21+
#include <rosbag2_storage/bag_metadata.hpp>
2122
#include <tier4_autoware_utils/geometry/geometry.hpp>
2223
#include <tier4_autoware_utils/math/unit_conversion.hpp>
2324

@@ -40,9 +41,11 @@
4041
#include <pcl_conversions/pcl_conversions.h>
4142

4243
#include <fstream>
44+
#include <map>
4345
#include <string>
4446
#include <tuple>
4547
#include <unordered_map>
48+
#include <utility>
4649
#include <vector>
4750

4851
// The reaction_analyzer namespace contains utility functions for the Reaction Analyzer tool.
@@ -82,6 +85,49 @@ using PipelineMap = std::map<rclcpp::Time, std::vector<ReactionPair>>;
8285
using TimestampedReactionPairsVector =
8386
std::vector<std::tuple<rclcpp::Time, std::vector<ReactionPair>>>;
8487

88+
/**
89+
* @brief Enum for the different types of messages that can be published.
90+
*/
91+
enum class PublisherMessageType {
92+
UNKNOWN = 0,
93+
CAMERA_INFO = 1,
94+
IMAGE = 2,
95+
POINTCLOUD2 = 3,
96+
POSE_WITH_COVARIANCE_STAMPED = 4,
97+
POSE_STAMPED = 5,
98+
ODOMETRY = 6,
99+
IMU = 7,
100+
CONTROL_MODE_REPORT = 8,
101+
GEAR_REPORT = 9,
102+
HAZARD_LIGHTS_REPORT = 10,
103+
STEERING_REPORT = 11,
104+
TURN_INDICATORS_REPORT = 12,
105+
VELOCITY_REPORT = 13,
106+
};
107+
108+
/**
109+
* @brief Enum for the different types of messages that can be subscribed to.
110+
*/
111+
enum class SubscriberMessageType {
112+
UNKNOWN = 0,
113+
ACKERMANN_CONTROL_COMMAND = 1,
114+
TRAJECTORY = 2,
115+
POINTCLOUD2 = 3,
116+
DETECTED_OBJECTS = 4,
117+
PREDICTED_OBJECTS = 5,
118+
TRACKED_OBJECTS = 6,
119+
};
120+
121+
/**
122+
* @brief Enum for the different types of reactions that can be analyzed.
123+
*/
124+
enum class ReactionType {
125+
UNKNOWN = 0,
126+
FIRST_BRAKE = 1,
127+
SEARCH_ZERO_VEL = 2,
128+
SEARCH_ENTITY = 3,
129+
};
130+
85131
/**
86132
* @brief Enum for the different running modes of the Reaction Analyzer.
87133
*/
@@ -128,6 +174,27 @@ struct LatencyStats
128174
double std_dev;
129175
};
130176

177+
/**
178+
* @brief Convert string to SubscriberMessageType.
179+
*/
180+
SubscriberMessageType get_subscriber_message_type(const std::string & message_type);
181+
182+
/**
183+
* @brief Convert string to PublisherMessageType.
184+
*/
185+
PublisherMessageType get_publisher_message_type(const std::string & message_type);
186+
187+
/**
188+
* @brief Get the PublisherMessageType for a given topic.
189+
*/
190+
PublisherMessageType get_publisher_message_type_for_topic(
191+
const std::vector<rosbag2_storage::TopicInformation> & topics, const std::string & topic_name);
192+
193+
/**
194+
* @brief Convert string to ReactionType.
195+
*/
196+
ReactionType get_reaction_type(const std::string & reaction_type);
197+
131198
/**
132199
* @brief Calculates the statistics of a vector of latencies.
133200
* @param latency_vec The vector of latencies.

0 commit comments

Comments
 (0)