Skip to content

Commit

Permalink
style(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
pre-commit-ci[bot] committed Dec 23, 2024
1 parent d13ae2c commit 9c3a9a4
Show file tree
Hide file tree
Showing 6 changed files with 25 additions and 43 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -12,14 +12,15 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include <gtest/gtest.h>
#include "autoware/universe_utils/ros/diagnostics_module.hpp"

#include <rclcpp/rclcpp.hpp>

#include <diagnostic_msgs/msg/diagnostic_array.hpp>
#include <diagnostic_msgs/msg/diagnostic_status.hpp>
#include <diagnostic_msgs/msg/key_value.hpp>

#include "autoware/universe_utils/ros/diagnostics_module.hpp"
#include <gtest/gtest.h>

using autoware::universe_utils::DiagnosticsModule;

Expand Down Expand Up @@ -47,21 +48,16 @@ TEST_F(TestDiagnosticsModule, ClearTest)
// Add some key-value pairs and update level/message
module.add_key_value("param1", 42);
module.update_level_and_message(
diagnostic_msgs::msg::DiagnosticStatus::WARN,
"Something is not OK"
);

diagnostic_msgs::msg::DiagnosticStatus::WARN, "Something is not OK");

// Call clear()
module.clear();

// After calling clear(), publish and check the contents of the message
diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr received_msg;
auto sub = node_->create_subscription<diagnostic_msgs::msg::DiagnosticArray>(
"/diagnostics", 10,
[&](diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg) {
received_msg = msg;
}
);
[&](diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg) { received_msg = msg; });

// Publish the message
module.publish(node_->now());
Expand Down Expand Up @@ -104,10 +100,7 @@ TEST_F(TestDiagnosticsModule, AddKeyValueTest)
diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr received_msg;
auto sub = node_->create_subscription<diagnostic_msgs::msg::DiagnosticArray>(
"/diagnostics", 10,
[&](diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg) {
received_msg = msg;
}
);
[&](diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg) { received_msg = msg; });
module.publish(node_->now());
rclcpp::spin_some(node_);

Expand Down Expand Up @@ -138,7 +131,6 @@ TEST_F(TestDiagnosticsModule, AddKeyValueTest)
EXPECT_EQ(status.message, "OK");
}


