Skip to content

Commit

Permalink
fix: clang-tidy
Browse files Browse the repository at this point in the history 
Signed-off-by: Berkay Karaman <[email protected]>
  • Loading branch information
@brkay54
brkay54 committed Mar 19, 2024
1 parent 18edb52 commit 605839a
Show file tree
Hide file tree
Showing 7 changed files with 60 additions and 94 deletions.
3 changes: 1 addition & 2 deletions tools/reaction_analyzer/include/reaction_analyzer_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,8 +66,7 @@ struct NodeParams
class ReactionAnalyzerNode : public rclcpp::Node
{
public:
explicit ReactionAnalyzerNode(rclcpp::NodeOptions options);
~ReactionAnalyzerNode() = default;
explicit ReactionAnalyzerNode(rclcpp::NodeOptions node_options);

Odometry::ConstSharedPtr odometry_ptr_;

Expand Down
10 changes: 2 additions & 8 deletions tools/reaction_analyzer/include/subscriber.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -141,13 +141,7 @@ class SubscriberBase
rclcpp::Node * node, Odometry::ConstSharedPtr & odometry, std::atomic<bool> & spawn_object_cmd,
const EntityParams & entity_params);

// Instances of SubscriberBase cannot be copied
SubscriberBase(const SubscriberBase &) = delete;
SubscriberBase & operator=(const SubscriberBase &) = delete;

~SubscriberBase() = default;

std::optional<std::unordered_map<std::string, MessageBufferVariant>> getMessageBuffersMap();
std::optional<std::unordered_map<std::string, MessageBufferVariant>> get_message_buffers_map();
void reset();

private:
Expand Down Expand Up @@ -179,7 +173,7 @@ class SubscriberBase
std::optional<size_t> find_first_brake_idx(
const std::vector<AckermannControlCommand> & cmd_array);
void set_control_command_to_buffer(
std::vector<AckermannControlCommand> & buffer, const AckermannControlCommand & cmd);
std::vector<AckermannControlCommand> & buffer, const AckermannControlCommand & cmd) const;

// Callbacks for modules are subscribed
void on_control_command(
Expand Down
13 changes: 6 additions & 7 deletions tools/reaction_analyzer/include/topic_publisher.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -110,23 +110,23 @@ 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
struct HasHeader : std::false_type
{
};

template <typename T>
struct has_header<T, std::void_t<decltype(T::header)>> : std::true_type
struct HasHeader<T, std::void_t<decltype(T::header)>> : std::true_type
{
};

// Template struct to check if a type has a stamp member.
template <typename T, typename = std::void_t<>>
struct has_stamp : std::false_type
struct HasStamp : std::false_type
{
};

template <typename T>
struct has_stamp<T, std::void_t<decltype(T::stamp)>> : std::true_type
struct HasStamp<T, std::void_t<decltype(T::stamp)>> : std::true_type
{
};

Expand All @@ -142,9 +142,9 @@ struct PublisherVarAccessor
} else {
*msg_to_be_published = *publisherVar.empty_area_message;
}
if constexpr (has_header<MessageType>::value) {
if constexpr (HasHeader<MessageType>::value) {
msg_to_be_published->header.stamp = current_time;
} else if constexpr (has_stamp<MessageType>::value) {
} else if constexpr (HasStamp<MessageType>::value) {
msg_to_be_published->stamp = current_time;
}
publisherVar.publisher->publish(std::move(msg_to_be_published));
Expand Down Expand Up @@ -200,7 +200,6 @@ class TopicPublisher
std::optional<rclcpp::Time> & spawn_cmd_time, const RunningMode & node_running_mode,
const EntityParams & entity_params);

~TopicPublisher() = default;
void reset();

