Skip to content

Commit 605839a

Browse files
committed
fix: clang-tidy
Signed-off-by: Berkay Karaman <brkay54@gmail.com>
1 parent 18edb52 commit 605839a

7 files changed

+60
-94
lines changed

tools/reaction_analyzer/include/reaction_analyzer_node.hpp

+1-2
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

tools/reaction_analyzer/include/subscriber.hpp

+2-8
Original file line numberDiff line numberDiff line change
@@ -141,13 +141,7 @@ class SubscriberBase
141141
rclcpp::Node * node, Odometry::ConstSharedPtr & odometry, std::atomic<bool> & spawn_object_cmd,
142142
const EntityParams & entity_params);
143143

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();
144+
std::optional<std::unordered_map<std::string, MessageBufferVariant>> get_message_buffers_map();
151145
void reset();
152146

153147
private:
@@ -179,7 +173,7 @@ class SubscriberBase
179173
std::optional<size_t> find_first_brake_idx(
180174
const std::vector<AckermannControlCommand> & cmd_array);
181175
void set_control_command_to_buffer(
182-
std::vector<AckermannControlCommand> & buffer, const AckermannControlCommand & cmd);
176+
std::vector<AckermannControlCommand> & buffer, const AckermannControlCommand & cmd) const;
183177