/*
* Test update_level_and_message():
* Verify that the level is updated to the highest severity and
Expand All @@ -149,33 +141,19 @@ TEST_F(TestDiagnosticsModule, UpdateLevelAndMessageTest)
DiagnosticsModule module(node_.get(), "test_diagnostic");

// Initial status is level=OK(0), message=""
module.update_level_and_message(
diagnostic_msgs::msg::DiagnosticStatus::OK,
"Initial OK"
);
module.update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::OK, "Initial OK");
// Update to WARN (1)
module.update_level_and_message(
diagnostic_msgs::msg::DiagnosticStatus::WARN,
"Low battery"
);
module.update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::WARN, "Low battery");
// Update to ERROR (2)
module.update_level_and_message(
diagnostic_msgs::msg::DiagnosticStatus::ERROR,
"Sensor failure"
);
module.update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Sensor failure");
// Another WARN (1) after ERROR should not downgrade the overall level
module.update_level_and_message(
diagnostic_msgs::msg::DiagnosticStatus::WARN,
"Should not override error"
);
diagnostic_msgs::msg::DiagnosticStatus::WARN, "Should not override error");

diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr received_msg;
auto sub = node_->create_subscription<diagnostic_msgs::msg::DiagnosticArray>(
"/diagnostics", 10,
[&](diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg) {
received_msg = msg;
}
);
[&](diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg) { received_msg = msg; });

module.publish(node_->now());
rclcpp::spin_some(node_);
Expand All @@ -195,4 +173,4 @@ TEST_F(TestDiagnosticsModule, UpdateLevelAndMessageTest)
EXPECT_TRUE(final_message.find("Low battery") != std::string::npos);
EXPECT_TRUE(final_message.find("Sensor failure") != std::string::npos);
EXPECT_TRUE(final_message.find("Should not override error") != std::string::npos);
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,6 @@
#include <autoware/image_projection_based_fusion/utils/utils.hpp>
#include <autoware/lidar_centerpoint/centerpoint_trt.hpp>
#include <autoware/lidar_centerpoint/detection_class_remapper.hpp>

#include <autoware/universe_utils/ros/diagnostics_module.hpp>

#include <map>
Expand All @@ -46,7 +45,7 @@ inline bool isInsideBbox(
class PointPaintingFusionNode
: public FusionNode<sensor_msgs::msg::PointCloud2, DetectedObjects, DetectedObjectsWithFeature>
{
using DiagnosticsModule = autoware::universe_utils::DiagnosticsModule;
using DiagnosticsModule = autoware::universe_utils::DiagnosticsModule;

public:
explicit PointPaintingFusionNode(const rclcpp::NodeOptions & options);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -412,11 +412,13 @@ void PointPaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painte

std::vector<autoware::lidar_centerpoint::Box3D> det_boxes3d;
bool is_num_pillars_within_range = true;
bool is_success = detector_ptr_->detect(painted_pointcloud_msg, tf_buffer_, det_boxes3d, is_num_pillars_within_range);
bool is_success = detector_ptr_->detect(
painted_pointcloud_msg, tf_buffer_, det_boxes3d, is_num_pillars_within_range);
if (!is_success) {
return;
}
diagnostics_module_ptr_->add_key_value("is_num_pillars_within_range", is_num_pillars_within_range);
diagnostics_module_ptr_->add_key_value(
"is_num_pillars_within_range", is_num_pillars_within_range);
if (!is_num_pillars_within_range) {
std::stringstream message;
message << "PointPaintingTRT::detect: The actual number of pillars exceeds its maximum value, "
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
#include "autoware/lidar_centerpoint/preprocess/voxel_generator.hpp"
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"

#include "sensor_msgs/msg/point_cloud2.hpp"

#include <memory>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,8 @@
#include "autoware/lidar_centerpoint/detection_class_remapper.hpp"
#include "autoware/lidar_centerpoint/postprocess/non_maximum_suppression.hpp"

#include <autoware/universe_utils/ros/diagnostics_module.hpp>
#include <autoware/universe_utils/ros/debug_publisher.hpp>
#include <autoware/universe_utils/ros/diagnostics_module.hpp>
#include <autoware/universe_utils/ros/published_time_publisher.hpp>
#include <autoware/universe_utils/system/stop_watch.hpp>
#include <rclcpp/rclcpp.hpp>
Expand All @@ -40,7 +40,7 @@ namespace autoware::lidar_centerpoint

class LidarCenterPointNode : public rclcpp::Node
{
using DiagnosticsModule = autoware::universe_utils::DiagnosticsModule;
using DiagnosticsModule = autoware::universe_utils::DiagnosticsModule;

public:
explicit LidarCenterPointNode(const rclcpp::NodeOptions & node_options);
Expand Down
6 changes: 4 additions & 2 deletions perception/autoware_lidar_centerpoint/src/node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -150,11 +150,13 @@ void LidarCenterPointNode::pointCloudCallback(

std::vector<Box3D> det_boxes3d;
bool is_num_pillars_within_range = true;
bool is_success = detector_ptr_->detect(*input_pointcloud_msg, tf_buffer_, det_boxes3d, is_num_pillars_within_range);
bool is_success = detector_ptr_->detect(
*input_pointcloud_msg, tf_buffer_, det_boxes3d, is_num_pillars_within_range);
if (!is_success) {
return;
}
diagnostics_module_ptr_->add_key_value("is_num_pillars_within_range", is_num_pillars_within_range);
diagnostics_module_ptr_->add_key_value(
"is_num_pillars_within_range", is_num_pillars_within_range);
if (!is_num_pillars_within_range) {
std::stringstream message;
message << "CenterPointTRT::detect: The actual number of pillars exceeds its maximum value, "
Expand Down

0 comments on commit 9c3a9a4

Please sign in to comment.