private:
Expand Down
33 changes: 16 additions & 17 deletions tools/reaction_analyzer/src/reaction_analyzer_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,8 +51,8 @@ void ReactionAnalyzerNode::on_ground_truth_pose(PoseStamped::ConstSharedPtr msg_
ground_truth_pose_ptr_ = std::move(msg_ptr);
}

ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions options)
: Node("reaction_analyzer_node", options.automatically_declare_parameters_from_overrides(true))
ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions node_options)
: Node("reaction_analyzer_node", node_options.automatically_declare_parameters_from_overrides(true))
{
using std::placeholders::_1;

Expand Down Expand Up @@ -173,7 +173,7 @@ void ReactionAnalyzerNode::on_timer()
if (!spawn_cmd_time) return;

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

if (message_buffers) {
// if reacted, calculate the results
Expand Down Expand Up @@ -298,20 +298,19 @@ void ReactionAnalyzerNode::init_ego(
RCLCPP_WARN(
get_logger(), "Ground truth pose is not received. Waiting for Ground truth pose...");
return;
} else {
constexpr double deviation_threshold = 0.1;
const auto deviation = tier4_autoware_utils::calcPoseDeviation(
ground_truth_pose_ptr->pose, odometry_ptr->pose.pose);
if (
deviation.longitudinal > deviation_threshold || deviation.lateral > deviation_threshold ||
deviation.yaw > deviation_threshold) {
RCLCPP_ERROR(
get_logger(),
"Deviation between ground truth position and ego position is high. Node is shutting "
"down. Longitudinal deviation: %f, Lateral deviation: %f, Yaw deviation: %f",
deviation.longitudinal, deviation.lateral, deviation.yaw);
rclcpp::shutdown();
}
}
constexpr double deviation_threshold = 0.1;
const auto deviation = tier4_autoware_utils::calcPoseDeviation(
ground_truth_pose_ptr->pose, odometry_ptr->pose.pose);
if (
deviation.longitudinal > deviation_threshold || deviation.lateral > deviation_threshold ||
deviation.yaw > deviation_threshold) {
RCLCPP_ERROR(
get_logger(),
"Deviation between ground truth position and ego position is high. Node is shutting "
"down. Longitudinal deviation: %f, Lateral deviation: %f, Yaw deviation: %f",
deviation.longitudinal, deviation.lateral, deviation.yaw);
rclcpp::shutdown();
}
}

Expand Down
18 changes: 8 additions & 10 deletions tools/reaction_analyzer/src/subscriber.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@ SubscriberBase::SubscriberBase(

void SubscriberBase::init_reaction_chains_and_params()
{
auto stringToMessageType = [](const std::string & input) {
auto string_to_message_type = [](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") {
Expand All @@ -53,21 +53,19 @@ void SubscriberBase::init_reaction_chains_and_params()
return SubscriberMessageType::DETECTED_OBJECTS;
} else if (input == "autoware_auto_perception_msgs/msg/TrackedObjects") {
return SubscriberMessageType::TRACKED_OBJECTS;
} else {
return SubscriberMessageType::UNKNOWN;
}
return SubscriberMessageType::UNKNOWN;
};

auto stringToReactionType = [](const std::string & input) {
auto string_to_reaction_type = [](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;
}
return ReactionType::UNKNOWN;
};

// Init Chains: get the topic addresses and message types of the modules in chain
Expand All @@ -82,7 +80,7 @@ void SubscriberBase::init_reaction_chains_and_params()
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());
string_to_message_type(node_->get_parameter(module_name + ".message_type").as_string());
if (tmp.message_type != SubscriberMessageType::UNKNOWN) {
chain_modules_.emplace_back(tmp);
} else {
Expand All @@ -99,7 +97,7 @@ void SubscriberBase::init_reaction_chains_and_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());
const auto type = string_to_reaction_type(splitted_name.back());
switch (type) {
case ReactionType::FIRST_BRAKE: {
reaction_params_.first_brake_params.debug_control_commands =
Expand Down Expand Up @@ -460,7 +458,7 @@ bool SubscriberBase::init_subscribers()
}

std::optional<std::unordered_map<std::string, MessageBufferVariant>>
SubscriberBase::getMessageBuffersMap()
SubscriberBase::get_message_buffers_map()
{
std::lock_guard<std::mutex> lock(mutex_);
if (message_buffers_.empty()) {
Expand Down Expand Up @@ -997,7 +995,7 @@ std::optional<size_t> SubscriberBase::find_first_brake_idx(
}

void SubscriberBase::set_control_command_to_buffer(
std::vector<AckermannControlCommand> & buffer, const AckermannControlCommand & cmd)
std::vector<AckermannControlCommand> & buffer, const AckermannControlCommand & cmd) const
{
const auto last_cmd_time = cmd.stamp;
if (!buffer.empty()) {
Expand Down
36 changes: 16 additions & 20 deletions tools/reaction_analyzer/src/topic_publisher.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -304,9 +304,8 @@ void TopicPublisher::init_rosbag_publishers()
if (it != topics.end()) {
return string_to_publisher_message_type(
it->topic_metadata.type); // Return the message type if found
} else {
return PublisherMessageType::UNKNOWN; //
}
return PublisherMessageType::UNKNOWN;
};

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

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

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

if (it != topics.end()) {
return string_to_publisher_message_type(
it->topic_metadata.type); // Return the message type if found
} else {
return PublisherMessageType::UNKNOWN;
}
return PublisherMessageType::UNKNOWN;
};

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

const auto message_type = getMessageTypeForTopic(current_topic);
const auto message_type = get_message_type_for_topic(current_topic);

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

PublisherVariablesVariant & publisherVar = topic_publisher_map_[topic_name];
PublisherVariablesVariant & publisher_variant = topic_publisher_map_[topic_name];
PublisherVarAccessor accessor;

std::visit([&](auto & var) { accessor.set_period(var, period_ms); }, publisherVar);
std::visit([&](auto & var) { accessor.set_period(var, period_ms); }, publisher_variant);
}
}
}
Expand Down Expand Up @@ -973,24 +971,21 @@ bool TopicPublisher::set_publishers_and_timers_to_variable_map()
// Create a timer to create phase difference bw timers which will be created for each lidar
// topics
auto one_shot_timer = node_->create_wall_timer(phase_dif, [this, period_pointcloud_ns]() {
for (const auto & [lidar_name, publisher_var_pair] : lidar_pub_variable_pair_map_) {
for (const auto & publisher_var_pair : lidar_pub_variable_pair_map_) {
const auto & lidar_name = publisher_var_pair.first;
const auto & publisher_var = publisher_var_pair.second;
if (
pointcloud_publish_timers_map_.find(lidar_name) == pointcloud_publish_timers_map_.end()) {
// Create the periodic timer
auto periodic_timer =
node_->create_wall_timer(period_pointcloud_ns, [this, publisher_var_pair]() {
this->pointcloud_messages_async_publisher(publisher_var_pair);
});
// Store the periodic timer to keep it alive
auto periodic_timer = node_->create_wall_timer(
period_pointcloud_ns,
[this, publisher_var]() { this->pointcloud_messages_async_publisher(publisher_var); });
pointcloud_publish_timers_map_[lidar_name] = periodic_timer;
return;
}
}
// close the timer
one_shot_timer_shared_ptr_->cancel();
});

one_shot_timer_shared_ptr_ = one_shot_timer; // Store a weak pointer to the timer
one_shot_timer_shared_ptr_ = one_shot_timer;
}
return true;
}
Expand All @@ -1010,7 +1005,8 @@ bool TopicPublisher::check_publishers_initialized_correctly()
node_->get_logger(),
"Empty area message couldn't found in the topic named: " << topic_name);
return false;
} else if (!object_spawned_message) {
}
if (!object_spawned_message) {
RCLCPP_ERROR_STREAM(
node_->get_logger(),
"Object spawned message couldn't found in the topic named: " << topic_name);
Expand Down
Loading

0 comments on commit 605839a

Please sign in to comment.