Skip to content

Commit 97fb59b

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

9 files changed

+831
-1123
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

+12-5
Original file line numberDiff line numberDiff line change
@@ -28,10 +28,10 @@
2828
#include <nav_msgs/msg/odometry.hpp>
2929

3030
#include <algorithm>
31+
#include <map>
3132
#include <memory>
3233
#include <mutex>
3334
#include <string>
34-
#include <unordered_map>
3535
#include <utility>
3636
#include <variant>
3737
#include <vector>
@@ -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,16 +106,24 @@ 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(
119-
const std::unordered_map<std::string, subscriber::MessageBufferVariant> & message_buffers,
126+
const std::map<std::string, subscriber::MessageBufferVariant> & message_buffers,
120127
const rclcpp::Time & spawn_cmd_time);
121128
void on_timer();
122129
void reset();

tools/reaction_analyzer/include/subscriber.hpp

+10-33
Original file line numberDiff line numberDiff line change
@@ -29,10 +29,10 @@
2929
#include <tf2_ros/buffer.h>
3030
#include <tf2_ros/transform_listener.h>
3131

32+
#include <map>
3233
#include <memory>
3334
#include <mutex>
3435
#include <string>
35-
#include <unordered_map>
3636
#include <utility>
3737
#include <variant>
3838
#include <vector>
@@ -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::map<std::string, MessageBufferVariant>> get_message_buffers_map();
151126
void reset();
152127

153128
private:
@@ -160,14 +135,14 @@ 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
169-
std::unordered_map<std::string, SubscriberVariablesVariant> subscriber_variables_map_;
170-
std::unordered_map<std::string, MessageBufferVariant> message_buffers_;
144+
std::map<std::string, SubscriberVariablesVariant> subscriber_variables_map_;
145+
std::map<std::string, MessageBufferVariant> message_buffers_;
171146

172147
// tf
173148
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
@@ -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

+34-48
Original file line numberDiff line numberDiff line change
@@ -35,10 +35,10 @@
3535
#include <tf2_ros/buffer.h>
3636
#include <tf2_ros/transform_listener.h>
3737

38+
#include <map>
3839
#include <memory>
3940
#include <mutex>
4041
#include <string>
41-
#include <unordered_map>
4242
#include <utility>
4343
#include <variant>
4444
#include <vector>
@@ -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,68 +93,68 @@ 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

133-
template <typename MessageType>
116+
template <typename T>
134117
void publish_with_current_time(
135-
const PublisherVariables<MessageType> & publisherVar, const rclcpp::Time & current_time,
118+
const PublisherVariables<T> & publisher_var, const rclcpp::Time & current_time,
136119
const bool is_object_spawned) const
137120
{
138-
std::unique_ptr<MessageType> msg_to_be_published = std::make_unique<MessageType>();
121+
std::unique_ptr<T> msg_to_be_published = std::make_unique<T>();
139122

140123
if (is_object_spawned) {
141-
*msg_to_be_published = *publisherVar.object_spawned_message;
124+
*msg_to_be_published = *publisher_var.object_spawned_message;
142125
} else {
143-
*msg_to_be_published = *publisherVar.empty_area_message;
126+
*msg_to_be_published = *publisher_var.empty_area_message;
144127
}
145-
if constexpr (has_header<MessageType>::value) {
128+
if constexpr (HasHeader<T>::value) {
146129
msg_to_be_published->header.stamp = current_time;
147-
} else if constexpr (has_stamp<MessageType>::value) {
130+
} else if constexpr (HasStamp<T>::value) {
148131
msg_to_be_published->stamp = current_time;
149132
}
150-
publisherVar.publisher->publish(std::move(msg_to_be_published));
133+
publisher_var.publisher->publish(std::move(msg_to_be_published));
151134
}
152135

153136
template <typename T>
154-
void set_period(T & publisherVar, std::chrono::milliseconds new_period)
137+
void set_period(T & publisher_var, std::chrono::milliseconds new_period)
155138
{
156-
publisherVar.period_ms = new_period;
139+
publisher_var.period_ms = new_period;
157140
}
158141

159142
template <typename T>
160-
std::chrono::milliseconds get_period(const T & publisherVar) const
143+
std::chrono::milliseconds get_period(const T & publisher_var) const
161144
{
162-
return publisherVar.period_ms;
145+
return publisher_var.period_ms;
163146
}
164147

165148
template <typename T>
166-
std::shared_ptr<void> get_empty_area_message(const T & publisherVar) const
149+
std::shared_ptr<void> get_empty_area_message(const T & publisher_var) const
167150
{
168-
return std::static_pointer_cast<void>(publisherVar.empty_area_message);
151+
return std::static_pointer_cast<void>(publisher_var.empty_area_message);
169152
}
170153

171154
template <typename T>
172-
std::shared_ptr<void> get_object_spawned_message(const T & publisherVar) const
155+
std::shared_ptr<void> get_object_spawned_message(const T & publisher_var) const
173156
{
174-
return std::static_pointer_cast<void>(publisherVar.object_spawned_message);
157+
return std::static_pointer_cast<void>(publisher_var.object_spawned_message);
175158
}
176159
};
177160

@@ -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:
@@ -229,21 +211,25 @@ class TopicPublisher
229211

230212
// Variables perception_planning mode
231213
PointcloudPublisherType pointcloud_publisher_type_;
232-
std::unordered_map<std::string, PublisherVariablesVariant> topic_publisher_map_;
233-
std::unordered_map<std::string, LidarOutputPair>
214+
std::map<std::string, PublisherVariablesVariant> topic_publisher_map_;
215+
std::map<std::string, LidarOutputPair>
234216
lidar_pub_variable_pair_map_; // used to publish pointcloud_raw and pointcloud_raw_ex
235217
bool is_object_spawned_message_published_{false};
236218
std::shared_ptr<rclcpp::TimerBase> one_shot_timer_shared_ptr_;
237219

238220
// Functions
239-
void set_message_to_variable_map(
240-
const PublisherMessageType & message_type, const std::string & topic_name,
241-
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();
221+
template <typename MessageType>
222+
void set_message(
223+
const std::string & topic_name, const rosbag2_storage::SerializedBagMessage & bag_message,
224+
const bool is_empty_area_message);
225+
void set_period(const std::map<std::string, std::vector<rclcpp::Time>> & time_map);
226+
std::map<std::string, PublisherVariables<PointCloud2>> set_general_publishers_and_timers();
227+
void set_pointcloud_publishers_and_timers(
228+
const std::map<std::string, PublisherVariables<PointCloud2>> & pointcloud_variables_map);
245229
bool check_publishers_initialized_correctly();
246230
void init_rosbag_publishers();
231+
void init_rosbag_publishers_with_object(const std::string & path_bag_with_object);
232+
void init_rosbag_publishers_without_object(const std::string & path_bag_without_object);
247233
void pointcloud_messages_sync_publisher(const PointcloudPublisherType type);
248234
void pointcloud_messages_async_publisher(
249235
const std::pair<
@@ -253,7 +239,7 @@ class TopicPublisher
253239
void dummy_perception_publisher(); // Only for planning_control mode
254240

255241
// Timers
256-
std::unordered_map<std::string, rclcpp::TimerBase::SharedPtr> pointcloud_publish_timers_map_;
242+
std::map<std::string, rclcpp::TimerBase::SharedPtr> pointcloud_publish_timers_map_;
257243
rclcpp::TimerBase::SharedPtr pointcloud_sync_publish_timer_;
258244
rclcpp::TimerBase::SharedPtr dummy_perception_timer_;
259245
};

0 commit comments

Comments
 (0)