184178
// Callbacks for modules are subscribed
185179
void on_control_command(

tools/reaction_analyzer/include/topic_publisher.hpp

+6-7
Original file line numberDiff line numberDiff line change
@@ -110,23 +110,23 @@ struct PublisherVarAccessor
110110
{
111111
// Template struct to check if a type has a header member.
112112
template <typename T, typename = std::void_t<>>
113-
struct has_header : std::false_type
113+
struct HasHeader : std::false_type
114114
{
115115
};
116116

117117
template <typename T>
118-
struct has_header<T, std::void_t<decltype(T::header)>> : std::true_type
118+
struct HasHeader<T, std::void_t<decltype(T::header)>> : std::true_type
119119
{
120120
};
121121

122122
// Template struct to check if a type has a stamp member.
123123
template <typename T, typename = std::void_t<>>
124-
struct has_stamp : std::false_type
124+
struct HasStamp : std::false_type
125125
{
126126
};
127127

128128
template <typename T>
129-
struct has_stamp<T, std::void_t<decltype(T::stamp)>> : std::true_type
129+
struct HasStamp<T, std::void_t<decltype(T::stamp)>> : std::true_type
130130
{
131131
};
132132

@@ -142,9 +142,9 @@ struct PublisherVarAccessor
142142
} else {
143143
*msg_to_be_published = *publisherVar.empty_area_message;
144144
}
145-
if constexpr (has_header<MessageType>::value) {
145+
if constexpr (HasHeader<MessageType>::value) {
146146
msg_to_be_published->header.stamp = current_time;
147-
} else if constexpr (has_stamp<MessageType>::value) {
147+
} else if constexpr (HasStamp<MessageType>::value) {
148148
msg_to_be_published->stamp = current_time;
149149
}
150150
publisherVar.publisher->publish(std::move(msg_to_be_published));
@@ -200,7 +200,6 @@ class TopicPublisher
200200
std::optional<rclcpp::Time> & spawn_cmd_time, const RunningMode & node_running_mode,
201201
const EntityParams & entity_params);
202202

203-
~TopicPublisher() = default;
204203
void reset();
205204

206205
private:

tools/reaction_analyzer/src/reaction_analyzer_node.cpp

+16-17
Original file line numberDiff line numberDiff line change
@@ -51,8 +51,8 @@ void ReactionAnalyzerNode::on_ground_truth_pose(PoseStamped::ConstSharedPtr msg_
5151
ground_truth_pose_ptr_ = std::move(msg_ptr);
5252
}
5353

54-
ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions options)
55-
: Node("reaction_analyzer_node", options.automatically_declare_parameters_from_overrides(true))
54+
ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions node_options)
55+
: Node("reaction_analyzer_node", node_options.automatically_declare_parameters_from_overrides(true))
5656
{
5757
using std::placeholders::_1;
5858

@@ -173,7 +173,7 @@ void ReactionAnalyzerNode::on_timer()
173173
if (!spawn_cmd_time) return;
174174

175175
// Get the reacted messages, if all modules reacted
176-
const auto message_buffers = subscriber_ptr_->getMessageBuffersMap();
176+
const auto message_buffers = subscriber_ptr_->get_message_buffers_map();
177177

178178
if (message_buffers) {
179179
// if reacted, calculate the results
@@ -298,20 +298,19 @@ void ReactionAnalyzerNode::init_ego(
298298
RCLCPP_WARN(
299299
get_logger(), "Ground truth pose is not received. Waiting for Ground truth pose...");
300300
return;
301-
} else {
302-
constexpr double deviation_threshold = 0.1;
303-
const auto deviation = tier4_autoware_utils::calcPoseDeviation(
304-
ground_truth_pose_ptr->pose, odometry_ptr->pose.pose);
305-
if (
306-
deviation.longitudinal > deviation_threshold || deviation.lateral > deviation_threshold ||
307-
deviation.yaw > deviation_threshold) {
308-
RCLCPP_ERROR(
309-
get_logger(),
310-
"Deviation between ground truth position and ego position is high. Node is shutting "
311-
"down. Longitudinal deviation: %f, Lateral deviation: %f, Yaw deviation: %f",
312-
deviation.longitudinal, deviation.lateral, deviation.yaw);
313-
rclcpp::shutdown();
314-
}
301+
}
302+
constexpr double deviation_threshold = 0.1;
303+
const auto deviation = tier4_autoware_utils::calcPoseDeviation(
304+
ground_truth_pose_ptr->pose, odometry_ptr->pose.pose);
305+
if (
306+
deviation.longitudinal > deviation_threshold || deviation.lateral > deviation_threshold ||
307+
deviation.yaw > deviation_threshold) {
308+
RCLCPP_ERROR(
309+
get_logger(),
310+
"Deviation between ground truth position and ego position is high. Node is shutting "
311+
"down. Longitudinal deviation: %f, Lateral deviation: %f, Yaw deviation: %f",
312+
deviation.longitudinal, deviation.lateral, deviation.yaw);
313+
rclcpp::shutdown();
315314
}
316315
}
317316

tools/reaction_analyzer/src/subscriber.cpp

+8-10
Original file line numberDiff line numberDiff line change
@@ -40,7 +40,7 @@ SubscriberBase::SubscriberBase(
4040

4141
void SubscriberBase::init_reaction_chains_and_params()
4242
{
43-
auto stringToMessageType = [](const std::string & input) {
43+
auto string_to_message_type = [](const std::string & input) {
4444
if (input == "autoware_auto_control_msgs/msg/AckermannControlCommand") {
4545
return SubscriberMessageType::ACKERMANN_CONTROL_COMMAND;
4646
} else if (input == "autoware_auto_planning_msgs/msg/Trajectory") {
@@ -53,21 +53,19 @@ void SubscriberBase::init_reaction_chains_and_params()
5353
return SubscriberMessageType::DETECTED_OBJECTS;
5454
} else if (input == "autoware_auto_perception_msgs/msg/TrackedObjects") {
5555
return SubscriberMessageType::TRACKED_OBJECTS;
56-
} else {
57-
return SubscriberMessageType::UNKNOWN;
5856
}
57+
return SubscriberMessageType::UNKNOWN;
5958
};
6059

61-
auto stringToReactionType = [](const std::string & input) {
60+
auto string_to_reaction_type = [](const std::string & input) {
6261
if (input == "first_brake_params") {
6362
return ReactionType::FIRST_BRAKE;
6463
} else if (input == "search_zero_vel_params") {
6564
return ReactionType::SEARCH_ZERO_VEL;
6665
} else if (input == "search_entity_params") {
6766
return ReactionType::SEARCH_ENTITY;
68-
} else {
69-
return ReactionType::UNKNOWN;
7067
}
68+
return ReactionType::UNKNOWN;
7169
};
7270

7371
// Init Chains: get the topic addresses and message types of the modules in chain
@@ -82,7 +80,7 @@ void SubscriberBase::init_reaction_chains_and_params()
8280
tmp.time_debug_topic_address =
8381
node_->get_parameter_or(module_name + ".time_debug_topic_name", std::string(""));
8482
tmp.message_type =
85-
stringToMessageType(node_->get_parameter(module_name + ".message_type").as_string());
83+
string_to_message_type(node_->get_parameter(module_name + ".message_type").as_string());
8684
if (tmp.message_type != SubscriberMessageType::UNKNOWN) {
8785
chain_modules_.emplace_back(tmp);
8886
} else {
@@ -99,7 +97,7 @@ void SubscriberBase::init_reaction_chains_and_params()
9997
const auto module_names = node_->list_parameters({param_key}, 3).prefixes;
10098
for (const auto & module_name : module_names) {
10199
const auto splitted_name = split(module_name, '.');
102-
const auto type = stringToReactionType(splitted_name.back());
100+
const auto type = string_to_reaction_type(splitted_name.back());
103101
switch (type) {
104102
case ReactionType::FIRST_BRAKE: {
105103
reaction_params_.first_brake_params.debug_control_commands =
@@ -460,7 +458,7 @@ bool SubscriberBase::init_subscribers()
460458
}
461459

462460
std::optional<std::unordered_map<std::string, MessageBufferVariant>>
463-
SubscriberBase::getMessageBuffersMap()
461+
SubscriberBase::get_message_buffers_map()
464462
{
465463
std::lock_guard<std::mutex> lock(mutex_);
466464
if (message_buffers_.empty()) {
@@ -997,7 +995,7 @@ std::optional<size_t> SubscriberBase::find_first_brake_idx(
997995
}
998996

999997
void SubscriberBase::set_control_command_to_buffer(
1000-
std::vector<AckermannControlCommand> & buffer, const AckermannControlCommand & cmd)
998+
std::vector<AckermannControlCommand> & buffer, const AckermannControlCommand & cmd) const
1001999
{
10021000
const auto last_cmd_time = cmd.stamp;
10031001
if (!buffer.empty()) {

tools/reaction_analyzer/src/topic_publisher.cpp

+16-20
Original file line numberDiff line numberDiff line change
@@ -304,9 +304,8 @@ void TopicPublisher::init_rosbag_publishers()
304304
if (it != topics.end()) {
305305
return string_to_publisher_message_type(
306306
it->topic_metadata.type); // Return the message type if found
307-
} else {
308-
return PublisherMessageType::UNKNOWN; //
309307
}
308+
return PublisherMessageType::UNKNOWN;
310309
};
311310

312311
// Collect timestamps for each topic to set the frequency of the publishers
@@ -349,25 +348,24 @@ void TopicPublisher::init_rosbag_publishers()
349348

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

352-
auto getMessageTypeForTopic = [&topics, &string_to_publisher_message_type](
353-
const std::string & topicName) -> PublisherMessageType {
351+
auto get_message_type_for_topic = [&topics, &string_to_publisher_message_type](
352+
const std::string & topicName) -> PublisherMessageType {
354353
auto it = std::find_if(topics.begin(), topics.end(), [&topicName](const auto & topic) {
355354
return topic.topic_metadata.name == topicName;
356355
});
357356

358357
if (it != topics.end()) {
359358
return string_to_publisher_message_type(
360359
it->topic_metadata.type); // Return the message type if found
361-
} else {
362-
return PublisherMessageType::UNKNOWN;
363360
}
361+
return PublisherMessageType::UNKNOWN;
364362
};
365363

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

370-
const auto message_type = getMessageTypeForTopic(current_topic);
368+
const auto message_type = get_message_type_for_topic(current_topic);
371369

372370
if (message_type == PublisherMessageType::UNKNOWN) {
373371
continue;
@@ -869,10 +867,10 @@ void TopicPublisher::set_period_to_variable_map(
869867
auto period_ms = std::chrono::duration_cast<std::chrono::milliseconds>(total_duration_ns) /
870868
(timestamps_tmp.size() - 1);
871869

872-
PublisherVariablesVariant & publisherVar = topic_publisher_map_[topic_name];
870+
PublisherVariablesVariant & publisher_variant = topic_publisher_map_[topic_name];
873871
PublisherVarAccessor accessor;
874872

875-
std::visit([&](auto & var) { accessor.set_period(var, period_ms); }, publisherVar);
873+
std::visit([&](auto & var) { accessor.set_period(var, period_ms); }, publisher_variant);
876874
}
877875
}
878876
}
@@ -973,24 +971,21 @@ bool TopicPublisher::set_publishers_and_timers_to_variable_map()
973971
// Create a timer to create phase difference bw timers which will be created for each lidar
974972
// topics
975973
auto one_shot_timer = node_->create_wall_timer(phase_dif, [this, period_pointcloud_ns]() {
976-
for (const auto & [lidar_name, publisher_var_pair] : lidar_pub_variable_pair_map_) {
974+
for (const auto & publisher_var_pair : lidar_pub_variable_pair_map_) {
975+
const auto & lidar_name = publisher_var_pair.first;
976+
const auto & publisher_var = publisher_var_pair.second;
977977
if (
978978
pointcloud_publish_timers_map_.find(lidar_name) == pointcloud_publish_timers_map_.end()) {
979-
// Create the periodic timer
980-
auto periodic_timer =
981-
node_->create_wall_timer(period_pointcloud_ns, [this, publisher_var_pair]() {
982-
this->pointcloud_messages_async_publisher(publisher_var_pair);
983-
});
984-
// Store the periodic timer to keep it alive
979+
auto periodic_timer = node_->create_wall_timer(
980+
period_pointcloud_ns,
981+
[this, publisher_var]() { this->pointcloud_messages_async_publisher(publisher_var); });
985982
pointcloud_publish_timers_map_[lidar_name] = periodic_timer;
986983
return;
987984
}
988985
}
989-
// close the timer
990986
one_shot_timer_shared_ptr_->cancel();
991987
});
992-
993-
one_shot_timer_shared_ptr_ = one_shot_timer; // Store a weak pointer to the timer
988+
one_shot_timer_shared_ptr_ = one_shot_timer;
994989
}
995990
return true;
996991
}
@@ -1010,7 +1005,8 @@ bool TopicPublisher::check_publishers_initialized_correctly()
10101005
node_->get_logger(),
10111006
"Empty area message couldn't found in the topic named: " << topic_name);
10121007
return false;
1013-
} else if (!object_spawned_message) {
1008+
}
1009+
if (!object_spawned_message) {
10141010
RCLCPP_ERROR_STREAM(
10151011
node_->get_logger(),
10161012
"Object spawned message couldn't found in the topic named: " << topic_name);

0 commit comments

Comments
 (0)