From e0eb44686fccb6f57cb232c20463b6fc92acde9a Mon Sep 17 00:00:00 2001 From: kenji-miyake Date: Mon, 3 Apr 2023 00:14:16 +0000 Subject: [PATCH 1/2] chore: sync files Signed-off-by: GitHub --- .clang-format | 1 + .github/PULL_REQUEST_TEMPLATE/small-change.md | 7 +++++++ .pre-commit-config.yaml | 2 +- 3 files changed, 9 insertions(+), 1 deletion(-) diff --git a/.clang-format b/.clang-format index c3b72db71b4ca..b41fae9129e43 100644 --- a/.clang-format +++ b/.clang-format @@ -4,6 +4,7 @@ BasedOnStyle: Google AccessModifierOffset: -2 AlignAfterOpenBracket: AlwaysBreak +AllowShortFunctionsOnASingleLine: InlineOnly BraceWrapping: AfterClass: true AfterFunction: true diff --git a/.github/PULL_REQUEST_TEMPLATE/small-change.md b/.github/PULL_REQUEST_TEMPLATE/small-change.md index 7120f5212efd2..527c8ed81fded 100644 --- a/.github/PULL_REQUEST_TEMPLATE/small-change.md +++ b/.github/PULL_REQUEST_TEMPLATE/small-change.md @@ -2,6 +2,13 @@ +## Tests performed + + + + +Not applicable. + ## Pre-review checklist for the PR author The PR author **must** check the checkboxes below when creating the PR. diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index da14e953ba1c1..0aed9c0ba2f96 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -60,7 +60,7 @@ repos: - id: isort - repo: https://github.com/psf/black - rev: 23.1.0 + rev: 23.3.0 hooks: - id: black args: [--line-length=100] From 9b77d8030b3116018e862a57582bd7786987ad39 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 3 Apr 2023 00:15:42 +0000 Subject: [PATCH 2/2] style(pre-commit): autofix --- .../src/spatial_hash.cpp | 10 ++- .../test/src/test_area.cpp | 5 +- .../detected_objects_display.cpp | 4 +- .../tracked_objects_display.cpp | 4 +- common/fake_test_node/src/fake_test_node.cpp | 10 ++- .../test/test_fake_test_node.cpp | 10 ++- .../grid_map_utils/src/polygon_iterator.cpp | 5 +- common/kalman_filter/src/kalman_filter.cpp | 58 ++++++++++++---- .../src/time_delay_kalman_filter.cpp | 14 +++- .../src/vehicle/vehicle_state_checker.cpp | 5 +- common/osqp_interface/src/osqp_interface.cpp | 5 +- .../src/rtc_manager_panel.cpp | 40 ++++++++--- common/signal_processing/src/butterworth.cpp | 30 ++++++-- .../src/lowpass_filter_1d.cpp | 19 ++++-- .../tensorrt_common/src/tensorrt_common.cpp | 10 ++- .../src/automatic_goal_panel.cpp | 5 +- .../src/automatic_goal_sender.cpp | 4 +- .../geometry/boost_geometry.hpp | 10 ++- .../geometry/geometry.hpp | 5 +- .../math/unit_conversion.hpp | 20 ++++-- .../src/math/trigonometry.cpp | 5 +- .../float32_multi_array_stamped_pie_chart.cpp | 5 +- .../src/jsk_overlay_utils.cpp | 30 ++++++-- .../src/pose_history/pose_history_display.cpp | 15 +++- .../src/pose_history_footprint/display.cpp | 15 +++- .../src/tools/interactive_object.cpp | 20 ++++-- .../src/pose_with_uuid_stamped/display.cpp | 10 ++- .../src/tools/jsk_overlay_utils.cpp | 30 ++++++-- .../src/tools/max_velocity.cpp | 5 +- .../src/screen_capture_panel.cpp | 18 +++-- .../src/autoware_state_panel.cpp | 5 +- .../src/tools/jsk_overlay_utils.cpp | 30 ++++++-- .../autonomous_emergency_braking/src/node.cpp | 10 ++- .../lane_departure_checker_node.cpp | 5 +- .../src/lowpass_filter.cpp | 4 +- .../src/mpc_lateral_controller.cpp | 4 +- .../src/qp_solver/qp_solver_osqp.cpp | 4 +- .../src/qp_solver/qp_solver_unconstr_fast.cpp | 4 +- .../vehicle_model/vehicle_model_interface.cpp | 30 ++++++-- .../src/compatibility.cpp | 5 +- .../src/pure_pursuit_core/interpolate.cpp | 13 +++- .../src/pure_pursuit_core/planning_utils.cpp | 5 +- .../src/longitudinal_controller_base.cpp | 5 +- .../src/simple_trajectory_follower.cpp | 5 +- .../vehicle_cmd_gate/src/pause_interface.cpp | 5 +- .../src/vehicle_cmd_filter.cpp | 4 +- .../vehicle_cmd_gate/src/vehicle_cmd_gate.cpp | 5 +- .../src/metrics_calculator.cpp | 10 ++- .../include/ekf_localizer/numeric.hpp | 10 ++- .../gyro_odometer/src/gyro_odometer_core.cpp | 4 +- .../localization_error_monitor/src/node.cpp | 5 +- .../src/pose_array_interpolator.cpp | 5 +- .../src/map_height_fitter.cpp | 9 ++- perception/bytetrack/lib/src/byte_tracker.cpp | 4 +- perception/bytetrack/lib/src/strack.cpp | 24 +++++-- .../src/bytetrack_visualizer_node.cpp | 5 +- .../lib/euclidean_cluster.cpp | 4 +- .../voxel_grid_based_euclidean_cluster.cpp | 4 +- .../src/feature_generator.cpp | 5 +- .../src/feature_map.cpp | 8 ++- .../src/feature_generator.cpp | 5 +- .../src/feature_map.cpp | 10 ++- .../src/lidar_apollo_segmentation_tvm.cpp | 5 +- .../lib/network/tensorrt_wrapper.cpp | 4 +- .../lib/postprocess/postprocess_kernel.cu | 5 +- .../lib/centerpoint_tvm.cpp | 4 +- .../postprocess/generate_detected_boxes.cpp | 5 +- .../lib/src/plugins/mish_plugin.cpp | 63 +++++++++++++---- .../lib/src/plugins/nms_plugin.cpp | 68 +++++++++++++++---- .../lib/src/plugins/yolo_layer.cu | 5 +- .../lib/src/plugins/yolo_layer_plugin.cpp | 48 ++++++++++--- perception/tensorrt_yolo/lib/src/trt_yolo.cpp | 5 +- .../utils/trt_common.cpp | 5 +- .../lib/src/trt_ssd.cpp | 5 +- .../marker_util/debug_utilities.hpp | 5 +- .../src/behavior_tree_manager.cpp | 5 +- .../external_request_lane_change_module.cpp | 5 +- .../lane_change/lane_change_module.cpp | 20 ++++-- .../lane_following/lane_following_module.cpp | 10 ++- .../scene_module/pull_out/pull_out_module.cpp | 10 ++- .../pull_over/pull_over_module.cpp | 10 ++- .../src/steering_factor_interface.cpp | 5 +- .../src/util/avoidance/util.cpp | 5 +- .../drivable_area_expansion.cpp | 10 ++- .../geometric_parallel_parking.cpp | 5 +- .../src/util/path_shifter/path_shifter.cpp | 10 ++- .../src/util/side_shift/util.cpp | 5 +- .../include/utilization/util.hpp | 5 +- .../src/scene_module/run_out/debug.cpp | 10 ++- .../test/src/test_state_machine.cpp | 5 +- .../freespace_planner_node.cpp | 5 +- .../test_freespace_planning_algorithms.cpp | 10 ++- .../src/lanelet2_plugins/default_planner.cpp | 5 +- .../velocity_planning_utils.cpp | 10 ++- .../src/smoother/jerk_filtered_smoother.cpp | 5 +- .../src/smoother/l2_pseudo_jerk_smoother.cpp | 5 +- .../smoother/linf_pseudo_jerk_smoother.cpp | 5 +- .../src/smoother/smoother_base.cpp | 30 ++++++-- .../src/trajectory_utils.cpp | 5 +- .../src/eb_path_smoother.cpp | 5 +- .../src/mpt_optimizer.cpp | 10 ++- .../vehicle_model/vehicle_model_interface.cpp | 15 +++- .../include/planning_debug_tools/util.hpp | 30 ++++++-- .../src/planning_interface_test_manager.cpp | 5 +- planning/route_handler/src/route_handler.cpp | 45 +++++++++--- planning/rtc_interface/src/rtc_interface.cpp | 15 +++- .../src/static_centerline_optimizer_node.cpp | 10 ++- sensing/geo_pos_conv/src/geo_pos_conv.cpp | 15 +++- .../sim_model_delay_steer_acc.cpp | 35 ++++++++-- .../sim_model_delay_steer_acc_geared.cpp | 35 ++++++++-- .../sim_model_delay_steer_vel.cpp | 35 ++++++++-- .../sim_model_ideal_steer_acc.cpp | 40 ++++++++--- .../sim_model_ideal_steer_acc_geared.cpp | 35 ++++++++-- .../sim_model_ideal_steer_vel.cpp | 35 ++++++++-- .../vehicle_model/sim_model_interface.cpp | 30 ++++++-- system/bluetooth_monitor/service/l2ping.cpp | 10 ++- .../service/l2ping_service.cpp | 9 ++- .../src/bluetooth_monitor.cpp | 5 +- system/default_ad_api/src/planning.cpp | 5 +- .../src/dummy_diag_publisher_core.cpp | 5 +- .../mrm_comfortable_stop_operator_core.cpp | 5 +- .../traffic_reader/traffic_reader_service.cpp | 5 +- .../src/cpu_monitor/cpu_monitor_base.cpp | 5 +- .../src/cpu_monitor/tegra_cpu_monitor.cpp | 4 +- .../src/gpu_monitor/gpu_monitor_base.cpp | 5 +- .../src/mem_monitor/mem_monitor.cpp | 5 +- .../src/net_monitor/net_monitor.cpp | 10 ++- .../src/net_monitor/nl80211.cpp | 4 +- .../src/ntp_monitor/ntp_monitor.cpp | 5 +- .../src/voltage_monitor/voltage_monitor.cpp | 5 +- .../test/src/cpu_monitor/mpstat1.cpp | 5 +- .../test/src/cpu_monitor/mpstat2.cpp | 5 +- .../cpu_monitor/test_unknown_cpu_monitor.cpp | 5 +- .../gpu_monitor/test_unknown_gpu_monitor.cpp | 5 +- .../test/src/hdd_monitor/df1.cpp | 5 +- .../test/src/mem_monitor/free1.cpp | 5 +- .../test/src/ntp_monitor/ntpdate1.cpp | 5 +- .../test/src/process_monitor/echo1.cpp | 5 +- .../test/src/process_monitor/sed1.cpp | 5 +- .../test/src/process_monitor/sort1.cpp | 5 +- .../test/src/process_monitor/top1.cpp | 5 +- .../test/src/process_monitor/top2.cpp | 5 +- .../topic_state_monitor.cpp | 9 ++- vehicle/external_cmd_converter/src/node.cpp | 5 +- .../src/csv_loader.cpp | 5 +- .../src/steer_offset_estimator_node.cpp | 10 ++- 146 files changed, 1336 insertions(+), 343 deletions(-) diff --git a/common/autoware_auto_geometry/src/spatial_hash.cpp b/common/autoware_auto_geometry/src/spatial_hash.cpp index a5c75a8d604ab..024cca48b8ee7 100644 --- a/common/autoware_auto_geometry/src/spatial_hash.cpp +++ b/common/autoware_auto_geometry/src/spatial_hash.cpp @@ -59,7 +59,10 @@ details::Index3 Config2d::index3_(const float32_t x, const float32_t y, const fl return {x_index(x), y_index(y), Index{}}; // zero initialization } //////////////////////////////////////////////////////////////////////////////// -Index Config2d::index_(const details::Index3 & idx) const { return bin_impl(idx.x, idx.y); } +Index Config2d::index_(const details::Index3 & idx) const +{ + return bin_impl(idx.x, idx.y); +} //////////////////////////////////////////////////////////////////////////////// Config3d::Config3d( const float32_t min_x, const float32_t max_x, const float32_t min_y, const float32_t max_y, @@ -88,7 +91,10 @@ details::Index3 Config3d::index3_(const float32_t x, const float32_t y, const fl return {x_index(x), y_index(y), z_index(z)}; // zero initialization } //////////////////////////////////////////////////////////////////////////////// -Index Config3d::index_(const details::Index3 & idx) const { return bin_impl(idx.x, idx.y, idx.z); } +Index Config3d::index_(const details::Index3 & idx) const +{ + return bin_impl(idx.x, idx.y, idx.z); +} //////////////////////////////////////////////////////////////////////////////// template class SpatialHash; template class SpatialHash; diff --git a/common/autoware_auto_geometry/test/src/test_area.cpp b/common/autoware_auto_geometry/test/src/test_area.cpp index 6710546fa73e9..d722314dada83 100644 --- a/common/autoware_auto_geometry/test/src/test_area.cpp +++ b/common/autoware_auto_geometry/test/src/test_area.cpp @@ -53,7 +53,10 @@ using TestTypes = TestTypes_; TYPED_TEST_SUITE(AreaTest, TestTypes, ); // The empty set has zero area -TYPED_TEST(AreaTest, DegenerateZero) { EXPECT_FLOAT_EQ(0.0, this->area()); } +TYPED_TEST(AreaTest, DegenerateZero) +{ + EXPECT_FLOAT_EQ(0.0, this->area()); +} // An individual point has zero area TYPED_TEST(AreaTest, DegenerateOne) diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp index 26ba30840b178..067173288e685 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp @@ -24,7 +24,9 @@ namespace rviz_plugins { namespace object_detection { -DetectedObjectsDisplay::DetectedObjectsDisplay() : ObjectPolygonDisplayBase("detected_objects") {} +DetectedObjectsDisplay::DetectedObjectsDisplay() : ObjectPolygonDisplayBase("detected_objects") +{ +} void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) { diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp index 57e3ce905d85d..00a1199c697ce 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp @@ -24,7 +24,9 @@ namespace rviz_plugins { namespace object_detection { -TrackedObjectsDisplay::TrackedObjectsDisplay() : ObjectPolygonDisplayBase("tracks") {} +TrackedObjectsDisplay::TrackedObjectsDisplay() : ObjectPolygonDisplayBase("tracks") +{ +} void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) { diff --git a/common/fake_test_node/src/fake_test_node.cpp b/common/fake_test_node/src/fake_test_node.cpp index a7cd4cef8080b..1704d371c3a5e 100644 --- a/common/fake_test_node/src/fake_test_node.cpp +++ b/common/fake_test_node/src/fake_test_node.cpp @@ -53,7 +53,10 @@ void detail::FakeNodeCore::set_up(const std::string & test_name) std::make_shared(m_tf_buffer, m_fake_node, kSpinThread); } -void detail::FakeNodeCore::tear_down() { (void)rclcpp::shutdown(); } +void detail::FakeNodeCore::tear_down() +{ + (void)rclcpp::shutdown(); +} std::string detail::get_test_name(const ::testing::TestInfo * info) { @@ -68,7 +71,10 @@ void FakeTestNode::SetUp() set_up(detail::get_test_name(::testing::UnitTest::GetInstance()->current_test_info())); } -void FakeTestNode::TearDown() { tear_down(); } +void FakeTestNode::TearDown() +{ + tear_down(); +} } // namespace testing } // namespace tools diff --git a/common/fake_test_node/test/test_fake_test_node.cpp b/common/fake_test_node/test/test_fake_test_node.cpp index f028c4c989223..d66d0fed3033c 100644 --- a/common/fake_test_node/test/test_fake_test_node.cpp +++ b/common/fake_test_node/test/test_fake_test_node.cpp @@ -91,7 +91,10 @@ void run_test(int32_t value_in_message, FixtureT * fixture) } // namespace /// @test Test that we can use a non-parametrized test. -TEST_F(FakeNodeFixture, Test) { run_test(15, this); } +TEST_F(FakeNodeFixture, Test) +{ + run_test(15, this); +} INSTANTIATE_TEST_SUITE_P( FakeNodeFixtureTests, FakeNodeFixtureParametrized, @@ -99,4 +102,7 @@ INSTANTIATE_TEST_SUITE_P( ::testing::Values(-5, 0, 42)); /// @test Test that we can use a parametrized test. -TEST_P(FakeNodeFixtureParametrized, Test) { run_test(GetParam(), this); } +TEST_P(FakeNodeFixtureParametrized, Test) +{ + run_test(GetParam(), this); +} diff --git a/common/grid_map_utils/src/polygon_iterator.cpp b/common/grid_map_utils/src/polygon_iterator.cpp index 3ab96bd0612da..d2a738a971263 100644 --- a/common/grid_map_utils/src/polygon_iterator.cpp +++ b/common/grid_map_utils/src/polygon_iterator.cpp @@ -153,7 +153,10 @@ bool PolygonIterator::operator!=(const PolygonIterator & other) const return current_line_ != other.current_line_ || current_col_ != other.current_col_; } -const grid_map::Index & PolygonIterator::operator*() const { return current_index_; } +const grid_map::Index & PolygonIterator::operator*() const +{ + return current_index_; +} void PolygonIterator::goToNextLine() { diff --git a/common/kalman_filter/src/kalman_filter.cpp b/common/kalman_filter/src/kalman_filter.cpp index e6e01a5bb1bdf..937819ffb0275 100644 --- a/common/kalman_filter/src/kalman_filter.cpp +++ b/common/kalman_filter/src/kalman_filter.cpp @@ -14,7 +14,9 @@ #include "kalman_filter/kalman_filter.hpp" -KalmanFilter::KalmanFilter() {} +KalmanFilter::KalmanFilter() +{ +} KalmanFilter::KalmanFilter( const Eigen::MatrixXd & x, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, const Eigen::MatrixXd & C, const Eigen::MatrixXd & Q, const Eigen::MatrixXd & R, @@ -22,7 +24,9 @@ KalmanFilter::KalmanFilter( { init(x, A, B, C, Q, R, P); } -KalmanFilter::~KalmanFilter() {} +KalmanFilter::~KalmanFilter() +{ +} bool KalmanFilter::init( const Eigen::MatrixXd & x, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, const Eigen::MatrixXd & C, const Eigen::MatrixXd & Q, const Eigen::MatrixXd & R, @@ -53,14 +57,38 @@ bool KalmanFilter::init(const Eigen::MatrixXd & x, const Eigen::MatrixXd & P0) return true; } -void KalmanFilter::setA(const Eigen::MatrixXd & A) { A_ = A; } -void KalmanFilter::setB(const Eigen::MatrixXd & B) { B_ = B; } -void KalmanFilter::setC(const Eigen::MatrixXd & C) { C_ = C; } -void KalmanFilter::setQ(const Eigen::MatrixXd & Q) { Q_ = Q; } -void KalmanFilter::setR(const Eigen::MatrixXd & R) { R_ = R; } -void KalmanFilter::getX(Eigen::MatrixXd & x) { x = x_; } -void KalmanFilter::getP(Eigen::MatrixXd & P) { P = P_; } -double KalmanFilter::getXelement(unsigned int i) { return x_(i); } +void KalmanFilter::setA(const Eigen::MatrixXd & A) +{ + A_ = A; +} +void KalmanFilter::setB(const Eigen::MatrixXd & B) +{ + B_ = B; +} +void KalmanFilter::setC(const Eigen::MatrixXd & C) +{ + C_ = C; +} +void KalmanFilter::setQ(const Eigen::MatrixXd & Q) +{ + Q_ = Q; +} +void KalmanFilter::setR(const Eigen::MatrixXd & R) +{ + R_ = R; +} +void KalmanFilter::getX(Eigen::MatrixXd & x) +{ + x = x_; +} +void KalmanFilter::getP(Eigen::MatrixXd & P) +{ + P = P_; +} +double KalmanFilter::getXelement(unsigned int i) +{ + return x_(i); +} bool KalmanFilter::predict( const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q) @@ -89,7 +117,10 @@ bool KalmanFilter::predict( const Eigen::MatrixXd x_next = A * x_ + B * u; return predict(x_next, A, Q); } -bool KalmanFilter::predict(const Eigen::MatrixXd & u) { return predict(u, A_, B_, Q_); } +bool KalmanFilter::predict(const Eigen::MatrixXd & u) +{ + return predict(u, A_, B_, Q_); +} bool KalmanFilter::update( const Eigen::MatrixXd & y, const Eigen::MatrixXd & y_pred, const Eigen::MatrixXd & C, @@ -121,4 +152,7 @@ bool KalmanFilter::update( const Eigen::MatrixXd y_pred = C * x_; return update(y, y_pred, C, R); } -bool KalmanFilter::update(const Eigen::MatrixXd & y) { return update(y, C_, R_); } +bool KalmanFilter::update(const Eigen::MatrixXd & y) +{ + return update(y, C_, R_); +} diff --git a/common/kalman_filter/src/time_delay_kalman_filter.cpp b/common/kalman_filter/src/time_delay_kalman_filter.cpp index 24f06dbb9821b..c4143429e6c05 100644 --- a/common/kalman_filter/src/time_delay_kalman_filter.cpp +++ b/common/kalman_filter/src/time_delay_kalman_filter.cpp @@ -14,7 +14,9 @@ #include "kalman_filter/time_delay_kalman_filter.hpp" -TimeDelayKalmanFilter::TimeDelayKalmanFilter() {} +TimeDelayKalmanFilter::TimeDelayKalmanFilter() +{ +} void TimeDelayKalmanFilter::init( const Eigen::MatrixXd & x, const Eigen::MatrixXd & P0, const int max_delay_step) @@ -32,9 +34,15 @@ void TimeDelayKalmanFilter::init( } } -Eigen::MatrixXd TimeDelayKalmanFilter::getLatestX() const { return x_.block(0, 0, dim_x_, 1); } +Eigen::MatrixXd TimeDelayKalmanFilter::getLatestX() const +{ + return x_.block(0, 0, dim_x_, 1); +} -Eigen::MatrixXd TimeDelayKalmanFilter::getLatestP() const { return P_.block(0, 0, dim_x_, dim_x_); } +Eigen::MatrixXd TimeDelayKalmanFilter::getLatestP() const +{ + return P_.block(0, 0, dim_x_, dim_x_); +} bool TimeDelayKalmanFilter::predictWithDelay( const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q) diff --git a/common/motion_utils/src/vehicle/vehicle_state_checker.cpp b/common/motion_utils/src/vehicle/vehicle_state_checker.cpp index f6cb78d49256a..a743bfaa59c23 100644 --- a/common/motion_utils/src/vehicle/vehicle_state_checker.cpp +++ b/common/motion_utils/src/vehicle/vehicle_state_checker.cpp @@ -128,5 +128,8 @@ bool VehicleArrivalChecker::isVehicleStoppedAtStopPoint(const double stop_durati th_arrived_distance_m; } -void VehicleArrivalChecker::onTrajectory(const Trajectory::SharedPtr msg) { trajectory_ptr_ = msg; } +void VehicleArrivalChecker::onTrajectory(const Trajectory::SharedPtr msg) +{ + trajectory_ptr_ = msg; +} } // namespace motion_utils diff --git a/common/osqp_interface/src/osqp_interface.cpp b/common/osqp_interface/src/osqp_interface.cpp index 972a586b164e9..f0dbc3ab22e4a 100644 --- a/common/osqp_interface/src/osqp_interface.cpp +++ b/common/osqp_interface/src/osqp_interface.cpp @@ -208,7 +208,10 @@ void OSQPInterface::updateAlpha(const double alpha) } } -void OSQPInterface::updateScaling(const int scaling) { m_settings->scaling = scaling; } +void OSQPInterface::updateScaling(const int scaling) +{ + m_settings->scaling = scaling; +} void OSQPInterface::updatePolish(const bool polish) { diff --git a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp index a08ba8874feb7..451c31a2fa665 100644 --- a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp +++ b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp @@ -25,8 +25,14 @@ namespace rviz_plugins { -inline std::string Bool2String(const bool var) { return var ? "True" : "False"; } -inline bool uint2bool(uint8_t var) { return var == static_cast(0) ? false : true; } +inline std::string Bool2String(const bool var) +{ + return var ? "True" : "False"; +} +inline bool uint2bool(uint8_t var) +{ + return var == static_cast(0) ? false : true; +} using std::placeholders::_1; using std::placeholders::_2; @@ -326,12 +332,30 @@ void RTCManagerPanel::onClickCommandRequest(const uint8_t command) client_rtc_commands_->async_send_request(executable_cooperate_commands_request); } -void RTCManagerPanel::onClickExecuteVelChange() { onClickChangeRequest(false, Command::ACTIVATE); } -void RTCManagerPanel::onClickWaitVelChange() { onClickChangeRequest(false, Command::DEACTIVATE); } -void RTCManagerPanel::onClickExecutePathChange() { onClickChangeRequest(true, Command::ACTIVATE); } -void RTCManagerPanel::onClickWaitPathChange() { onClickChangeRequest(true, Command::DEACTIVATE); } -void RTCManagerPanel::onClickExecution() { onClickCommandRequest(Command::ACTIVATE); } -void RTCManagerPanel::onClickWait() { onClickCommandRequest(Command::DEACTIVATE); } +void RTCManagerPanel::onClickExecuteVelChange() +{ + onClickChangeRequest(false, Command::ACTIVATE); +} +void RTCManagerPanel::onClickWaitVelChange() +{ + onClickChangeRequest(false, Command::DEACTIVATE); +} +void RTCManagerPanel::onClickExecutePathChange() +{ + onClickChangeRequest(true, Command::ACTIVATE); +} +void RTCManagerPanel::onClickWaitPathChange() +{ + onClickChangeRequest(true, Command::DEACTIVATE); +} +void RTCManagerPanel::onClickExecution() +{ + onClickCommandRequest(Command::ACTIVATE); +} +void RTCManagerPanel::onClickWait() +{ + onClickCommandRequest(Command::DEACTIVATE); +} void RTCManagerPanel::onRTCStatus(const CooperateStatusArray::ConstSharedPtr msg) { diff --git a/common/signal_processing/src/butterworth.cpp b/common/signal_processing/src/butterworth.cpp index 88a8b6c194601..a0e4d61f1412b 100644 --- a/common/signal_processing/src/butterworth.cpp +++ b/common/signal_processing/src/butterworth.cpp @@ -52,9 +52,15 @@ void ButterworthFilter::Buttord( setCutOffFrequency(right_lim); } -void ButterworthFilter::setOrder(const int & N) { filter_specs_.N = N; } +void ButterworthFilter::setOrder(const int & N) +{ + filter_specs_.N = N; +} -void ButterworthFilter::setCutOffFrequency(const double & Wc) { filter_specs_.Wc_rad_sec = Wc; } +void ButterworthFilter::setCutOffFrequency(const double & Wc) +{ + filter_specs_.Wc_rad_sec = Wc; +} /** * @brief Sets the cut-off and sampling frequencies. @@ -76,7 +82,10 @@ void ButterworthFilter::setCutOffFrequency(const double & fc, const double & fs) filter_specs_.fs = fs; } -sOrderCutOff ButterworthFilter::getOrderCutOff() const { return filter_specs_; } +sOrderCutOff ButterworthFilter::getOrderCutOff() const +{ + return filter_specs_; +} /** * @brief Matlab equivalent : [b, a] = butter(n, Wn, 's') @@ -306,9 +315,18 @@ void ButterworthFilter::printDiscreteTimeTF() const RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "[%s]", stream.str().c_str()); } -std::vector ButterworthFilter::getAn() const { return AnBn_.An; } -std::vector ButterworthFilter::getBn() const { return AnBn_.Bn; } -sDifferenceAnBn ButterworthFilter::getAnBn() const { return AnBn_; } +std::vector ButterworthFilter::getAn() const +{ + return AnBn_.An; +} +std::vector ButterworthFilter::getBn() const +{ + return AnBn_.Bn; +} +sDifferenceAnBn ButterworthFilter::getAnBn() const +{ + return AnBn_; +} void ButterworthFilter::printFilterSpecs() const { diff --git a/common/signal_processing/src/lowpass_filter_1d.cpp b/common/signal_processing/src/lowpass_filter_1d.cpp index 4bc53368a6f6d..8142d907e5c2c 100644 --- a/common/signal_processing/src/lowpass_filter_1d.cpp +++ b/common/signal_processing/src/lowpass_filter_1d.cpp @@ -14,13 +14,24 @@ #include "signal_processing/lowpass_filter_1d.hpp" -LowpassFilter1d::LowpassFilter1d(const double gain) : gain_(gain) {} +LowpassFilter1d::LowpassFilter1d(const double gain) : gain_(gain) +{ +} -void LowpassFilter1d::reset() { x_ = {}; } +void LowpassFilter1d::reset() +{ + x_ = {}; +} -void LowpassFilter1d::reset(const double x) { x_ = x; } +void LowpassFilter1d::reset(const double x) +{ + x_ = x; +} -boost::optional LowpassFilter1d::getValue() const { return x_; } +boost::optional LowpassFilter1d::getValue() const +{ + return x_; +} double LowpassFilter1d::filter(const double u) { diff --git a/common/tensorrt_common/src/tensorrt_common.cpp b/common/tensorrt_common/src/tensorrt_common.cpp index 0b9ea27004cb1..9d31b7a3e870f 100644 --- a/common/tensorrt_common/src/tensorrt_common.cpp +++ b/common/tensorrt_common/src/tensorrt_common.cpp @@ -201,14 +201,20 @@ bool TrtCommon::buildEngineFromOnnx( return true; } -bool TrtCommon::isInitialized() { return is_initialized_; } +bool TrtCommon::isInitialized() +{ + return is_initialized_; +} nvinfer1::Dims TrtCommon::getBindingDimensions(const int32_t index) const { return context_->getBindingDimensions(index); } -int32_t TrtCommon::getNbBindings() { return engine_->getNbBindings(); } +int32_t TrtCommon::getNbBindings() +{ + return engine_->getNbBindings(); +} bool TrtCommon::setBindingDimensions(const int32_t index, const nvinfer1::Dims & dimensions) const { diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp index e04228a8f7b88..bee390bfe29ed 100644 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp @@ -251,7 +251,10 @@ void AutowareAutomaticGoalPanel::onAppendGoal(const PoseStamped::ConstSharedPtr } // Override -void AutowareAutomaticGoalPanel::onCallResult() { updateGUI(); } +void AutowareAutomaticGoalPanel::onCallResult() +{ + updateGUI(); +} void AutowareAutomaticGoalPanel::onGoalListUpdated() { goals_list_widget_ptr_->clear(); diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp index 51f7716863465..659ca64947952 100644 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp @@ -15,7 +15,9 @@ namespace automatic_goal { -AutowareAutomaticGoalSender::AutowareAutomaticGoalSender() : Node("automatic_goal_sender") {} +AutowareAutomaticGoalSender::AutowareAutomaticGoalSender() : Node("automatic_goal_sender") +{ +} void AutowareAutomaticGoalSender::init() { diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp index 6cff428fa6097..453675c4fb201 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp @@ -65,9 +65,15 @@ struct Point3d : public Eigen::Vector3d [[nodiscard]] Point2d to_2d() const; }; -inline Point3d Point2d::to_3d(const double z) const { return Point3d{x(), y(), z}; } +inline Point3d Point2d::to_3d(const double z) const +{ + return Point3d{x(), y(), z}; +} -inline Point2d Point3d::to_2d() const { return Point2d{x(), y()}; } +inline Point2d Point3d::to_2d() const +{ + return Point2d{x(), y()}; +} inline geometry_msgs::msg::Point toMsg(const Point3d & point) { diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp index 3251c29718fec..03316e84483c1 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp @@ -747,7 +747,10 @@ inline geometry_msgs::msg::Twist createTwist( return geometry_msgs::build().linear(velocity).angular(angular); } -inline double calcNorm(const geometry_msgs::msg::Vector3 & v) { return std::hypot(v.x, v.y, v.z); } +inline double calcNorm(const geometry_msgs::msg::Vector3 & v) +{ + return std::hypot(v.x, v.y, v.z); +} /** * @brief Judge whether twist covariance is valid. diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/unit_conversion.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/math/unit_conversion.hpp index 44eb5a3ff0666..38a0a7696775a 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/unit_conversion.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/math/unit_conversion.hpp @@ -19,11 +19,23 @@ namespace tier4_autoware_utils { -constexpr double deg2rad(const double deg) { return deg * pi / 180.0; } -constexpr double rad2deg(const double rad) { return rad * 180.0 / pi; } +constexpr double deg2rad(const double deg) +{ + return deg * pi / 180.0; +} +constexpr double rad2deg(const double rad) +{ + return rad * 180.0 / pi; +} -constexpr double kmph2mps(const double kmph) { return kmph * 1000.0 / 3600.0; } -constexpr double mps2kmph(const double mps) { return mps * 3600.0 / 1000.0; } +constexpr double kmph2mps(const double kmph) +{ + return kmph * 1000.0 / 3600.0; +} +constexpr double mps2kmph(const double mps) +{ + return mps * 3600.0 / 1000.0; +} } // namespace tier4_autoware_utils #endif // TIER4_AUTOWARE_UTILS__MATH__UNIT_CONVERSION_HPP_ diff --git a/common/tier4_autoware_utils/src/math/trigonometry.cpp b/common/tier4_autoware_utils/src/math/trigonometry.cpp index d5b999ee62772..15f5c71012722 100644 --- a/common/tier4_autoware_utils/src/math/trigonometry.cpp +++ b/common/tier4_autoware_utils/src/math/trigonometry.cpp @@ -44,6 +44,9 @@ float sin(float radian) return mul * g_sin_table[idx]; } -float cos(float radian) { return sin(radian + static_cast(tier4_autoware_utils::pi) / 2.f); } +float cos(float radian) +{ + return sin(radian + static_cast(tier4_autoware_utils::pi) / 2.f); +} } // namespace tier4_autoware_utils diff --git a/common/tier4_debug_rviz_plugin/src/float32_multi_array_stamped_pie_chart.cpp b/common/tier4_debug_rviz_plugin/src/float32_multi_array_stamped_pie_chart.cpp index 1fa3aaaf2413d..0187cc3e22de0 100644 --- a/common/tier4_debug_rviz_plugin/src/float32_multi_array_stamped_pie_chart.cpp +++ b/common/tier4_debug_rviz_plugin/src/float32_multi_array_stamped_pie_chart.cpp @@ -301,7 +301,10 @@ void Float32MultiArrayStampedPieChartDisplay::subscribe() } } -void Float32MultiArrayStampedPieChartDisplay::unsubscribe() { sub_.reset(); } +void Float32MultiArrayStampedPieChartDisplay::unsubscribe() +{ + sub_.reset(); +} void Float32MultiArrayStampedPieChartDisplay::onEnable() { diff --git a/common/tier4_debug_rviz_plugin/src/jsk_overlay_utils.cpp b/common/tier4_debug_rviz_plugin/src/jsk_overlay_utils.cpp index b3090346aafd2..b1933ebb3e157 100644 --- a/common/tier4_debug_rviz_plugin/src/jsk_overlay_utils.cpp +++ b/common/tier4_debug_rviz_plugin/src/jsk_overlay_utils.cpp @@ -57,9 +57,15 @@ ScopedPixelBuffer::ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_bu pixel_buffer_->lock(Ogre::HardwareBuffer::HBL_NORMAL); } -ScopedPixelBuffer::~ScopedPixelBuffer() { pixel_buffer_->unlock(); } +ScopedPixelBuffer::~ScopedPixelBuffer() +{ + pixel_buffer_->unlock(); +} -Ogre::HardwarePixelBufferSharedPtr ScopedPixelBuffer::getPixelBuffer() { return pixel_buffer_; } +Ogre::HardwarePixelBufferSharedPtr ScopedPixelBuffer::getPixelBuffer() +{ + return pixel_buffer_; +} QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height) { @@ -119,7 +125,10 @@ OverlayObject::~OverlayObject() // delete overlay_; } -std::string OverlayObject::getName() { return name_; } +std::string OverlayObject::getName() +{ + return name_; +} void OverlayObject::hide() { @@ -135,7 +144,10 @@ void OverlayObject::show() } } -bool OverlayObject::isTextureReady() { return static_cast(texture_); } +bool OverlayObject::isTextureReady() +{ + return static_cast(texture_); +} void OverlayObject::updateTextureSize(unsigned int width, unsigned int height) { @@ -177,14 +189,20 @@ ScopedPixelBuffer OverlayObject::getBuffer() } } -void OverlayObject::setPosition(double left, double top) { panel_->setPosition(left, top); } +void OverlayObject::setPosition(double left, double top) +{ + panel_->setPosition(left, top); +} void OverlayObject::setDimensions(double width, double height) { panel_->setDimensions(width, height); } -bool OverlayObject::isVisible() { return overlay_->isVisible(); } +bool OverlayObject::isVisible() +{ + return overlay_->isVisible(); +} unsigned int OverlayObject::getTextureWidth() { diff --git a/common/tier4_localization_rviz_plugin/src/pose_history/pose_history_display.cpp b/common/tier4_localization_rviz_plugin/src/pose_history/pose_history_display.cpp index e11db6d7f1d9b..9e65b117b3f3f 100644 --- a/common/tier4_localization_rviz_plugin/src/pose_history/pose_history_display.cpp +++ b/common/tier4_localization_rviz_plugin/src/pose_history/pose_history_display.cpp @@ -50,9 +50,15 @@ void PoseHistory::onInitialize() lines_ = std::make_unique(scene_manager_, scene_node_); } -void PoseHistory::onEnable() { subscribe(); } +void PoseHistory::onEnable() +{ + subscribe(); +} -void PoseHistory::onDisable() { unsubscribe(); } +void PoseHistory::onDisable() +{ + unsubscribe(); +} void PoseHistory::update(float wall_dt, float ros_dt) { @@ -67,7 +73,10 @@ void PoseHistory::update(float wall_dt, float ros_dt) } } -void PoseHistory::subscribe() { MFDClass::subscribe(); } +void PoseHistory::subscribe() +{ + MFDClass::subscribe(); +} void PoseHistory::unsubscribe() { diff --git a/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp b/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp index 5b0266311c0e5..49a859e5c7251 100644 --- a/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp +++ b/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp @@ -109,11 +109,20 @@ void PoseHistoryFootprint::onInitialize() scene_node_->attachObject(trajectory_footprint_manual_object_); } -void PoseHistoryFootprint::onEnable() { subscribe(); } +void PoseHistoryFootprint::onEnable() +{ + subscribe(); +} -void PoseHistoryFootprint::onDisable() { unsubscribe(); } +void PoseHistoryFootprint::onDisable() +{ + unsubscribe(); +} -void PoseHistoryFootprint::subscribe() { MFDClass::subscribe(); } +void PoseHistoryFootprint::subscribe() +{ + MFDClass::subscribe(); +} void PoseHistoryFootprint::unsubscribe() { diff --git a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.cpp b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.cpp index c9626532cbecf..ca55bc20276e9 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.cpp +++ b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.cpp @@ -68,7 +68,10 @@ InteractiveObject::InteractiveObject(const Ogre::Vector3 & point) std::generate(uuid_.begin(), uuid_.end(), bit_eng); } -std::array InteractiveObject::uuid() const { return uuid_; } +std::array InteractiveObject::uuid() const +{ + return uuid_; +} void InteractiveObject::twist(geometry_msgs::msg::Twist & twist) const { @@ -113,11 +116,20 @@ void InteractiveObject::update(const Ogre::Vector3 & point) : std::atan2(velocity_.y, velocity_.x); } -void InteractiveObject::reset() { velocity_ = Ogre::Vector3::ZERO; } +void InteractiveObject::reset() +{ + velocity_ = Ogre::Vector3::ZERO; +} -double InteractiveObject::distance(const Ogre::Vector3 & point) { return point_.distance(point); } +double InteractiveObject::distance(const Ogre::Vector3 & point) +{ + return point_.distance(point); +} -InteractiveObjectCollection::InteractiveObjectCollection() { target_ = nullptr; } +InteractiveObjectCollection::InteractiveObjectCollection() +{ + target_ = nullptr; +} void InteractiveObjectCollection::select(const Ogre::Vector3 & point) { diff --git a/common/tier4_planning_rviz_plugin/src/pose_with_uuid_stamped/display.cpp b/common/tier4_planning_rviz_plugin/src/pose_with_uuid_stamped/display.cpp index 76b1a7be0cf0d..8df04e04b2963 100644 --- a/common/tier4_planning_rviz_plugin/src/pose_with_uuid_stamped/display.cpp +++ b/common/tier4_planning_rviz_plugin/src/pose_with_uuid_stamped/display.cpp @@ -76,9 +76,15 @@ void AutowarePoseWithUuidStampedDisplay::onDisable() axes_->getSceneNode()->setVisible(false); } -void AutowarePoseWithUuidStampedDisplay::subscribe() { MFDClass::subscribe(); } +void AutowarePoseWithUuidStampedDisplay::subscribe() +{ + MFDClass::subscribe(); +} -void AutowarePoseWithUuidStampedDisplay::unsubscribe() { MFDClass::unsubscribe(); } +void AutowarePoseWithUuidStampedDisplay::unsubscribe() +{ + MFDClass::unsubscribe(); +} void AutowarePoseWithUuidStampedDisplay::updateVisualization() { diff --git a/common/tier4_planning_rviz_plugin/src/tools/jsk_overlay_utils.cpp b/common/tier4_planning_rviz_plugin/src/tools/jsk_overlay_utils.cpp index 53a75e421b73e..70f3e3736a1a8 100644 --- a/common/tier4_planning_rviz_plugin/src/tools/jsk_overlay_utils.cpp +++ b/common/tier4_planning_rviz_plugin/src/tools/jsk_overlay_utils.cpp @@ -57,9 +57,15 @@ ScopedPixelBuffer::ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_bu pixel_buffer_->lock(Ogre::HardwareBuffer::HBL_NORMAL); } -ScopedPixelBuffer::~ScopedPixelBuffer() { pixel_buffer_->unlock(); } +ScopedPixelBuffer::~ScopedPixelBuffer() +{ + pixel_buffer_->unlock(); +} -Ogre::HardwarePixelBufferSharedPtr ScopedPixelBuffer::getPixelBuffer() { return pixel_buffer_; } +Ogre::HardwarePixelBufferSharedPtr ScopedPixelBuffer::getPixelBuffer() +{ + return pixel_buffer_; +} QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height) { @@ -119,7 +125,10 @@ OverlayObject::~OverlayObject() // delete overlay_; } -std::string OverlayObject::getName() { return name_; } +std::string OverlayObject::getName() +{ + return name_; +} void OverlayObject::hide() { @@ -135,7 +144,10 @@ void OverlayObject::show() } } -bool OverlayObject::isTextureReady() { return static_cast(texture_); } +bool OverlayObject::isTextureReady() +{ + return static_cast(texture_); +} void OverlayObject::updateTextureSize(unsigned int width, unsigned int height) { @@ -177,14 +189,20 @@ ScopedPixelBuffer OverlayObject::getBuffer() } } -void OverlayObject::setPosition(double left, double top) { panel_->setPosition(left, top); } +void OverlayObject::setPosition(double left, double top) +{ + panel_->setPosition(left, top); +} void OverlayObject::setDimensions(double width, double height) { panel_->setDimensions(width, height); } -bool OverlayObject::isVisible() { return overlay_->isVisible(); } +bool OverlayObject::isVisible() +{ + return overlay_->isVisible(); +} unsigned int OverlayObject::getTextureWidth() { diff --git a/common/tier4_planning_rviz_plugin/src/tools/max_velocity.cpp b/common/tier4_planning_rviz_plugin/src/tools/max_velocity.cpp index 9a9f804438f88..0520b7292080f 100644 --- a/common/tier4_planning_rviz_plugin/src/tools/max_velocity.cpp +++ b/common/tier4_planning_rviz_plugin/src/tools/max_velocity.cpp @@ -122,7 +122,10 @@ void MaxVelocityDisplay::subscribe() } } -void MaxVelocityDisplay::unsubscribe() { max_vel_sub_.reset(); } +void MaxVelocityDisplay::unsubscribe() +{ + max_vel_sub_.reset(); +} void MaxVelocityDisplay::processMessage( const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg_ptr) diff --git a/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.cpp b/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.cpp index 9e3ea7ed3ab4d..5ec8f9b02d0e8 100644 --- a/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.cpp +++ b/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.cpp @@ -98,9 +98,13 @@ void AutowareScreenCapturePanel::onInitialize() std::bind(&AutowareScreenCapturePanel::onCaptureTrigger, this, _1, _2)); } -void onPrefixChanged() {} +void onPrefixChanged() +{ +} -void AutowareScreenCapturePanel::onRateChanged() {} +void AutowareScreenCapturePanel::onRateChanged() +{ +} void AutowareScreenCapturePanel::onClickScreenCapture() { @@ -188,9 +192,15 @@ void AutowareScreenCapturePanel::update() setFormatDate(ros_time_label_, rclcpp::Clock().now().seconds()); } -void AutowareScreenCapturePanel::save(rviz_common::Config config) const { Panel::save(config); } +void AutowareScreenCapturePanel::save(rviz_common::Config config) const +{ + Panel::save(config); +} -void AutowareScreenCapturePanel::load(const rviz_common::Config & config) { Panel::load(config); } +void AutowareScreenCapturePanel::load(const rviz_common::Config & config) +{ + Panel::load(config); +} AutowareScreenCapturePanel::~AutowareScreenCapturePanel() = default; diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp index 845f02fb18b89..3cc04347d2ed3 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp @@ -26,7 +26,10 @@ #include #include -inline std::string Bool2String(const bool var) { return var ? "True" : "False"; } +inline std::string Bool2String(const bool var) +{ + return var ? "True" : "False"; +} namespace rviz_plugins { diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/jsk_overlay_utils.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/jsk_overlay_utils.cpp index 70ab348a670db..36c5d1ce82693 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/jsk_overlay_utils.cpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/jsk_overlay_utils.cpp @@ -57,9 +57,15 @@ ScopedPixelBuffer::ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_bu pixel_buffer_->lock(Ogre::HardwareBuffer::HBL_NORMAL); } -ScopedPixelBuffer::~ScopedPixelBuffer() { pixel_buffer_->unlock(); } +ScopedPixelBuffer::~ScopedPixelBuffer() +{ + pixel_buffer_->unlock(); +} -Ogre::HardwarePixelBufferSharedPtr ScopedPixelBuffer::getPixelBuffer() { return pixel_buffer_; } +Ogre::HardwarePixelBufferSharedPtr ScopedPixelBuffer::getPixelBuffer() +{ + return pixel_buffer_; +} QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height) { @@ -116,7 +122,10 @@ OverlayObject::~OverlayObject() // delete overlay_; } -std::string OverlayObject::getName() { return name_; } +std::string OverlayObject::getName() +{ + return name_; +} void OverlayObject::hide() { @@ -132,7 +141,10 @@ void OverlayObject::show() } } -bool OverlayObject::isTextureReady() { return static_cast(texture_); } +bool OverlayObject::isTextureReady() +{ + return static_cast(texture_); +} void OverlayObject::updateTextureSize(unsigned int width, unsigned int height) { @@ -174,14 +186,20 @@ ScopedPixelBuffer OverlayObject::getBuffer() } } -void OverlayObject::setPosition(double left, double top) { panel_->setPosition(left, top); } +void OverlayObject::setPosition(double left, double top) +{ + panel_->setPosition(left, top); +} void OverlayObject::setDimensions(double width, double height) { panel_->setDimensions(width, height); } -bool OverlayObject::isVisible() { return overlay_->isVisible(); } +bool OverlayObject::isVisible() +{ + return overlay_->isVisible(); +} unsigned int OverlayObject::getTextureWidth() { diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp index 92ede8b3a9c05..168c0c8f23f6e 100644 --- a/control/autonomous_emergency_braking/src/node.cpp +++ b/control/autonomous_emergency_braking/src/node.cpp @@ -138,14 +138,20 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) timer_ = rclcpp::create_timer(this, get_clock(), period_ns, std::bind(&AEB::onTimer, this)); } -void AEB::onTimer() { updater_.force_update(); } +void AEB::onTimer() +{ + updater_.force_update(); +} void AEB::onVelocity(const VelocityReport::ConstSharedPtr input_msg) { current_velocity_ptr_ = input_msg; } -void AEB::onImu(const Imu::ConstSharedPtr input_msg) { imu_ptr_ = input_msg; } +void AEB::onImu(const Imu::ConstSharedPtr input_msg) +{ + imu_ptr_ = input_msg; +} void AEB::onPredictedTrajectory( const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr input_msg) diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index a3508018e0869..ed9149cd6df2d 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -204,7 +204,10 @@ void LaneDepartureCheckerNode::onLaneletMapBin(const HADMapBin::ConstSharedPtr m shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets); } -void LaneDepartureCheckerNode::onRoute(const LaneletRoute::ConstSharedPtr msg) { route_ = msg; } +void LaneDepartureCheckerNode::onRoute(const LaneletRoute::ConstSharedPtr msg) +{ + route_ = msg; +} void LaneDepartureCheckerNode::onReferenceTrajectory(const Trajectory::ConstSharedPtr msg) { diff --git a/control/mpc_lateral_controller/src/lowpass_filter.cpp b/control/mpc_lateral_controller/src/lowpass_filter.cpp index c40ee6987a6d2..7a936e8c9d3c8 100644 --- a/control/mpc_lateral_controller/src/lowpass_filter.cpp +++ b/control/mpc_lateral_controller/src/lowpass_filter.cpp @@ -23,7 +23,9 @@ Butterworth2dFilter::Butterworth2dFilter(double dt, double f_cutoff_hz) initialize(dt, f_cutoff_hz); } -Butterworth2dFilter::~Butterworth2dFilter() {} +Butterworth2dFilter::~Butterworth2dFilter() +{ +} void Butterworth2dFilter::initialize(const double & dt, const double & f_cutoff_hz) { diff --git a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp index 2a129d544587b..6b89a3325ad05 100644 --- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -178,7 +178,9 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} m_mpc.setClock(node_->get_clock()); } -MpcLateralController::~MpcLateralController() {} +MpcLateralController::~MpcLateralController() +{ +} trajectory_follower::LateralOutput MpcLateralController::run( trajectory_follower::InputData const & input_data) diff --git a/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp b/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp index dbb11e7c0e100..9bd4d4ab02769 100644 --- a/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp +++ b/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp @@ -19,7 +19,9 @@ namespace autoware::motion::control::mpc_lateral_controller { -QPSolverOSQP::QPSolverOSQP(const rclcpp::Logger & logger) : logger_{logger} {} +QPSolverOSQP::QPSolverOSQP(const rclcpp::Logger & logger) : logger_{logger} +{ +} bool QPSolverOSQP::solve( const Eigen::MatrixXd & h_mat, const Eigen::MatrixXd & f_vec, const Eigen::MatrixXd & a, const Eigen::VectorXd & lb, const Eigen::VectorXd & ub, const Eigen::VectorXd & lb_a, diff --git a/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstr_fast.cpp b/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstr_fast.cpp index 33cd60fe0f318..a8b95e9e3d980 100644 --- a/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstr_fast.cpp +++ b/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstr_fast.cpp @@ -16,7 +16,9 @@ namespace autoware::motion::control::mpc_lateral_controller { -QPSolverEigenLeastSquareLLT::QPSolverEigenLeastSquareLLT() {} +QPSolverEigenLeastSquareLLT::QPSolverEigenLeastSquareLLT() +{ +} bool QPSolverEigenLeastSquareLLT::solve( const Eigen::MatrixXd & h_mat, const Eigen::MatrixXd & f_vec, const Eigen::MatrixXd & /*a*/, const Eigen::VectorXd & /*lb*/, const Eigen::VectorXd & /*ub*/, const Eigen::VectorXd & /*lb_a*/, diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp index c3e276f628e93..97e3f2742f546 100644 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp +++ b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp @@ -20,10 +20,28 @@ VehicleModelInterface::VehicleModelInterface(int dim_x, int dim_u, int dim_y, do : m_dim_x(dim_x), m_dim_u(dim_u), m_dim_y(dim_y), m_wheelbase(wheelbase) { } -int VehicleModelInterface::getDimX() { return m_dim_x; } -int VehicleModelInterface::getDimU() { return m_dim_u; } -int VehicleModelInterface::getDimY() { return m_dim_y; } -double VehicleModelInterface::getWheelbase() { return m_wheelbase; } -void VehicleModelInterface::setVelocity(const double velocity) { m_velocity = velocity; } -void VehicleModelInterface::setCurvature(const double curvature) { m_curvature = curvature; } +int VehicleModelInterface::getDimX() +{ + return m_dim_x; +} +int VehicleModelInterface::getDimU() +{ + return m_dim_u; +} +int VehicleModelInterface::getDimY() +{ + return m_dim_y; +} +double VehicleModelInterface::getWheelbase() +{ + return m_wheelbase; +} +void VehicleModelInterface::setVelocity(const double velocity) +{ + m_velocity = velocity; +} +void VehicleModelInterface::setCurvature(const double curvature) +{ + m_curvature = curvature; +} } // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/operation_mode_transition_manager/src/compatibility.cpp b/control/operation_mode_transition_manager/src/compatibility.cpp index 6a56f23612a6e..66a5fbd6b1b13 100644 --- a/control/operation_mode_transition_manager/src/compatibility.cpp +++ b/control/operation_mode_transition_manager/src/compatibility.cpp @@ -42,7 +42,10 @@ void Compatibility::on_autoware_engage(const AutowareEngage::ConstSharedPtr msg) autoware_engage_ = msg; } -void Compatibility::on_gate_mode(const GateMode::ConstSharedPtr msg) { gate_mode_ = msg; } +void Compatibility::on_gate_mode(const GateMode::ConstSharedPtr msg) +{ + gate_mode_ = msg; +} void Compatibility::on_selector_mode(const SelectorModeMsg::ConstSharedPtr msg) { diff --git a/control/pure_pursuit/src/pure_pursuit_core/interpolate.cpp b/control/pure_pursuit/src/pure_pursuit_core/interpolate.cpp index 6ae083d64023d..8890e0c7da25d 100644 --- a/control/pure_pursuit/src/pure_pursuit_core/interpolate.cpp +++ b/control/pure_pursuit/src/pure_pursuit_core/interpolate.cpp @@ -83,9 +83,16 @@ bool LinearInterpolate::interpolate( * spline interpolation */ -SplineInterpolate::SplineInterpolate() {} -SplineInterpolate::SplineInterpolate(const std::vector & x) { generateSpline(x); } -SplineInterpolate::~SplineInterpolate() {} +SplineInterpolate::SplineInterpolate() +{ +} +SplineInterpolate::SplineInterpolate(const std::vector & x) +{ + generateSpline(x); +} +SplineInterpolate::~SplineInterpolate() +{ +} void SplineInterpolate::generateSpline(const std::vector & x) { int N = x.size(); diff --git a/control/pure_pursuit/src/pure_pursuit_core/planning_utils.cpp b/control/pure_pursuit/src/pure_pursuit_core/planning_utils.cpp index c873516d87694..2597d7087ad61 100644 --- a/control/pure_pursuit/src/pure_pursuit_core/planning_utils.cpp +++ b/control/pure_pursuit/src/pure_pursuit_core/planning_utils.cpp @@ -197,7 +197,10 @@ bool isInPolygon( return isInPolygon(polygon_conv, point_conv); } -double kmph2mps(const double velocity_kmph) { return (velocity_kmph * 1000) / (60 * 60); } +double kmph2mps(const double velocity_kmph) +{ + return (velocity_kmph * 1000) / (60 * 60); +} double normalizeEulerAngle(const double euler) { diff --git a/control/trajectory_follower_base/src/longitudinal_controller_base.cpp b/control/trajectory_follower_base/src/longitudinal_controller_base.cpp index c358f732643e0..f7779158f5791 100644 --- a/control/trajectory_follower_base/src/longitudinal_controller_base.cpp +++ b/control/trajectory_follower_base/src/longitudinal_controller_base.cpp @@ -20,5 +20,8 @@ void LongitudinalControllerBase::sync(LateralSyncData const & lateral_sync_data) { lateral_sync_data_ = lateral_sync_data; } -void LongitudinalControllerBase::reset() { lateral_sync_data_.is_steer_converged = false; } +void LongitudinalControllerBase::reset() +{ + lateral_sync_data_.is_steer_converged = false; +} } // namespace autoware::motion::control::trajectory_follower diff --git a/control/trajectory_follower_node/src/simple_trajectory_follower.cpp b/control/trajectory_follower_node/src/simple_trajectory_follower.cpp index 9d0160a8cc65f..f073574cc229f 100644 --- a/control/trajectory_follower_node/src/simple_trajectory_follower.cpp +++ b/control/trajectory_follower_node/src/simple_trajectory_follower.cpp @@ -109,7 +109,10 @@ double SimpleTrajectoryFollower::calcAccCmd() return acc; } -bool SimpleTrajectoryFollower::checkData() { return (trajectory_ && odometry_); } +bool SimpleTrajectoryFollower::checkData() +{ + return (trajectory_ && odometry_); +} } // namespace simple_trajectory_follower diff --git a/control/vehicle_cmd_gate/src/pause_interface.cpp b/control/vehicle_cmd_gate/src/pause_interface.cpp index 99798a2c0d607..e5a844272abd3 100644 --- a/control/vehicle_cmd_gate/src/pause_interface.cpp +++ b/control/vehicle_cmd_gate/src/pause_interface.cpp @@ -29,7 +29,10 @@ PauseInterface::PauseInterface(rclcpp::Node * node) : node_(node) publish(); } -bool PauseInterface::is_paused() { return is_paused_; } +bool PauseInterface::is_paused() +{ + return is_paused_; +} void PauseInterface::publish() { diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp index 5696e0445e407..49735da876d9a 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp @@ -20,7 +20,9 @@ namespace vehicle_cmd_gate { -VehicleCmdFilter::VehicleCmdFilter() {} +VehicleCmdFilter::VehicleCmdFilter() +{ +} void VehicleCmdFilter::limitLongitudinalWithVel(AckermannControlCommand & input) const { diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index d2fea975be174..b10e1828d35f3 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -574,7 +574,10 @@ void VehicleCmdGate::onGateMode(GateMode::ConstSharedPtr msg) } } -void VehicleCmdGate::onEngage(EngageMsg::ConstSharedPtr msg) { is_engaged_ = msg->engage; } +void VehicleCmdGate::onEngage(EngageMsg::ConstSharedPtr msg) +{ + is_engaged_ = msg->engage; +} void VehicleCmdGate::onEngageService( const EngageSrv::Request::SharedPtr request, const EngageSrv::Response::SharedPtr response) diff --git a/evaluator/planning_evaluator/src/metrics_calculator.cpp b/evaluator/planning_evaluator/src/metrics_calculator.cpp index adfdddd7d2c5e..da04d6fcd2bfa 100644 --- a/evaluator/planning_evaluator/src/metrics_calculator.cpp +++ b/evaluator/planning_evaluator/src/metrics_calculator.cpp @@ -106,9 +106,15 @@ void MetricsCalculator::setPredictedObjects(const PredictedObjects & objects) dynamic_objects_ = objects; } -void MetricsCalculator::setEgoPose(const geometry_msgs::msg::Pose & pose) { ego_pose_ = pose; } +void MetricsCalculator::setEgoPose(const geometry_msgs::msg::Pose & pose) +{ + ego_pose_ = pose; +} -Pose MetricsCalculator::getEgoPose() { return ego_pose_; } +Pose MetricsCalculator::getEgoPose() +{ + return ego_pose_; +} Trajectory MetricsCalculator::getLookaheadTrajectory( const Trajectory & traj, const double max_dist_m, const double max_time_s) const diff --git a/localization/ekf_localizer/include/ekf_localizer/numeric.hpp b/localization/ekf_localizer/include/ekf_localizer/numeric.hpp index 0719c64e65ac1..6554f80aee014 100644 --- a/localization/ekf_localizer/include/ekf_localizer/numeric.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/numeric.hpp @@ -19,8 +19,14 @@ #include -inline bool hasInf(const Eigen::MatrixXd & v) { return v.array().isInf().any(); } +inline bool hasInf(const Eigen::MatrixXd & v) +{ + return v.array().isInf().any(); +} -inline bool hasNan(const Eigen::MatrixXd & v) { return v.array().isNaN().any(); } +inline bool hasNan(const Eigen::MatrixXd & v) +{ + return v.array().isNaN().any(); +} #endif // EKF_LOCALIZER__NUMERIC_HPP_ diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp index 764f6c63812d1..8ad9e77453db5 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -127,7 +127,9 @@ GyroOdometer::GyroOdometer(const rclcpp::NodeOptions & options) // TODO(YamatoAndo) createTimer } -GyroOdometer::~GyroOdometer() {} +GyroOdometer::~GyroOdometer() +{ +} void GyroOdometer::callbackVehicleTwist( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_ptr) diff --git a/localization/localization_error_monitor/src/node.cpp b/localization/localization_error_monitor/src/node.cpp index b5647e46c5236..75858c1f4be4d 100644 --- a/localization/localization_error_monitor/src/node.cpp +++ b/localization/localization_error_monitor/src/node.cpp @@ -63,7 +63,10 @@ LocalizationErrorMonitor::LocalizationErrorMonitor() this, get_clock(), 100ms, std::bind(&LocalizationErrorMonitor::onTimer, this)); } -void LocalizationErrorMonitor::onTimer() { updater_.force_update(); } +void LocalizationErrorMonitor::onTimer() +{ + updater_.force_update(); +} void LocalizationErrorMonitor::checkLocalizationAccuracy( diagnostic_updater::DiagnosticStatusWrapper & stat) diff --git a/localization/ndt_scan_matcher/src/pose_array_interpolator.cpp b/localization/ndt_scan_matcher/src/pose_array_interpolator.cpp index b770295277346..d7635c052142f 100644 --- a/localization/ndt_scan_matcher/src/pose_array_interpolator.cpp +++ b/localization/ndt_scan_matcher/src/pose_array_interpolator.cpp @@ -70,7 +70,10 @@ geometry_msgs::msg::PoseWithCovarianceStamped PoseArrayInterpolator::get_new_pos return *new_pose_ptr_; } -bool PoseArrayInterpolator::is_success() { return success_; } +bool PoseArrayInterpolator::is_success() +{ + return success_; +} bool PoseArrayInterpolator::validate_time_stamp_difference( const rclcpp::Time & target_time, const rclcpp::Time & reference_time, diff --git a/map/map_height_fitter/src/map_height_fitter.cpp b/map/map_height_fitter/src/map_height_fitter.cpp index 25e4003479f73..1bc1a109e6233 100644 --- a/map/map_height_fitter/src/map_height_fitter.cpp +++ b/map/map_height_fitter/src/map_height_fitter.cpp @@ -195,9 +195,14 @@ Point MapHeightFitter::Impl::fit(const Point & position, const std::string & fra return result; } -MapHeightFitter::MapHeightFitter(rclcpp::Node * node) { impl_ = std::make_unique(node); } +MapHeightFitter::MapHeightFitter(rclcpp::Node * node) +{ + impl_ = std::make_unique(node); +} -MapHeightFitter::~MapHeightFitter() {} +MapHeightFitter::~MapHeightFitter() +{ +} Point MapHeightFitter::fit(const Point & position, const std::string & frame) { diff --git a/perception/bytetrack/lib/src/byte_tracker.cpp b/perception/bytetrack/lib/src/byte_tracker.cpp index 77daf8437ea02..94aae0a5c94cc 100644 --- a/perception/bytetrack/lib/src/byte_tracker.cpp +++ b/perception/bytetrack/lib/src/byte_tracker.cpp @@ -51,7 +51,9 @@ ByteTracker::ByteTracker( std::cout << "Init ByteTrack!" << std::endl; } -ByteTracker::~ByteTracker() {} +ByteTracker::~ByteTracker() +{ +} std::vector ByteTracker::update(const std::vector & objects) { diff --git a/perception/bytetrack/lib/src/strack.cpp b/perception/bytetrack/lib/src/strack.cpp index c57085fe8d248..b06456098cc38 100644 --- a/perception/bytetrack/lib/src/strack.cpp +++ b/perception/bytetrack/lib/src/strack.cpp @@ -61,7 +61,9 @@ STrack::STrack(std::vector tlwh_, float score, int label) this->label = label; } -STrack::~STrack() {} +STrack::~STrack() +{ +} void STrack::activate(byte_kalman::KalmanFilter & kalman_filter, int frame_id) { @@ -186,7 +188,10 @@ std::vector STrack::tlwh_to_xyah(std::vector tlwh_tmp) return tlwh_output; } -std::vector STrack::to_xyah() { return tlwh_to_xyah(tlwh); } +std::vector STrack::to_xyah() +{ + return tlwh_to_xyah(tlwh); +} std::vector STrack::tlbr_to_tlwh(std::vector & tlbr) { @@ -195,9 +200,15 @@ std::vector STrack::tlbr_to_tlwh(std::vector & tlbr) return tlbr; } -void STrack::mark_lost() { state = TrackState::Lost; } +void STrack::mark_lost() +{ + state = TrackState::Lost; +} -void STrack::mark_removed() { state = TrackState::Removed; } +void STrack::mark_removed() +{ + state = TrackState::Removed; +} int STrack::next_id() { @@ -206,7 +217,10 @@ int STrack::next_id() return _count; } -int STrack::end_frame() { return this->frame_id; } +int STrack::end_frame() +{ + return this->frame_id; +} void STrack::multi_predict( std::vector & stracks, byte_kalman::KalmanFilter & kalman_filter) diff --git a/perception/bytetrack/src/bytetrack_visualizer_node.cpp b/perception/bytetrack/src/bytetrack_visualizer_node.cpp index 07189c67d5f03..1ab65272333d3 100644 --- a/perception/bytetrack/src/bytetrack_visualizer_node.cpp +++ b/perception/bytetrack/src/bytetrack_visualizer_node.cpp @@ -39,7 +39,10 @@ ByteTrackVisualizerNode::ByteTrackVisualizerNode(const rclcpp::NodeOptions & nod image_pub_ = image_transport::create_publisher(this, "~/out/image"); } -ByteTrackVisualizerNode::~ByteTrackVisualizerNode() { cv::destroyAllWindows(); } +ByteTrackVisualizerNode::~ByteTrackVisualizerNode() +{ + cv::destroyAllWindows(); +} bool ByteTrackVisualizerNode::get_topic_qos(const std::string & query_topic, rclcpp::QoS & qos) { diff --git a/perception/euclidean_cluster/lib/euclidean_cluster.cpp b/perception/euclidean_cluster/lib/euclidean_cluster.cpp index 425cac67aa0e2..bd8270bd6b881 100644 --- a/perception/euclidean_cluster/lib/euclidean_cluster.cpp +++ b/perception/euclidean_cluster/lib/euclidean_cluster.cpp @@ -19,7 +19,9 @@ namespace euclidean_cluster { -EuclideanCluster::EuclideanCluster() {} +EuclideanCluster::EuclideanCluster() +{ +} EuclideanCluster::EuclideanCluster(bool use_height, int min_cluster_size, int max_cluster_size) : EuclideanClusterInterface(use_height, min_cluster_size, max_cluster_size) diff --git a/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp b/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp index 5f747b332936e..b8276296dde5f 100644 --- a/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp +++ b/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp @@ -21,7 +21,9 @@ namespace euclidean_cluster { -VoxelGridBasedEuclideanCluster::VoxelGridBasedEuclideanCluster() {} +VoxelGridBasedEuclideanCluster::VoxelGridBasedEuclideanCluster() +{ +} VoxelGridBasedEuclideanCluster::VoxelGridBasedEuclideanCluster( bool use_height, int min_cluster_size, int max_cluster_size) diff --git a/perception/lidar_apollo_instance_segmentation/src/feature_generator.cpp b/perception/lidar_apollo_instance_segmentation/src/feature_generator.cpp index 5a4b4a6b79f6c..eb563f0abba8c 100644 --- a/perception/lidar_apollo_instance_segmentation/src/feature_generator.cpp +++ b/perception/lidar_apollo_instance_segmentation/src/feature_generator.cpp @@ -18,7 +18,10 @@ namespace { -inline float normalizeIntensity(float intensity) { return intensity / 255.0f; } +inline float normalizeIntensity(float intensity) +{ + return intensity / 255.0f; +} } // namespace FeatureGenerator::FeatureGenerator( diff --git a/perception/lidar_apollo_instance_segmentation/src/feature_map.cpp b/perception/lidar_apollo_instance_segmentation/src/feature_map.cpp index 63cb84c8bfe9a..7c097f1fc519d 100644 --- a/perception/lidar_apollo_instance_segmentation/src/feature_map.cpp +++ b/perception/lidar_apollo_instance_segmentation/src/feature_map.cpp @@ -44,7 +44,9 @@ FeatureMap::FeatureMap(const int width, const int height, const int range) count_data = &(map_data[0]) + width * height * 2; nonempty_data = &(map_data[0]) + width * height * 3; } -void FeatureMap::initializeMap([[maybe_unused]] std::vector & map) {} +void FeatureMap::initializeMap([[maybe_unused]] std::vector & map) +{ +} void FeatureMap::resetMap([[maybe_unused]] std::vector & map) { const int size = width * height; @@ -66,7 +68,9 @@ FeatureMapWithIntensity::FeatureMapWithIntensity(const int width, const int heig mean_intensity_data = &(map_data[0]) + width * height * 4; nonempty_data = &(map_data[0]) + width * height * 5; } -void FeatureMapWithIntensity::initializeMap([[maybe_unused]] std::vector & map) {} +void FeatureMapWithIntensity::initializeMap([[maybe_unused]] std::vector & map) +{ +} void FeatureMapWithIntensity::resetMap([[maybe_unused]] std::vector & map) { const int size = width * height; diff --git a/perception/lidar_apollo_segmentation_tvm/src/feature_generator.cpp b/perception/lidar_apollo_segmentation_tvm/src/feature_generator.cpp index c103af1b40646..0575b633dd539 100644 --- a/perception/lidar_apollo_segmentation_tvm/src/feature_generator.cpp +++ b/perception/lidar_apollo_segmentation_tvm/src/feature_generator.cpp @@ -31,7 +31,10 @@ namespace lidar_apollo_segmentation_tvm { namespace { -inline float32_t normalizeIntensity(float32_t intensity) { return intensity / 255; } +inline float32_t normalizeIntensity(float32_t intensity) +{ + return intensity / 255; +} } // namespace FeatureGenerator::FeatureGenerator( diff --git a/perception/lidar_apollo_segmentation_tvm/src/feature_map.cpp b/perception/lidar_apollo_segmentation_tvm/src/feature_map.cpp index 959561965f657..5533ba00a645c 100644 --- a/perception/lidar_apollo_segmentation_tvm/src/feature_map.cpp +++ b/perception/lidar_apollo_segmentation_tvm/src/feature_map.cpp @@ -53,7 +53,10 @@ FeatureMap::FeatureMap(const int32_t width, const int32_t height, const int32_t count_data = &(map_data[0]) + width * height * 2; nonempty_data = &(map_data[0]) + width * height * 3; } -void FeatureMap::initializeMap(std::vector & map) { (void)map; } +void FeatureMap::initializeMap(std::vector & map) +{ + (void)map; +} void FeatureMap::resetMap(std::vector & map) { const int32_t size = width * height; @@ -77,7 +80,10 @@ FeatureMapWithIntensity::FeatureMapWithIntensity( mean_intensity_data = &(map_data[0]) + width * height * 4; nonempty_data = &(map_data[0]) + width * height * 5; } -void FeatureMapWithIntensity::initializeMap(std::vector & map) { (void)map; } +void FeatureMapWithIntensity::initializeMap(std::vector & map) +{ + (void)map; +} void FeatureMapWithIntensity::resetMap(std::vector & map) { const int32_t size = width * height; diff --git a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp index 49ecbae311b58..f6f4531450d05 100644 --- a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp +++ b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp @@ -194,7 +194,10 @@ std::shared_ptr ApolloLidarSegmentation::detec return output; } -const std::string & ApolloLidarSegmentation::network_name() const { return config.network_name; } +const std::string & ApolloLidarSegmentation::network_name() const +{ + return config.network_name; +} tvm_utility::Version ApolloLidarSegmentation::version_check() const { diff --git a/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp b/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp index 4840b63940df1..2255df99dbcf4 100644 --- a/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp +++ b/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp @@ -24,7 +24,9 @@ namespace centerpoint { -TensorRTWrapper::TensorRTWrapper(const CenterPointConfig & config) : config_(config) {} +TensorRTWrapper::TensorRTWrapper(const CenterPointConfig & config) : config_(config) +{ +} TensorRTWrapper::~TensorRTWrapper() { diff --git a/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu b/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu index 765f678784574..ea2111862759a 100644 --- a/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu +++ b/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu @@ -48,7 +48,10 @@ struct score_greater __device__ bool operator()(const Box3D & lb, const Box3D & rb) { return lb.score > rb.score; } }; -__device__ inline float sigmoid(float x) { return 1.0f / (1.0f + expf(-x)); } +__device__ inline float sigmoid(float x) +{ + return 1.0f / (1.0f + expf(-x)); +} __global__ void generateBoxes3D_kernel( const float * out_heatmap, const float * out_offset, const float * out_z, const float * out_dim, diff --git a/perception/lidar_centerpoint_tvm/lib/centerpoint_tvm.cpp b/perception/lidar_centerpoint_tvm/lib/centerpoint_tvm.cpp index fb15a6c7aeaba..7b31bdf3c8fd3 100644 --- a/perception/lidar_centerpoint_tvm/lib/centerpoint_tvm.cpp +++ b/perception/lidar_centerpoint_tvm/lib/centerpoint_tvm.cpp @@ -174,7 +174,9 @@ CenterPointTVM::CenterPointTVM( initPtr(); } -CenterPointTVM::~CenterPointTVM() {} +CenterPointTVM::~CenterPointTVM() +{ +} void CenterPointTVM::initPtr() { diff --git a/perception/lidar_centerpoint_tvm/lib/postprocess/generate_detected_boxes.cpp b/perception/lidar_centerpoint_tvm/lib/postprocess/generate_detected_boxes.cpp index 405223c800eba..c372e3b32805c 100644 --- a/perception/lidar_centerpoint_tvm/lib/postprocess/generate_detected_boxes.cpp +++ b/perception/lidar_centerpoint_tvm/lib/postprocess/generate_detected_boxes.cpp @@ -52,7 +52,10 @@ struct score_greater bool operator()(const Box3D & lb, const Box3D & rb) { return lb.score > rb.score; } }; -inline float sigmoid(float x) { return 1.0f / (1.0f + expf(-x)); } +inline float sigmoid(float x) +{ + return 1.0f / (1.0f + expf(-x)); +} void generateBoxes3D_worker( const std::vector & out_heatmap, const std::vector & out_offset, diff --git a/perception/tensorrt_yolo/lib/src/plugins/mish_plugin.cpp b/perception/tensorrt_yolo/lib/src/plugins/mish_plugin.cpp index efdf8540ee922..fe8688ed946e8 100644 --- a/perception/tensorrt_yolo/lib/src/plugins/mish_plugin.cpp +++ b/perception/tensorrt_yolo/lib/src/plugins/mish_plugin.cpp @@ -74,7 +74,9 @@ inline int64_t volume(const Dims & d) namespace yolo { -MishPlugin::MishPlugin() {} +MishPlugin::MishPlugin() +{ +} // create the plugin at runtime from a byte stream MishPlugin::MishPlugin(const void * data, size_t length) @@ -85,28 +87,54 @@ MishPlugin::MishPlugin(const void * data, size_t length) // IPluginV2 Methods -const char * MishPlugin::getPluginType() const noexcept { return MISH_PLUGIN_NAME; } +const char * MishPlugin::getPluginType() const noexcept +{ + return MISH_PLUGIN_NAME; +} -const char * MishPlugin::getPluginVersion() const noexcept { return MISH_PLUGIN_VERSION; } +const char * MishPlugin::getPluginVersion() const noexcept +{ + return MISH_PLUGIN_VERSION; +} -int MishPlugin::getNbOutputs() const noexcept { return 1; } +int MishPlugin::getNbOutputs() const noexcept +{ + return 1; +} -int MishPlugin::initialize() noexcept { return 0; } +int MishPlugin::initialize() noexcept +{ + return 0; +} -void MishPlugin::terminate() noexcept {} +void MishPlugin::terminate() noexcept +{ +} -size_t MishPlugin::getSerializationSize() const noexcept { return 0; } +size_t MishPlugin::getSerializationSize() const noexcept +{ + return 0; +} -void MishPlugin::serialize(void * buffer) const noexcept { (void)buffer; } +void MishPlugin::serialize(void * buffer) const noexcept +{ + (void)buffer; +} -void MishPlugin::destroy() noexcept { delete this; } +void MishPlugin::destroy() noexcept +{ + delete this; +} void MishPlugin::setPluginNamespace(const char * pluginNamespace) noexcept { mPluginNamespace = pluginNamespace; } -const char * MishPlugin::getPluginNamespace() const noexcept { return mPluginNamespace; } +const char * MishPlugin::getPluginNamespace() const noexcept +{ + return mPluginNamespace; +} // IPluginV2Ext Methods @@ -204,11 +232,20 @@ MishPluginCreator::MishPluginCreator() mFC.fields = mPluginAttributes.data(); } -const char * MishPluginCreator::getPluginName() const noexcept { return MISH_PLUGIN_NAME; } +const char * MishPluginCreator::getPluginName() const noexcept +{ + return MISH_PLUGIN_NAME; +} -const char * MishPluginCreator::getPluginVersion() const noexcept { return MISH_PLUGIN_VERSION; } +const char * MishPluginCreator::getPluginVersion() const noexcept +{ + return MISH_PLUGIN_VERSION; +} -const PluginFieldCollection * MishPluginCreator::getFieldNames() noexcept { return &mFC; } +const PluginFieldCollection * MishPluginCreator::getFieldNames() noexcept +{ + return &mFC; +} IPluginV2DynamicExt * MishPluginCreator::createPlugin( const char * name, const PluginFieldCollection * fc) noexcept diff --git a/perception/tensorrt_yolo/lib/src/plugins/nms_plugin.cpp b/perception/tensorrt_yolo/lib/src/plugins/nms_plugin.cpp index 783e8d83f7a01..4ff96bf15257b 100644 --- a/perception/tensorrt_yolo/lib/src/plugins/nms_plugin.cpp +++ b/perception/tensorrt_yolo/lib/src/plugins/nms_plugin.cpp @@ -102,15 +102,29 @@ NMSPlugin::NMSPlugin(void const * data, size_t length) read(d, count_); } -const char * NMSPlugin::getPluginType() const noexcept { return NMS_PLUGIN_NAME; } +const char * NMSPlugin::getPluginType() const noexcept +{ + return NMS_PLUGIN_NAME; +} -const char * NMSPlugin::getPluginVersion() const noexcept { return NMS_PLUGIN_VERSION; } +const char * NMSPlugin::getPluginVersion() const noexcept +{ + return NMS_PLUGIN_VERSION; +} -int NMSPlugin::getNbOutputs() const noexcept { return 3; } +int NMSPlugin::getNbOutputs() const noexcept +{ + return 3; +} -int NMSPlugin::initialize() noexcept { return 0; } +int NMSPlugin::initialize() noexcept +{ + return 0; +} -void NMSPlugin::terminate() noexcept {} +void NMSPlugin::terminate() noexcept +{ +} size_t NMSPlugin::getSerializationSize() const noexcept { @@ -125,11 +139,20 @@ void NMSPlugin::serialize(void * buffer) const noexcept write(d, count_); } -void NMSPlugin::destroy() noexcept { delete this; } +void NMSPlugin::destroy() noexcept +{ + delete this; +} -void NMSPlugin::setPluginNamespace(const char * N) noexcept { (void)N; } +void NMSPlugin::setPluginNamespace(const char * N) noexcept +{ + (void)N; +} -const char * NMSPlugin::getPluginNamespace() const noexcept { return NMS_PLUGIN_NAMESPACE; } +const char * NMSPlugin::getPluginNamespace() const noexcept +{ + return NMS_PLUGIN_NAMESPACE; +} // IPluginV2Ext Methods @@ -216,16 +239,33 @@ int NMSPlugin::enqueue( getWorkspaceSize(inputDesc, 3, outputDesc, 3), stream); } -NMSPluginCreator::NMSPluginCreator() {} +NMSPluginCreator::NMSPluginCreator() +{ +} -const char * NMSPluginCreator::getPluginName() const noexcept { return NMS_PLUGIN_NAME; } +const char * NMSPluginCreator::getPluginName() const noexcept +{ + return NMS_PLUGIN_NAME; +} -const char * NMSPluginCreator::getPluginVersion() const noexcept { return NMS_PLUGIN_VERSION; } +const char * NMSPluginCreator::getPluginVersion() const noexcept +{ + return NMS_PLUGIN_VERSION; +} -const char * NMSPluginCreator::getPluginNamespace() const noexcept { return NMS_PLUGIN_NAMESPACE; } +const char * NMSPluginCreator::getPluginNamespace() const noexcept +{ + return NMS_PLUGIN_NAMESPACE; +} -void NMSPluginCreator::setPluginNamespace(const char * N) noexcept { (void)N; } -const PluginFieldCollection * NMSPluginCreator::getFieldNames() noexcept { return nullptr; } +void NMSPluginCreator::setPluginNamespace(const char * N) noexcept +{ + (void)N; +} +const PluginFieldCollection * NMSPluginCreator::getFieldNames() noexcept +{ + return nullptr; +} IPluginV2DynamicExt * NMSPluginCreator::createPlugin( const char * name, const PluginFieldCollection * fc) noexcept { diff --git a/perception/tensorrt_yolo/lib/src/plugins/yolo_layer.cu b/perception/tensorrt_yolo/lib/src/plugins/yolo_layer.cu index 1e25bfb97dad9..0da2c342faaa9 100644 --- a/perception/tensorrt_yolo/lib/src/plugins/yolo_layer.cu +++ b/perception/tensorrt_yolo/lib/src/plugins/yolo_layer.cu @@ -68,7 +68,10 @@ namespace yolo { -inline __device__ float sigmoid(float x) { return 1.0f / (1.0f + __expf(-x)); } +inline __device__ float sigmoid(float x) +{ + return 1.0f / (1.0f + __expf(-x)); +} inline __device__ float scaleSigmoid(float x, float scale) { diff --git a/perception/tensorrt_yolo/lib/src/plugins/yolo_layer_plugin.cpp b/perception/tensorrt_yolo/lib/src/plugins/yolo_layer_plugin.cpp index 5e73b989089c7..78edc568bb7c4 100644 --- a/perception/tensorrt_yolo/lib/src/plugins/yolo_layer_plugin.cpp +++ b/perception/tensorrt_yolo/lib/src/plugins/yolo_layer_plugin.cpp @@ -135,18 +135,29 @@ YoloLayerPlugin::YoloLayerPlugin(const void * data, size_t length) } // IPluginV2 Methods -const char * YoloLayerPlugin::getPluginType() const noexcept { return YOLO_LAYER_PLUGIN_NAME; } +const char * YoloLayerPlugin::getPluginType() const noexcept +{ + return YOLO_LAYER_PLUGIN_NAME; +} const char * YoloLayerPlugin::getPluginVersion() const noexcept { return YOLO_LAYER_PLUGIN_VERSION; } -int YoloLayerPlugin::getNbOutputs() const noexcept { return 3; } +int YoloLayerPlugin::getNbOutputs() const noexcept +{ + return 3; +} -int YoloLayerPlugin::initialize() noexcept { return 0; } +int YoloLayerPlugin::initialize() noexcept +{ + return 0; +} -void YoloLayerPlugin::terminate() noexcept {} +void YoloLayerPlugin::terminate() noexcept +{ +} size_t YoloLayerPlugin::getSerializationSize() const noexcept { @@ -169,9 +180,15 @@ void YoloLayerPlugin::serialize(void * buffer) const noexcept write(d, use_darknet_layer_); } -void YoloLayerPlugin::destroy() noexcept { delete this; } +void YoloLayerPlugin::destroy() noexcept +{ + delete this; +} -void YoloLayerPlugin::setPluginNamespace(const char * N) noexcept { (void)N; } +void YoloLayerPlugin::setPluginNamespace(const char * N) noexcept +{ + (void)N; +} const char * YoloLayerPlugin::getPluginNamespace() const noexcept { @@ -192,7 +209,10 @@ DataType YoloLayerPlugin::getOutputDataType( // IPluginV2DynamicExt Methods -IPluginV2DynamicExt * YoloLayerPlugin::clone() const noexcept { return new YoloLayerPlugin(*this); } +IPluginV2DynamicExt * YoloLayerPlugin::clone() const noexcept +{ + return new YoloLayerPlugin(*this); +} DimsExprs YoloLayerPlugin::getOutputDimensions( int outputIndex, const DimsExprs * inputs, int nbInputs, IExprBuilder & exprBuilder) noexcept @@ -272,7 +292,9 @@ int YoloLayerPlugin::enqueue( return status; } -YoloLayerPluginCreator::YoloLayerPluginCreator() {} +YoloLayerPluginCreator::YoloLayerPluginCreator() +{ +} const char * YoloLayerPluginCreator::getPluginName() const noexcept { @@ -289,8 +311,14 @@ const char * YoloLayerPluginCreator::getPluginNamespace() const noexcept return YOLO_LAYER_PLUGIN_NAMESPACE; } -void YoloLayerPluginCreator::setPluginNamespace(const char * N) noexcept { (void)N; } -const PluginFieldCollection * YoloLayerPluginCreator::getFieldNames() noexcept { return nullptr; } +void YoloLayerPluginCreator::setPluginNamespace(const char * N) noexcept +{ + (void)N; +} +const PluginFieldCollection * YoloLayerPluginCreator::getFieldNames() noexcept +{ + return nullptr; +} IPluginV2DynamicExt * YoloLayerPluginCreator::createPlugin( const char * name, const PluginFieldCollection * fc) noexcept diff --git a/perception/tensorrt_yolo/lib/src/trt_yolo.cpp b/perception/tensorrt_yolo/lib/src/trt_yolo.cpp index 65fb298aff388..7f1d2dbbf4577 100644 --- a/perception/tensorrt_yolo/lib/src/trt_yolo.cpp +++ b/perception/tensorrt_yolo/lib/src/trt_yolo.cpp @@ -333,6 +333,9 @@ int Net::getInputSize() const return input_size; } -int Net::getMaxDetections() const { return engine_->getBindingDimensions(1).d[1]; } +int Net::getMaxDetections() const +{ + return engine_->getBindingDimensions(1).d[1]; +} } // namespace yolo diff --git a/perception/traffic_light_classifier/utils/trt_common.cpp b/perception/traffic_light_classifier/utils/trt_common.cpp index aa87a974fe542..f12be16879321 100644 --- a/perception/traffic_light_classifier/utils/trt_common.cpp +++ b/perception/traffic_light_classifier/utils/trt_common.cpp @@ -150,7 +150,10 @@ bool TrtCommon::buildEngineFromOnnx(std::string onnx_file_path, std::string outp return true; } -bool TrtCommon::isInitialized() { return is_initialized_; } +bool TrtCommon::isInitialized() +{ + return is_initialized_; +} int TrtCommon::getNumInput() { diff --git a/perception/traffic_light_ssd_fine_detector/lib/src/trt_ssd.cpp b/perception/traffic_light_ssd_fine_detector/lib/src/trt_ssd.cpp index d810eb5275058..9b7bf632e1019 100644 --- a/perception/traffic_light_ssd_fine_detector/lib/src/trt_ssd.cpp +++ b/perception/traffic_light_ssd_fine_detector/lib/src/trt_ssd.cpp @@ -193,6 +193,9 @@ int Net::getMaxBatchSize() return engine_->getProfileDimensions(0, 0, nvinfer1::OptProfileSelector::kMAX).d[0]; } -int Net::getMaxDetections() { return engine_->getBindingDimensions(1).d[1]; } +int Net::getMaxDetections() +{ + return engine_->getBindingDimensions(1).d[1]; +} } // namespace ssd diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_util/debug_utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_util/debug_utilities.hpp index fc6b0f7772ef2..d9203e63b9875 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_util/debug_utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_util/debug_utilities.hpp @@ -80,7 +80,10 @@ constexpr std::array, 10> colorsList() magenta, medium_orchid, light_pink, light_yellow, light_steel_blue}; } -inline int64_t bitShift(int64_t original_id) { return original_id << (sizeof(int32_t) * 8 / 2); } +inline int64_t bitShift(int64_t original_id) +{ + return original_id << (sizeof(int32_t) * 8 / 2); +} MarkerArray createPoseMarkerArray( const Pose & pose, std::string && ns, const int32_t & id, const float & r, const float & g, diff --git a/planning/behavior_path_planner/src/behavior_tree_manager.cpp b/planning/behavior_path_planner/src/behavior_tree_manager.cpp index 0f016c8e5836b..7ea07a1597973 100644 --- a/planning/behavior_path_planner/src/behavior_tree_manager.cpp +++ b/planning/behavior_path_planner/src/behavior_tree_manager.cpp @@ -130,7 +130,10 @@ void BehaviorTreeManager::resetNotRunningModulePathCandidate() } } -void BehaviorTreeManager::resetBehaviorTree() { bt_tree_.haltTree(); } +void BehaviorTreeManager::resetBehaviorTree() +{ + bt_tree_.haltTree(); +} void BehaviorTreeManager::addGrootMonitoring( BT::Tree * tree, uint16_t publisher_port, uint16_t server_port, uint16_t max_msg_per_second) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp index 78bd5daa0bcd2..90e1e7ad9dce9 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp @@ -399,7 +399,10 @@ std::pair ExternalRequestLaneChangeModule::getSafePath( return {found_valid_path, found_safe_path}; } -bool ExternalRequestLaneChangeModule::isSafe() const { return status_.is_safe; } +bool ExternalRequestLaneChangeModule::isSafe() const +{ + return status_.is_safe; +} bool ExternalRequestLaneChangeModule::isValidPath(const PathWithLaneId & path) const { diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index 190178d6d5bd8..f8d7876104e04 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -505,9 +505,15 @@ std::pair LaneChangeModule::getSafePath( return {found_valid_path, found_safe_path}; } -bool LaneChangeModule::isSafe() const { return status_.is_safe; } +bool LaneChangeModule::isSafe() const +{ + return status_.is_safe; +} -bool LaneChangeModule::isValidPath() const { return status_.is_valid_path; } +bool LaneChangeModule::isValidPath() const +{ + return status_.is_valid_path; +} bool LaneChangeModule::isValidPath(const PathWithLaneId & path) const { @@ -744,8 +750,14 @@ void LaneChangeModule::updateSteeringFactorPtr( {output.start_distance_to_path_change, output.finish_distance_to_path_change}, SteeringFactor::LANE_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING, ""); } -Pose LaneChangeModule::getEgoPose() const { return planner_data_->self_odometry->pose.pose; } -Twist LaneChangeModule::getEgoTwist() const { return planner_data_->self_odometry->twist.twist; } +Pose LaneChangeModule::getEgoPose() const +{ + return planner_data_->self_odometry->pose.pose; +} +Twist LaneChangeModule::getEgoTwist() const +{ + return planner_data_->self_odometry->twist.twist; +} std_msgs::msg::Header LaneChangeModule::getRouteHeader() const { return planner_data_->route_handler->getRouteHeader(); diff --git a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp index 56508b1884611..ef0fe5b70db74 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp @@ -37,9 +37,15 @@ void LaneFollowingModule::initParam() clearWaitingApproval(); // no need approval } -bool LaneFollowingModule::isExecutionRequested() const { return true; } +bool LaneFollowingModule::isExecutionRequested() const +{ + return true; +} -bool LaneFollowingModule::isExecutionReady() const { return true; } +bool LaneFollowingModule::isExecutionReady() const +{ + return true; +} BT::NodeStatus LaneFollowingModule::updateState() { diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index f1572500ba458..ea5b7426c6bb2 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -188,7 +188,10 @@ bool PullOutModule::isExecutionRequested() const return true; } -bool PullOutModule::isExecutionReady() const { return true; } +bool PullOutModule::isExecutionReady() const +{ + return true; +} // this runs only when RUNNING ModuleStatus PullOutModule::updateState() @@ -290,7 +293,10 @@ BehaviorModuleOutput PullOutModule::plan() return output; } -CandidateOutput PullOutModule::planCandidate() const { return CandidateOutput{}; } +CandidateOutput PullOutModule::planCandidate() const +{ + return CandidateOutput{}; +} std::shared_ptr PullOutModule::getCurrentPlanner() const { diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index b2f5dfae39425..1cb8e0ec8ce1f 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -379,7 +379,10 @@ bool PullOverModule::isExecutionRequested() const return true; } -bool PullOverModule::isExecutionReady() const { return true; } +bool PullOverModule::isExecutionReady() const +{ + return true; +} Pose PullOverModule::calcRefinedGoal(const Pose & goal_pose) const { @@ -727,7 +730,10 @@ BehaviorModuleOutput PullOverModule::plan() // This const function can not change the menber variables like the goal. // so implement generating candidate path in planWaitingApproval(). // No specific path for the candidate. It's same to the one generated by plan(). -CandidateOutput PullOverModule::planCandidate() const { return CandidateOutput{}; } +CandidateOutput PullOverModule::planCandidate() const +{ + return CandidateOutput{}; +} BehaviorModuleOutput PullOverModule::planWaitingApproval() { diff --git a/planning/behavior_path_planner/src/steering_factor_interface.cpp b/planning/behavior_path_planner/src/steering_factor_interface.cpp index 31c55af9b1603..9ba79f1266fdc 100644 --- a/planning/behavior_path_planner/src/steering_factor_interface.cpp +++ b/planning/behavior_path_planner/src/steering_factor_interface.cpp @@ -54,6 +54,9 @@ void SteeringFactorInterface::clearSteeringFactors() registered_steering_factors_.factors.clear(); } -rclcpp::Logger SteeringFactorInterface::getLogger() const { return logger_; } +rclcpp::Logger SteeringFactorInterface::getLogger() const +{ + return logger_; +} } // namespace steering_factor_interface diff --git a/planning/behavior_path_planner/src/util/avoidance/util.cpp b/planning/behavior_path_planner/src/util/avoidance/util.cpp index 5e7de8f2e2e6a..a754c8df2a407 100644 --- a/planning/behavior_path_planner/src/util/avoidance/util.cpp +++ b/planning/behavior_path_planner/src/util/avoidance/util.cpp @@ -365,7 +365,10 @@ std::vector updateBoundary( } } // namespace -bool isOnRight(const ObjectData & obj) { return obj.lateral < 0.0; } +bool isOnRight(const ObjectData & obj) +{ + return obj.lateral < 0.0; +} double calcShiftLength( const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin) diff --git a/planning/behavior_path_planner/src/util/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/util/drivable_area_expansion/drivable_area_expansion.cpp index 50a8336a7b631..acaf845da7feb 100644 --- a/planning/behavior_path_planner/src/util/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/util/drivable_area_expansion/drivable_area_expansion.cpp @@ -43,9 +43,15 @@ void expandDrivableArea( updateDrivableAreaBounds(path, expanded_drivable_area); } -point_t convert_point(const Point & p) { return point_t{p.x, p.y}; } +point_t convert_point(const Point & p) +{ + return point_t{p.x, p.y}; +} -Point convert_point(const point_t & p) { return Point().set__x(p.x()).set__y(p.y()); } +Point convert_point(const point_t & p) +{ + return Point().set__x(p.x()).set__y(p.y()); +} polygon_t createExpandedDrivableAreaPolygon( const PathWithLaneId & path, const multipolygon_t & expansion_polygons) diff --git a/planning/behavior_path_planner/src/util/geometric_parallel_parking/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/util/geometric_parallel_parking/geometric_parallel_parking.cpp index b14a748ddc91b..8cf2e129ae15e 100644 --- a/planning/behavior_path_planner/src/util/geometric_parallel_parking/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/util/geometric_parallel_parking/geometric_parallel_parking.cpp @@ -95,7 +95,10 @@ PathWithLaneId GeometricParallelParking::getArcPath() const return path; } -bool GeometricParallelParking::isParking() const { return current_path_idx_ > 0; } +bool GeometricParallelParking::isParking() const +{ + return current_path_idx_ > 0; +} void GeometricParallelParking::setVelocityToArcPaths( std::vector & arc_paths, const double velocity, const bool set_stop_end) diff --git a/planning/behavior_path_planner/src/util/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/src/util/path_shifter/path_shifter.cpp index 4e0ec714031bd..7be5b4dbad467 100644 --- a/planning/behavior_path_planner/src/util/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner/src/util/path_shifter/path_shifter.cpp @@ -63,9 +63,15 @@ void PathShifter::setPath(const PathWithLaneId & path) updateShiftLinesIndices(shift_lines_); sortShiftLinesAlongPath(shift_lines_); } -void PathShifter::setVelocity(const double velocity) { velocity_ = velocity; } +void PathShifter::setVelocity(const double velocity) +{ + velocity_ = velocity; +} -void PathShifter::setLateralAccelerationLimit(const double acc) { acc_limit_ = acc; } +void PathShifter::setLateralAccelerationLimit(const double acc) +{ + acc_limit_ = acc; +} void PathShifter::addShiftLine(const ShiftLine & line) { diff --git a/planning/behavior_path_planner/src/util/side_shift/util.cpp b/planning/behavior_path_planner/src/util/side_shift/util.cpp index d2c9eab215bf7..cfe99ba3b15f9 100644 --- a/planning/behavior_path_planner/src/util/side_shift/util.cpp +++ b/planning/behavior_path_planner/src/util/side_shift/util.cpp @@ -81,7 +81,10 @@ bool getStartAvoidPose( return false; } -bool isAlmostZero(double v) { return std::fabs(v) < 1.0e-4; } +bool isAlmostZero(double v) +{ + return std::fabs(v) < 1.0e-4; +} Point transformToGrid( const Point & pt, const double longitudinal_offset, const double lateral_offset, const double yaw, diff --git a/planning/behavior_velocity_planner/include/utilization/util.hpp b/planning/behavior_velocity_planner/include/utilization/util.hpp index 18dab98533616..b7c8d118c9b97 100644 --- a/planning/behavior_velocity_planner/include/utilization/util.hpp +++ b/planning/behavior_velocity_planner/include/utilization/util.hpp @@ -155,7 +155,10 @@ void setVelocityFromIndex(const size_t begin_idx, const double vel, PathWithLane void insertVelocity( PathWithLaneId & path, const PathPointWithLaneId & path_point, const double v, size_t & insert_index, const double min_distance = 0.001); -inline int64_t bitShift(int64_t original_id) { return original_id << (sizeof(int32_t) * 8 / 2); } +inline int64_t bitShift(int64_t original_id) +{ + return original_id << (sizeof(int32_t) * 8 / 2); +} geometry_msgs::msg::Pose transformRelCoordinate2D( const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin); diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/debug.cpp index c60c45808001a..c679ec0d6ae09 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/run_out/debug.cpp @@ -284,7 +284,10 @@ visualization_msgs::msg::MarkerArray RunOutDebug::createVisualizationMarkerArray return msg; } -void RunOutDebug::setAccelReason(const AccelReason & accel_reason) { accel_reason_ = accel_reason; } +void RunOutDebug::setAccelReason(const AccelReason & accel_reason) +{ + accel_reason_ = accel_reason; +} void RunOutDebug::publishDebugValue() { @@ -333,7 +336,10 @@ void RunOutDebug::publishEmptyPointCloud() pub_debug_pointcloud_->publish(pc); } -void RunOutDebug::setHeight(const double height) { height_ = height; } +void RunOutDebug::setHeight(const double height) +{ + height_ = height; +} // scene module visualization_msgs::msg::MarkerArray RunOutModule::createDebugMarkerArray() diff --git a/planning/behavior_velocity_planner/test/src/test_state_machine.cpp b/planning/behavior_velocity_planner/test/src/test_state_machine.cpp index 467f8cb41a338..34a6b5b0767b4 100644 --- a/planning/behavior_velocity_planner/test/src/test_state_machine.cpp +++ b/planning/behavior_velocity_planner/test/src/test_state_machine.cpp @@ -26,7 +26,10 @@ using StateMachine = behavior_velocity_planner::StateMachine; using State = behavior_velocity_planner::StateMachine::State; -int enumToInt(State s) { return static_cast(s); } +int enumToInt(State s) +{ + return static_cast(s); +} TEST(state_machine, on_initialized) { diff --git a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp index 6919b4eac142f..75215afa75cd3 100644 --- a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp +++ b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp @@ -326,7 +326,10 @@ void FreespacePlannerNode::onOccupancyGrid(const OccupancyGrid::ConstSharedPtr m occupancy_grid_ = msg; } -void FreespacePlannerNode::onScenario(const Scenario::ConstSharedPtr msg) { scenario_ = msg; } +void FreespacePlannerNode::onScenario(const Scenario::ConstSharedPtr msg) +{ + scenario_ = msg; +} void FreespacePlannerNode::onOdometry(const Odometry::ConstSharedPtr msg) { diff --git a/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp b/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp index 40c07b0fa5b14..c11c3df73d467 100644 --- a/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp +++ b/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp @@ -364,9 +364,15 @@ TEST(AstarSearchTestSuite, MultiCurvature) EXPECT_TRUE(test_algorithm(AlgorithmType::ASTAR_MULTI)); } -TEST(RRTStarTestSuite, Fastest) { EXPECT_TRUE(test_algorithm(AlgorithmType::RRTSTAR_FASTEST)); } +TEST(RRTStarTestSuite, Fastest) +{ + EXPECT_TRUE(test_algorithm(AlgorithmType::RRTSTAR_FASTEST)); +} -TEST(RRTStarTestSuite, Update) { EXPECT_TRUE(test_algorithm(AlgorithmType::RRTSTAR_UPDATE)); } +TEST(RRTStarTestSuite, Update) +{ + EXPECT_TRUE(test_algorithm(AlgorithmType::RRTSTAR_UPDATE)); +} TEST(RRTStarTestSuite, InformedUpdate) { diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index c01d66be2542d..dbd561ba2c676 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -163,7 +163,10 @@ void DefaultPlanner::initialize(rclcpp::Node * node, const HADMapBin::ConstShare map_callback(msg); } -bool DefaultPlanner::ready() const { return is_graph_ready_; } +bool DefaultPlanner::ready() const +{ + return is_graph_ready_; +} void DefaultPlanner::map_callback(const HADMapBin::ConstSharedPtr msg) { diff --git a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp index b65147ccfabb9..3d72d6e4b5c83 100644 --- a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp +++ b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp @@ -343,9 +343,15 @@ double integ_x(double x0, double v0, double a0, double j0, double t) return x0 + v0 * t + 0.5 * a0 * t * t + (1.0 / 6.0) * j0 * t * t * t; } -double integ_v(double v0, double a0, double j0, double t) { return v0 + a0 * t + 0.5 * j0 * t * t; } +double integ_v(double v0, double a0, double j0, double t) +{ + return v0 + a0 * t + 0.5 * j0 * t * t; +} -double integ_a(double a0, double j0, double t) { return a0 + j0 * t; } +double integ_a(double a0, double j0, double t) +{ + return a0 + j0 * t; +} } // namespace analytical_velocity_planning_utils } // namespace motion_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp index 04dc06cb0bb18..8a364f876b329 100644 --- a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp @@ -49,7 +49,10 @@ void JerkFilteredSmoother::setParam(const Param & smoother_param) smoother_param_ = smoother_param; } -JerkFilteredSmoother::Param JerkFilteredSmoother::getParam() const { return smoother_param_; } +JerkFilteredSmoother::Param JerkFilteredSmoother::getParam() const +{ + return smoother_param_; +} bool JerkFilteredSmoother::apply( const double v0, const double a0, const TrajectoryPoints & input, TrajectoryPoints & output, diff --git a/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp index fbce49c1c9af0..f80fc0231e9cd 100644 --- a/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp @@ -43,7 +43,10 @@ void L2PseudoJerkSmoother::setParam(const Param & smoother_param) smoother_param_ = smoother_param; } -L2PseudoJerkSmoother::Param L2PseudoJerkSmoother::getParam() const { return smoother_param_; } +L2PseudoJerkSmoother::Param L2PseudoJerkSmoother::getParam() const +{ + return smoother_param_; +} bool L2PseudoJerkSmoother::apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, diff --git a/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp index e76c1cb4ffe5c..7c3fe6e0ceed2 100644 --- a/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp @@ -43,7 +43,10 @@ void LinfPseudoJerkSmoother::setParam(const Param & smoother_param) smoother_param_ = smoother_param; } -LinfPseudoJerkSmoother::Param LinfPseudoJerkSmoother::getParam() const { return smoother_param_; } +LinfPseudoJerkSmoother::Param LinfPseudoJerkSmoother::getParam() const +{ + return smoother_param_; +} bool LinfPseudoJerkSmoother::apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, diff --git a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp index 01f6090e157aa..fdfa0f2514b58 100644 --- a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp @@ -58,17 +58,35 @@ SmootherBase::SmootherBase(rclcpp::Node & node) node.declare_parameter("sparse_min_interval_distance"); } -void SmootherBase::setParam(const BaseParam & param) { base_param_ = param; } +void SmootherBase::setParam(const BaseParam & param) +{ + base_param_ = param; +} -SmootherBase::BaseParam SmootherBase::getBaseParam() const { return base_param_; } +SmootherBase::BaseParam SmootherBase::getBaseParam() const +{ + return base_param_; +} -double SmootherBase::getMaxAccel() const { return base_param_.max_accel; } +double SmootherBase::getMaxAccel() const +{ + return base_param_.max_accel; +} -double SmootherBase::getMinDecel() const { return base_param_.min_decel; } +double SmootherBase::getMinDecel() const +{ + return base_param_.min_decel; +} -double SmootherBase::getMaxJerk() const { return base_param_.max_jerk; } +double SmootherBase::getMaxJerk() const +{ + return base_param_.max_jerk; +} -double SmootherBase::getMinJerk() const { return base_param_.min_jerk; } +double SmootherBase::getMinJerk() const +{ + return base_param_.min_jerk; +} TrajectoryPoints SmootherBase::applyLateralAccelerationFilter( const TrajectoryPoints & input, [[maybe_unused]] const double v0, diff --git a/planning/motion_velocity_smoother/src/trajectory_utils.cpp b/planning/motion_velocity_smoother/src/trajectory_utils.cpp index 629ba1b15040a..4c96f56835990 100644 --- a/planning/motion_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/motion_velocity_smoother/src/trajectory_utils.cpp @@ -55,7 +55,10 @@ inline double integ_v(double v0, double a0, double j0, double t) return v0 + a0 * t + 0.5 * j0 * t * t; } -inline double integ_a(double a0, double j0, double t) { return a0 + j0 * t; } +inline double integ_a(double a0, double j0, double t) +{ + return a0 + j0 * t; +} TrajectoryPoint calcInterpolatedTrajectoryPoint( const TrajectoryPoints & trajectory, const Pose & target_pose, const size_t seg_idx) diff --git a/planning/obstacle_avoidance_planner/src/eb_path_smoother.cpp b/planning/obstacle_avoidance_planner/src/eb_path_smoother.cpp index 8e31a6355af42..a988f78c9074e 100644 --- a/planning/obstacle_avoidance_planner/src/eb_path_smoother.cpp +++ b/planning/obstacle_avoidance_planner/src/eb_path_smoother.cpp @@ -168,7 +168,10 @@ void EBPathSmoother::initialize(const bool enable_debug_info, const TrajectoryPa traj_param_ = traj_param; } -void EBPathSmoother::resetPreviousData() { prev_eb_traj_points_ptr_ = nullptr; } +void EBPathSmoother::resetPreviousData() +{ + prev_eb_traj_points_ptr_ = nullptr; +} std::optional> EBPathSmoother::getEBTrajectory(const PlannerData & planner_data) diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 7eb43152d1636..565ef893facaf 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -467,7 +467,10 @@ void MPTOptimizer::initialize(const bool enable_debug_info, const TrajectoryPara traj_param_ = traj_param; } -void MPTOptimizer::resetPreviousData() { prev_ref_points_ptr_ = nullptr; } +void MPTOptimizer::resetPreviousData() +{ + prev_ref_points_ptr_ = nullptr; +} void MPTOptimizer::onParam(const std::vector & parameters) { @@ -1528,7 +1531,10 @@ double MPTOptimizer::getTrajectoryLength() const return forward_traj_length + backward_traj_length; } -int MPTOptimizer::getNumberOfPoints() const { return mpt_param_.num_points; } +int MPTOptimizer::getNumberOfPoints() const +{ + return mpt_param_.num_points; +} size_t MPTOptimizer::getNumberOfSlackVariables() const { diff --git a/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_interface.cpp b/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_interface.cpp index 35da4a2eab467..07a2ab00e7546 100644 --- a/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_interface.cpp +++ b/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_interface.cpp @@ -21,6 +21,15 @@ VehicleModelInterface::VehicleModelInterface( { } -int VehicleModelInterface::getDimX() const { return dim_x_; } -int VehicleModelInterface::getDimU() const { return dim_u_; } -int VehicleModelInterface::getDimY() const { return dim_y_; } +int VehicleModelInterface::getDimX() const +{ + return dim_x_; +} +int VehicleModelInterface::getDimU() const +{ + return dim_u_; +} +int VehicleModelInterface::getDimY() const +{ + return dim_y_; +} diff --git a/planning/planning_debug_tools/include/planning_debug_tools/util.hpp b/planning/planning_debug_tools/include/planning_debug_tools/util.hpp index 96488fbde4b96..1e332e7127dc4 100644 --- a/planning/planning_debug_tools/include/planning_debug_tools/util.hpp +++ b/planning/planning_debug_tools/include/planning_debug_tools/util.hpp @@ -36,13 +36,31 @@ using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::getPoint; using tier4_autoware_utils::getRPY; -double getVelocity(const PathPoint & p) { return p.longitudinal_velocity_mps; } -double getVelocity(const PathPointWithLaneId & p) { return p.point.longitudinal_velocity_mps; } -double getVelocity(const TrajectoryPoint & p) { return p.longitudinal_velocity_mps; } +double getVelocity(const PathPoint & p) +{ + return p.longitudinal_velocity_mps; +} +double getVelocity(const PathPointWithLaneId & p) +{ + return p.point.longitudinal_velocity_mps; +} +double getVelocity(const TrajectoryPoint & p) +{ + return p.longitudinal_velocity_mps; +} -double getYaw(const PathPoint & p) { return getRPY(p.pose.orientation).z; } -double getYaw(const PathPointWithLaneId & p) { return getRPY(p.point.pose.orientation).z; } -double getYaw(const TrajectoryPoint & p) { return getRPY(p.pose.orientation).z; } +double getYaw(const PathPoint & p) +{ + return getRPY(p.pose.orientation).z; +} +double getYaw(const PathPointWithLaneId & p) +{ + return getRPY(p.point.pose.orientation).z; +} +double getYaw(const TrajectoryPoint & p) +{ + return getRPY(p.pose.orientation).z; +} template inline std::vector getYawArray(const T & points) diff --git a/planning/planning_test_utils/src/planning_interface_test_manager.cpp b/planning/planning_test_utils/src/planning_interface_test_manager.cpp index 35fe57d70698c..fc52f9beec603 100644 --- a/planning/planning_test_utils/src/planning_interface_test_manager.cpp +++ b/planning/planning_test_utils/src/planning_interface_test_manager.cpp @@ -120,6 +120,9 @@ void PlanningIntefaceTestManager::publishAbnormalTrajectory( test_utils::spinSomeNodes(test_node_, target_node); } -int PlanningIntefaceTestManager::getReceivedTopicNum() { return count_; } +int PlanningIntefaceTestManager::getReceivedTopicNum() +{ + return count_; +} } // namespace planning_test_utils diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index fc9cb719f9af5..cbd556d3ad7c3 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -123,7 +123,10 @@ std::string toString(const geometry_msgs::msg::Pose & pose) namespace route_handler { -RouteHandler::RouteHandler(const HADMapBin & map_msg) { setMap(map_msg); } +RouteHandler::RouteHandler(const HADMapBin & map_msg) +{ + setMap(map_msg); +} void RouteHandler::setMap(const HADMapBin & map_msg) { @@ -180,7 +183,10 @@ void RouteHandler::setRoute(const LaneletRoute & route_msg) } } -bool RouteHandler::isHandlerReady() const { return is_handler_ready_; } +bool RouteHandler::isHandlerReady() const +{ + return is_handler_ready_; +} void RouteHandler::setRouteLanelets(const lanelet::ConstLanelets & path_lanelets) { @@ -334,9 +340,15 @@ lanelet::ConstPolygon3d RouteHandler::getIntersectionAreaById(const lanelet::Id return lanelet_map_ptr_->polygonLayer.get(id); } -Header RouteHandler::getRouteHeader() const { return route_msg_.header; } +Header RouteHandler::getRouteHeader() const +{ + return route_msg_.header; +} -UUID RouteHandler::getRouteUuid() const { return route_msg_.uuid; } +UUID RouteHandler::getRouteUuid() const +{ + return route_msg_.uuid; +} std::vector RouteHandler::getLanesBeforePose( const geometry_msgs::msg::Pose & pose, const double length) const @@ -374,9 +386,15 @@ std::vector RouteHandler::getLanesAfterGoal( return succeeding_lanes_vec.front(); } -lanelet::ConstLanelets RouteHandler::getRouteLanelets() const { return route_lanelets_; } +lanelet::ConstLanelets RouteHandler::getRouteLanelets() const +{ + return route_lanelets_; +} -Pose RouteHandler::getGoalPose() const { return route_msg_.goal_pose; } +Pose RouteHandler::getGoalPose() const +{ + return route_msg_.goal_pose; +} lanelet::Id RouteHandler::getGoalLaneId() const { @@ -1498,7 +1516,10 @@ lanelet::ConstLanelets RouteHandler::getCheckTargetLanesFromPath( return check_lanelets; } -bool RouteHandler::isMapMsgReady() const { return is_map_msg_ready_; } +bool RouteHandler::isMapMsgReady() const +{ + return is_map_msg_ready_; +} lanelet::routing::RoutingGraphPtr RouteHandler::getRoutingGraphPtr() const { @@ -1516,7 +1537,10 @@ std::shared_ptr RouteHandler::get return overall_graphs_ptr_; } -lanelet::LaneletMapPtr RouteHandler::getLaneletMapPtr() const { return lanelet_map_ptr_; } +lanelet::LaneletMapPtr RouteHandler::getLaneletMapPtr() const +{ + return lanelet_map_ptr_; +} lanelet::routing::RelationType RouteHandler::getRelation( const lanelet::ConstLanelet & prev_lane, const lanelet::ConstLanelet & next_lane) const @@ -1553,7 +1577,10 @@ lanelet::routing::RelationType RouteHandler::getRelation( return lanelet::routing::RelationType::None; } -lanelet::ConstLanelets RouteHandler::getShoulderLanelets() const { return shoulder_lanelets_; } +lanelet::ConstLanelets RouteHandler::getShoulderLanelets() const +{ + return shoulder_lanelets_; +} lanelet::ConstLanelets RouteHandler::getPreviousLaneletSequence( const lanelet::ConstLanelets & lanelet_sequence) const diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/planning/rtc_interface/src/rtc_interface.cpp index 11230b3013ceb..de8af479bd2f0 100644 --- a/planning/rtc_interface/src/rtc_interface.cpp +++ b/planning/rtc_interface/src/rtc_interface.cpp @@ -274,7 +274,10 @@ bool RTCInterface::isRegistered(const UUID & uuid) return itr != registered_status_.statuses.end(); } -void RTCInterface::lockCommandUpdate() { is_locked_ = true; } +void RTCInterface::lockCommandUpdate() +{ + is_locked_ = true; +} void RTCInterface::unlockCommandUpdate() { @@ -282,8 +285,14 @@ void RTCInterface::unlockCommandUpdate() updateCooperateCommandStatus(stored_commands_); } -rclcpp::Logger RTCInterface::getLogger() const { return logger_; } +rclcpp::Logger RTCInterface::getLogger() const +{ + return logger_; +} -bool RTCInterface::isLocked() const { return is_locked_; } +bool RTCInterface::isLocked() const +{ + return is_locked_; +} } // namespace rtc_interface diff --git a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp index 5199641a9f23f..f2a7b1885198c 100644 --- a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp +++ b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp @@ -89,9 +89,15 @@ lanelet::ConstLanelets get_lanelets_from_ids( return lanelets; } -rclcpp::NodeOptions create_node_options() { return rclcpp::NodeOptions{}; } +rclcpp::NodeOptions create_node_options() +{ + return rclcpp::NodeOptions{}; +} -rclcpp::QoS create_transient_local_qos() { return rclcpp::QoS{1}.transient_local(); } +rclcpp::QoS create_transient_local_qos() +{ + return rclcpp::QoS{1}.transient_local(); +} lanelet::BasicPoint2d convertToLaneletPoint(const geometry_msgs::msg::Point & geom_point) { diff --git a/sensing/geo_pos_conv/src/geo_pos_conv.cpp b/sensing/geo_pos_conv/src/geo_pos_conv.cpp index a046b73f65a16..5fc489d46980f 100644 --- a/sensing/geo_pos_conv/src/geo_pos_conv.cpp +++ b/sensing/geo_pos_conv/src/geo_pos_conv.cpp @@ -34,11 +34,20 @@ geo_pos_conv::geo_pos_conv() { } -double geo_pos_conv::x() const { return m_x; } +double geo_pos_conv::x() const +{ + return m_x; +} -double geo_pos_conv::y() const { return m_y; } +double geo_pos_conv::y() const +{ + return m_y; +} -double geo_pos_conv::z() const { return m_z; } +double geo_pos_conv::z() const +{ + return m_z; +} void geo_pos_conv::set_plane(double lat, double lon) { diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp index a86a896b2dd0e..870444acb5215 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp @@ -35,17 +35,38 @@ SimModelDelaySteerAcc::SimModelDelaySteerAcc( initializeInputQueue(dt); } -double SimModelDelaySteerAcc::getX() { return state_(IDX::X); } -double SimModelDelaySteerAcc::getY() { return state_(IDX::Y); } -double SimModelDelaySteerAcc::getYaw() { return state_(IDX::YAW); } -double SimModelDelaySteerAcc::getVx() { return state_(IDX::VX); } -double SimModelDelaySteerAcc::getVy() { return 0.0; } -double SimModelDelaySteerAcc::getAx() { return state_(IDX::ACCX); } +double SimModelDelaySteerAcc::getX() +{ + return state_(IDX::X); +} +double SimModelDelaySteerAcc::getY() +{ + return state_(IDX::Y); +} +double SimModelDelaySteerAcc::getYaw() +{ + return state_(IDX::YAW); +} +double SimModelDelaySteerAcc::getVx() +{ + return state_(IDX::VX); +} +double SimModelDelaySteerAcc::getVy() +{ + return 0.0; +} +double SimModelDelaySteerAcc::getAx() +{ + return state_(IDX::ACCX); +} double SimModelDelaySteerAcc::getWz() { return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; } -double SimModelDelaySteerAcc::getSteer() { return state_(IDX::STEER); } +double SimModelDelaySteerAcc::getSteer() +{ + return state_(IDX::STEER); +} void SimModelDelaySteerAcc::update(const double & dt) { Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp index e15de4152290d..5170ed914e0f1 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp @@ -37,17 +37,38 @@ SimModelDelaySteerAccGeared::SimModelDelaySteerAccGeared( initializeInputQueue(dt); } -double SimModelDelaySteerAccGeared::getX() { return state_(IDX::X); } -double SimModelDelaySteerAccGeared::getY() { return state_(IDX::Y); } -double SimModelDelaySteerAccGeared::getYaw() { return state_(IDX::YAW); } -double SimModelDelaySteerAccGeared::getVx() { return state_(IDX::VX); } -double SimModelDelaySteerAccGeared::getVy() { return 0.0; } -double SimModelDelaySteerAccGeared::getAx() { return state_(IDX::ACCX); } +double SimModelDelaySteerAccGeared::getX() +{ + return state_(IDX::X); +} +double SimModelDelaySteerAccGeared::getY() +{ + return state_(IDX::Y); +} +double SimModelDelaySteerAccGeared::getYaw() +{ + return state_(IDX::YAW); +} +double SimModelDelaySteerAccGeared::getVx() +{ + return state_(IDX::VX); +} +double SimModelDelaySteerAccGeared::getVy() +{ + return 0.0; +} +double SimModelDelaySteerAccGeared::getAx() +{ + return state_(IDX::ACCX); +} double SimModelDelaySteerAccGeared::getWz() { return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; } -double SimModelDelaySteerAccGeared::getSteer() { return state_(IDX::STEER); } +double SimModelDelaySteerAccGeared::getSteer() +{ + return state_(IDX::STEER); +} void SimModelDelaySteerAccGeared::update(const double & dt) { Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp index 82973a2e8a004..1ac71fbb58c23 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp @@ -35,17 +35,38 @@ SimModelDelaySteerVel::SimModelDelaySteerVel( initializeInputQueue(dt); } -double SimModelDelaySteerVel::getX() { return state_(IDX::X); } -double SimModelDelaySteerVel::getY() { return state_(IDX::Y); } -double SimModelDelaySteerVel::getYaw() { return state_(IDX::YAW); } -double SimModelDelaySteerVel::getVx() { return state_(IDX::VX); } -double SimModelDelaySteerVel::getVy() { return 0.0; } -double SimModelDelaySteerVel::getAx() { return current_ax_; } +double SimModelDelaySteerVel::getX() +{ + return state_(IDX::X); +} +double SimModelDelaySteerVel::getY() +{ + return state_(IDX::Y); +} +double SimModelDelaySteerVel::getYaw() +{ + return state_(IDX::YAW); +} +double SimModelDelaySteerVel::getVx() +{ + return state_(IDX::VX); +} +double SimModelDelaySteerVel::getVy() +{ + return 0.0; +} +double SimModelDelaySteerVel::getAx() +{ + return current_ax_; +} double SimModelDelaySteerVel::getWz() { return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; } -double SimModelDelaySteerVel::getSteer() { return state_(IDX::STEER); } +double SimModelDelaySteerVel::getSteer() +{ + return state_(IDX::STEER); +} void SimModelDelaySteerVel::update(const double & dt) { Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp index 08dc3f2e34116..2edf43f0a743c 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp @@ -19,18 +19,42 @@ SimModelIdealSteerAcc::SimModelIdealSteerAcc(double wheelbase) { } -double SimModelIdealSteerAcc::getX() { return state_(IDX::X); } -double SimModelIdealSteerAcc::getY() { return state_(IDX::Y); } -double SimModelIdealSteerAcc::getYaw() { return state_(IDX::YAW); } -double SimModelIdealSteerAcc::getVx() { return state_(IDX::VX); } -double SimModelIdealSteerAcc::getVy() { return 0.0; } -double SimModelIdealSteerAcc::getAx() { return input_(IDX_U::AX_DES); } +double SimModelIdealSteerAcc::getX() +{ + return state_(IDX::X); +} +double SimModelIdealSteerAcc::getY() +{ + return state_(IDX::Y); +} +double SimModelIdealSteerAcc::getYaw() +{ + return state_(IDX::YAW); +} +double SimModelIdealSteerAcc::getVx() +{ + return state_(IDX::VX); +} +double SimModelIdealSteerAcc::getVy() +{ + return 0.0; +} +double SimModelIdealSteerAcc::getAx() +{ + return input_(IDX_U::AX_DES); +} double SimModelIdealSteerAcc::getWz() { return state_(IDX::VX) * std::tan(input_(IDX_U::STEER_DES)) / wheelbase_; } -double SimModelIdealSteerAcc::getSteer() { return input_(IDX_U::STEER_DES); } -void SimModelIdealSteerAcc::update(const double & dt) { updateRungeKutta(dt, input_); } +double SimModelIdealSteerAcc::getSteer() +{ + return input_(IDX_U::STEER_DES); +} +void SimModelIdealSteerAcc::update(const double & dt) +{ + updateRungeKutta(dt, input_); +} Eigen::VectorXd SimModelIdealSteerAcc::calcModel( const Eigen::VectorXd & state, const Eigen::VectorXd & input) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp index 8abad2df84936..b2bfe56209938 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp @@ -23,17 +23,38 @@ SimModelIdealSteerAccGeared::SimModelIdealSteerAccGeared(double wheelbase) { } -double SimModelIdealSteerAccGeared::getX() { return state_(IDX::X); } -double SimModelIdealSteerAccGeared::getY() { return state_(IDX::Y); } -double SimModelIdealSteerAccGeared::getYaw() { return state_(IDX::YAW); } -double SimModelIdealSteerAccGeared::getVx() { return state_(IDX::VX); } -double SimModelIdealSteerAccGeared::getVy() { return 0.0; } -double SimModelIdealSteerAccGeared::getAx() { return current_acc_; } +double SimModelIdealSteerAccGeared::getX() +{ + return state_(IDX::X); +} +double SimModelIdealSteerAccGeared::getY() +{ + return state_(IDX::Y); +} +double SimModelIdealSteerAccGeared::getYaw() +{ + return state_(IDX::YAW); +} +double SimModelIdealSteerAccGeared::getVx() +{ + return state_(IDX::VX); +} +double SimModelIdealSteerAccGeared::getVy() +{ + return 0.0; +} +double SimModelIdealSteerAccGeared::getAx() +{ + return current_acc_; +} double SimModelIdealSteerAccGeared::getWz() { return state_(IDX::VX) * std::tan(input_(IDX_U::STEER_DES)) / wheelbase_; } -double SimModelIdealSteerAccGeared::getSteer() { return input_(IDX_U::STEER_DES); } +double SimModelIdealSteerAccGeared::getSteer() +{ + return input_(IDX_U::STEER_DES); +} void SimModelIdealSteerAccGeared::update(const double & dt) { const auto prev_state = state_; diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp index e126f442c1e91..e43ac4aa8eae5 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp @@ -19,17 +19,38 @@ SimModelIdealSteerVel::SimModelIdealSteerVel(double wheelbase) { } -double SimModelIdealSteerVel::getX() { return state_(IDX::X); } -double SimModelIdealSteerVel::getY() { return state_(IDX::Y); } -double SimModelIdealSteerVel::getYaw() { return state_(IDX::YAW); } -double SimModelIdealSteerVel::getVx() { return input_(IDX_U::VX_DES); } -double SimModelIdealSteerVel::getVy() { return 0.0; } -double SimModelIdealSteerVel::getAx() { return current_ax_; } +double SimModelIdealSteerVel::getX() +{ + return state_(IDX::X); +} +double SimModelIdealSteerVel::getY() +{ + return state_(IDX::Y); +} +double SimModelIdealSteerVel::getYaw() +{ + return state_(IDX::YAW); +} +double SimModelIdealSteerVel::getVx() +{ + return input_(IDX_U::VX_DES); +} +double SimModelIdealSteerVel::getVy() +{ + return 0.0; +} +double SimModelIdealSteerVel::getAx() +{ + return current_ax_; +} double SimModelIdealSteerVel::getWz() { return input_(IDX_U::VX_DES) * std::tan(input_(IDX_U::STEER_DES)) / wheelbase_; } -double SimModelIdealSteerVel::getSteer() { return input_(IDX_U::STEER_DES); } +double SimModelIdealSteerVel::getSteer() +{ + return input_(IDX_U::STEER_DES); +} void SimModelIdealSteerVel::update(const double & dt) { updateRungeKutta(dt, input_); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp index 0740540801f68..8d17f72e0c3b3 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp @@ -33,9 +33,27 @@ void SimModelInterface::updateEuler(const double & dt, const Eigen::VectorXd & i { state_ += calcModel(state_, input) * dt; } -void SimModelInterface::getState(Eigen::VectorXd & state) { state = state_; } -void SimModelInterface::getInput(Eigen::VectorXd & input) { input = input_; } -void SimModelInterface::setState(const Eigen::VectorXd & state) { state_ = state; } -void SimModelInterface::setInput(const Eigen::VectorXd & input) { input_ = input; } -void SimModelInterface::setGear(const uint8_t gear) { gear_ = gear; } -uint8_t SimModelInterface::getGear() const { return gear_; } +void SimModelInterface::getState(Eigen::VectorXd & state) +{ + state = state_; +} +void SimModelInterface::getInput(Eigen::VectorXd & input) +{ + input = input_; +} +void SimModelInterface::setState(const Eigen::VectorXd & state) +{ + state_ = state; +} +void SimModelInterface::setInput(const Eigen::VectorXd & input) +{ + input_ = input; +} +void SimModelInterface::setGear(const uint8_t gear) +{ + gear_ = gear; +} +uint8_t SimModelInterface::getGear() const +{ + return gear_; +} diff --git a/system/bluetooth_monitor/service/l2ping.cpp b/system/bluetooth_monitor/service/l2ping.cpp index 9a297c5bb56de..6499f3a2b9612 100644 --- a/system/bluetooth_monitor/service/l2ping.cpp +++ b/system/bluetooth_monitor/service/l2ping.cpp @@ -180,6 +180,12 @@ void L2ping::setStatusCode(StatusCode code) status_.error_message = {}; } -L2pingStatus L2ping::getStatus() const { return status_; } +L2pingStatus L2ping::getStatus() const +{ + return status_; +} -const std::string & L2ping::getAddress() const { return status_.address; } +const std::string & L2ping::getAddress() const +{ + return status_.address; +} diff --git a/system/bluetooth_monitor/service/l2ping_service.cpp b/system/bluetooth_monitor/service/l2ping_service.cpp index 76f8c62874a0f..e811ffb030cd6 100644 --- a/system/bluetooth_monitor/service/l2ping_service.cpp +++ b/system/bluetooth_monitor/service/l2ping_service.cpp @@ -31,7 +31,9 @@ namespace bp = boost::process; -L2pingService::L2pingService(const int port) : port_(port), socket_(-1) {} +L2pingService::L2pingService(const int port) : port_(port), socket_(-1) +{ +} bool L2pingService::initialize() { @@ -74,7 +76,10 @@ bool L2pingService::initialize() return true; } -void L2pingService::shutdown() { close(socket_); } +void L2pingService::shutdown() +{ + close(socket_); +} void L2pingService::run() { diff --git a/system/bluetooth_monitor/src/bluetooth_monitor.cpp b/system/bluetooth_monitor/src/bluetooth_monitor.cpp index f8414c6353cfd..e7e20e0801965 100644 --- a/system/bluetooth_monitor/src/bluetooth_monitor.cpp +++ b/system/bluetooth_monitor/src/bluetooth_monitor.cpp @@ -130,7 +130,10 @@ bool BluetoothMonitor::receiveData(diagnostic_updater::DiagnosticStatusWrapper & return true; } -void BluetoothMonitor::closeConnection() { close(socket_); } +void BluetoothMonitor::closeConnection() +{ + close(socket_); +} void BluetoothMonitor::setErrorLevel(diagnostic_updater::DiagnosticStatusWrapper & stat) { diff --git a/system/default_ad_api/src/planning.cpp b/system/default_ad_api/src/planning.cpp index 2fa7ae5b97749..57daa8f840428 100644 --- a/system/default_ad_api/src/planning.cpp +++ b/system/default_ad_api/src/planning.cpp @@ -102,7 +102,10 @@ PlanningNode::PlanningNode(const rclcpp::NodeOptions & options) : Node("planning timer_ = rclcpp::create_timer(this, get_clock(), rate.period(), [this]() { on_timer(); }); } -void PlanningNode::on_trajectory(const Trajectory::ConstSharedPtr msg) { trajectory_ = msg; } +void PlanningNode::on_trajectory(const Trajectory::ConstSharedPtr msg) +{ + trajectory_ = msg; +} void PlanningNode::on_kinematic_state(const KinematicState::ConstSharedPtr msg) { diff --git a/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp b/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp index 21e0aa3283a26..cfe1692c91df9 100644 --- a/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp +++ b/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp @@ -240,7 +240,10 @@ void DummyDiagPublisher::addDiagByStatus(const std::string & diag_name, const St } } -void DummyDiagPublisher::onTimer() { updater_.force_update(); } +void DummyDiagPublisher::onTimer() +{ + updater_.force_update(); +} DummyDiagPublisher::DummyDiagPublisher() : Node( diff --git a/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp b/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp index a2e39f36e7076..5c9562463f891 100644 --- a/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp +++ b/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp @@ -96,7 +96,10 @@ void MrmComfortableStopOperator::publishVelocityLimitClearCommand() const pub_velocity_limit_clear_command_->publish(velocity_limit_clear_command); } -void MrmComfortableStopOperator::onTimer() const { publishStatus(); } +void MrmComfortableStopOperator::onTimer() const +{ + publishStatus(); +} } // namespace mrm_comfortable_stop_operator diff --git a/system/system_monitor/reader/traffic_reader/traffic_reader_service.cpp b/system/system_monitor/reader/traffic_reader/traffic_reader_service.cpp index 90d8867235e37..592ce515f9a1f 100644 --- a/system/system_monitor/reader/traffic_reader/traffic_reader_service.cpp +++ b/system/system_monitor/reader/traffic_reader/traffic_reader_service.cpp @@ -61,7 +61,10 @@ bool TrafficReaderService::initialize() return true; } -void TrafficReaderService::shutdown() { io_service_.stop(); } +void TrafficReaderService::shutdown() +{ + io_service_.stop(); +} void TrafficReaderService::run() { diff --git a/system/system_monitor/src/cpu_monitor/cpu_monitor_base.cpp b/system/system_monitor/src/cpu_monitor/cpu_monitor_base.cpp index ff1156b3059eb..b5e0e51c0ceaf 100644 --- a/system/system_monitor/src/cpu_monitor/cpu_monitor_base.cpp +++ b/system/system_monitor/src/cpu_monitor/cpu_monitor_base.cpp @@ -74,7 +74,10 @@ CPUMonitorBase::CPUMonitorBase(const std::string & node_name, const rclcpp::Node this->create_publisher("~/cpu_usage", durable_qos); } -void CPUMonitorBase::update() { updater_.force_update(); } +void CPUMonitorBase::update() +{ + updater_.force_update(); +} void CPUMonitorBase::checkTemp(diagnostic_updater::DiagnosticStatusWrapper & stat) { diff --git a/system/system_monitor/src/cpu_monitor/tegra_cpu_monitor.cpp b/system/system_monitor/src/cpu_monitor/tegra_cpu_monitor.cpp index d4050f9d779c4..0cadc01bf9e61 100644 --- a/system/system_monitor/src/cpu_monitor/tegra_cpu_monitor.cpp +++ b/system/system_monitor/src/cpu_monitor/tegra_cpu_monitor.cpp @@ -31,7 +31,9 @@ CPUMonitor::CPUMonitor(const rclcpp::NodeOptions & options) : CPUMonitorBase("cp updater_.removeByName("CPU Thermal Throttling"); } -void CPUMonitor::checkThrottling(diagnostic_updater::DiagnosticStatusWrapper & /* stat */) {} +void CPUMonitor::checkThrottling(diagnostic_updater::DiagnosticStatusWrapper & /* stat */) +{ +} void CPUMonitor::getTempNames() { diff --git a/system/system_monitor/src/gpu_monitor/gpu_monitor_base.cpp b/system/system_monitor/src/gpu_monitor/gpu_monitor_base.cpp index d21de67f643ce..1e763146162bd 100644 --- a/system/system_monitor/src/gpu_monitor/gpu_monitor_base.cpp +++ b/system/system_monitor/src/gpu_monitor/gpu_monitor_base.cpp @@ -44,7 +44,10 @@ GPUMonitorBase::GPUMonitorBase(const std::string & node_name, const rclcpp::Node updater_.add("GPU Frequency", this, &GPUMonitorBase::checkFrequency); } -void GPUMonitorBase::update() { updater_.force_update(); } +void GPUMonitorBase::update() +{ + updater_.force_update(); +} void GPUMonitorBase::shut_down() { /*NOOP by default.*/ diff --git a/system/system_monitor/src/mem_monitor/mem_monitor.cpp b/system/system_monitor/src/mem_monitor/mem_monitor.cpp index 489a4dc72bbe8..7a76f84612da8 100644 --- a/system/system_monitor/src/mem_monitor/mem_monitor.cpp +++ b/system/system_monitor/src/mem_monitor/mem_monitor.cpp @@ -40,7 +40,10 @@ MemMonitor::MemMonitor(const rclcpp::NodeOptions & options) updater_.add("Memory Usage", this, &MemMonitor::checkUsage); } -void MemMonitor::update() { updater_.force_update(); } +void MemMonitor::update() +{ + updater_.force_update(); +} void MemMonitor::checkUsage(diagnostic_updater::DiagnosticStatusWrapper & stat) { diff --git a/system/system_monitor/src/net_monitor/net_monitor.cpp b/system/system_monitor/src/net_monitor/net_monitor.cpp index 4ceec63ca862c..07f0a6833ca16 100644 --- a/system/system_monitor/src/net_monitor/net_monitor.cpp +++ b/system/system_monitor/src/net_monitor/net_monitor.cpp @@ -85,7 +85,10 @@ NetMonitor::NetMonitor(const rclcpp::NodeOptions & options) send_start_nethogs_request(); } -NetMonitor::~NetMonitor() { shutdown_nl80211(); } +NetMonitor::~NetMonitor() +{ + shutdown_nl80211(); +} void NetMonitor::check_connection(diagnostic_updater::DiagnosticStatusWrapper & status) { @@ -323,7 +326,10 @@ void NetMonitor::on_timer() update_network_list(); } -void NetMonitor::shutdown_nl80211() { nl80211_.shutdown(); } +void NetMonitor::shutdown_nl80211() +{ + nl80211_.shutdown(); +} void NetMonitor::make_invalid_diagnostic_status( const NetworkInfomation & network, int index, diff --git a/system/system_monitor/src/net_monitor/nl80211.cpp b/system/system_monitor/src/net_monitor/nl80211.cpp index f7cf54197099b..cd39786301f53 100644 --- a/system/system_monitor/src/net_monitor/nl80211.cpp +++ b/system/system_monitor/src/net_monitor/nl80211.cpp @@ -24,7 +24,9 @@ #include #include -NL80211::NL80211() : bitrate_(0.0), initialized_(false), socket_(nullptr), id_(-1), cb_(nullptr) {} +NL80211::NL80211() : bitrate_(0.0), initialized_(false), socket_(nullptr), id_(-1), cb_(nullptr) +{ +} // Attribute validation policy static struct nla_policy stats_policy[NL80211_STA_INFO_MAX + 1]; diff --git a/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp b/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp index 10dba2f96e164..6ccf7a54b940f 100644 --- a/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp +++ b/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp @@ -49,7 +49,10 @@ NTPMonitor::NTPMonitor(const rclcpp::NodeOptions & options) updater_.add("NTP Offset", this, &NTPMonitor::checkOffset); } -void NTPMonitor::update() { updater_.force_update(); } +void NTPMonitor::update() +{ + updater_.force_update(); +} void NTPMonitor::checkOffset(diagnostic_updater::DiagnosticStatusWrapper & stat) { diff --git a/system/system_monitor/src/voltage_monitor/voltage_monitor.cpp b/system/system_monitor/src/voltage_monitor/voltage_monitor.cpp index f0e3979adbe9b..a8ede38bc52cd 100644 --- a/system/system_monitor/src/voltage_monitor/voltage_monitor.cpp +++ b/system/system_monitor/src/voltage_monitor/voltage_monitor.cpp @@ -200,7 +200,10 @@ void VoltageMonitor::checkBatteryStatus(diagnostic_updater::DiagnosticStatusWrap SystemMonitorUtility::stopMeasurement(t_start, stat); } -void VoltageMonitor::update() { updater_.force_update(); } +void VoltageMonitor::update() +{ + updater_.force_update(); +} #include RCLCPP_COMPONENTS_REGISTER_NODE(VoltageMonitor) diff --git a/system/system_monitor/test/src/cpu_monitor/mpstat1.cpp b/system/system_monitor/test/src/cpu_monitor/mpstat1.cpp index 0b44abd00f720..a29697d546b95 100644 --- a/system/system_monitor/test/src/cpu_monitor/mpstat1.cpp +++ b/system/system_monitor/test/src/cpu_monitor/mpstat1.cpp @@ -17,4 +17,7 @@ * @brief dummy mpstat mpstat to return error */ -int main(int argc, char ** argv) { return -1; } +int main(int argc, char ** argv) +{ + return -1; +} diff --git a/system/system_monitor/test/src/cpu_monitor/mpstat2.cpp b/system/system_monitor/test/src/cpu_monitor/mpstat2.cpp index 35466a986817a..5a8028777d927 100644 --- a/system/system_monitor/test/src/cpu_monitor/mpstat2.cpp +++ b/system/system_monitor/test/src/cpu_monitor/mpstat2.cpp @@ -17,4 +17,7 @@ * @brief dummy mpstat executable to provide nothing */ -int main(int argc, char ** argv) { return 0; } +int main(int argc, char ** argv) +{ + return 0; +} diff --git a/system/system_monitor/test/src/cpu_monitor/test_unknown_cpu_monitor.cpp b/system/system_monitor/test/src/cpu_monitor/test_unknown_cpu_monitor.cpp index 53fffe03bb02b..0904e064478e4 100644 --- a/system/system_monitor/test/src/cpu_monitor/test_unknown_cpu_monitor.cpp +++ b/system/system_monitor/test/src/cpu_monitor/test_unknown_cpu_monitor.cpp @@ -68,7 +68,10 @@ class CPUMonitorTestSuite : public ::testing::Test void TearDown() { rclcpp::shutdown(); } }; -TEST_F(CPUMonitorTestSuite, test) { ASSERT_TRUE(true); } +TEST_F(CPUMonitorTestSuite, test) +{ + ASSERT_TRUE(true); +} int main(int argc, char ** argv) { diff --git a/system/system_monitor/test/src/gpu_monitor/test_unknown_gpu_monitor.cpp b/system/system_monitor/test/src/gpu_monitor/test_unknown_gpu_monitor.cpp index 06ebce8cc6ce2..302898b3bc664 100644 --- a/system/system_monitor/test/src/gpu_monitor/test_unknown_gpu_monitor.cpp +++ b/system/system_monitor/test/src/gpu_monitor/test_unknown_gpu_monitor.cpp @@ -66,7 +66,10 @@ class GPUMonitorTestSuite : public ::testing::Test void TearDown() { rclcpp::shutdown(); } }; -TEST_F(GPUMonitorTestSuite, test) { ASSERT_TRUE(true); } +TEST_F(GPUMonitorTestSuite, test) +{ + ASSERT_TRUE(true); +} int main(int argc, char ** argv) { diff --git a/system/system_monitor/test/src/hdd_monitor/df1.cpp b/system/system_monitor/test/src/hdd_monitor/df1.cpp index 99700d9d381d5..71f2e4f1b082f 100644 --- a/system/system_monitor/test/src/hdd_monitor/df1.cpp +++ b/system/system_monitor/test/src/hdd_monitor/df1.cpp @@ -17,4 +17,7 @@ * @brief dummy df executable to return error */ -int main(int argc, char ** argv) { return -1; } +int main(int argc, char ** argv) +{ + return -1; +} diff --git a/system/system_monitor/test/src/mem_monitor/free1.cpp b/system/system_monitor/test/src/mem_monitor/free1.cpp index a6e2bb49c3093..34356391eabdd 100644 --- a/system/system_monitor/test/src/mem_monitor/free1.cpp +++ b/system/system_monitor/test/src/mem_monitor/free1.cpp @@ -17,4 +17,7 @@ * @brief dummy free executable to return error */ -int main(int argc, char ** argv) { return -1; } +int main(int argc, char ** argv) +{ + return -1; +} diff --git a/system/system_monitor/test/src/ntp_monitor/ntpdate1.cpp b/system/system_monitor/test/src/ntp_monitor/ntpdate1.cpp index 0f79345c1e6c0..bc5510cb04cae 100644 --- a/system/system_monitor/test/src/ntp_monitor/ntpdate1.cpp +++ b/system/system_monitor/test/src/ntp_monitor/ntpdate1.cpp @@ -17,4 +17,7 @@ * @brief dummy ntpdate executable to return error */ -int main(int argc, char ** argv) { return -1; } +int main(int argc, char ** argv) +{ + return -1; +} diff --git a/system/system_monitor/test/src/process_monitor/echo1.cpp b/system/system_monitor/test/src/process_monitor/echo1.cpp index e029bc48c6ed2..7f3f0e47759f2 100644 --- a/system/system_monitor/test/src/process_monitor/echo1.cpp +++ b/system/system_monitor/test/src/process_monitor/echo1.cpp @@ -17,4 +17,7 @@ * @brief dummy echo executable to return error */ -int main(int argc, char ** argv) { return -1; } +int main(int argc, char ** argv) +{ + return -1; +} diff --git a/system/system_monitor/test/src/process_monitor/sed1.cpp b/system/system_monitor/test/src/process_monitor/sed1.cpp index 5e7d953cf244a..b8b821d6849a3 100644 --- a/system/system_monitor/test/src/process_monitor/sed1.cpp +++ b/system/system_monitor/test/src/process_monitor/sed1.cpp @@ -17,4 +17,7 @@ * @brief dummy sed executable to return error */ -int main(int argc, char ** argv) { return -1; } +int main(int argc, char ** argv) +{ + return -1; +} diff --git a/system/system_monitor/test/src/process_monitor/sort1.cpp b/system/system_monitor/test/src/process_monitor/sort1.cpp index ba48b612f05b7..857d46855b351 100644 --- a/system/system_monitor/test/src/process_monitor/sort1.cpp +++ b/system/system_monitor/test/src/process_monitor/sort1.cpp @@ -17,4 +17,7 @@ * @brief dummy sort executable to return error */ -int main(int argc, char ** argv) { return -1; } +int main(int argc, char ** argv) +{ + return -1; +} diff --git a/system/system_monitor/test/src/process_monitor/top1.cpp b/system/system_monitor/test/src/process_monitor/top1.cpp index 56f9243122f23..facb1deb528e4 100644 --- a/system/system_monitor/test/src/process_monitor/top1.cpp +++ b/system/system_monitor/test/src/process_monitor/top1.cpp @@ -17,4 +17,7 @@ * @brief dummy top executable to return error */ -int main(int argc, char ** argv) { return -1; } +int main(int argc, char ** argv) +{ + return -1; +} diff --git a/system/system_monitor/test/src/process_monitor/top2.cpp b/system/system_monitor/test/src/process_monitor/top2.cpp index c759ed2c21441..03c1d9cf0eb32 100644 --- a/system/system_monitor/test/src/process_monitor/top2.cpp +++ b/system/system_monitor/test/src/process_monitor/top2.cpp @@ -19,4 +19,7 @@ #include -int main(int argc, char ** argv) { return 0; } +int main(int argc, char ** argv) +{ + return 0; +} diff --git a/system/topic_state_monitor/src/topic_state_monitor/topic_state_monitor.cpp b/system/topic_state_monitor/src/topic_state_monitor/topic_state_monitor.cpp index b6a480c0966fb..ff6806e96fac9 100644 --- a/system/topic_state_monitor/src/topic_state_monitor/topic_state_monitor.cpp +++ b/system/topic_state_monitor/src/topic_state_monitor/topic_state_monitor.cpp @@ -16,7 +16,9 @@ namespace topic_state_monitor { -TopicStateMonitor::TopicStateMonitor(rclcpp::Node & node) : clock_(node.get_clock()) {} +TopicStateMonitor::TopicStateMonitor(rclcpp::Node & node) : clock_(node.get_clock()) +{ +} void TopicStateMonitor::update() { @@ -64,7 +66,10 @@ double TopicStateMonitor::calcTopicRate() const return static_cast(num_intervals) / time_diff; } -bool TopicStateMonitor::isNotReceived() const { return time_buffer_.empty(); } +bool TopicStateMonitor::isNotReceived() const +{ + return time_buffer_.empty(); +} bool TopicStateMonitor::isWarnRate() const { diff --git a/vehicle/external_cmd_converter/src/node.cpp b/vehicle/external_cmd_converter/src/node.cpp index 9be35847d0606..2a310b16c0841 100644 --- a/vehicle/external_cmd_converter/src/node.cpp +++ b/vehicle/external_cmd_converter/src/node.cpp @@ -79,7 +79,10 @@ ExternalCmdConverterNode::ExternalCmdConverterNode(const rclcpp::NodeOptions & n current_shift_cmd_ = std::make_shared(); } -void ExternalCmdConverterNode::onTimer() { updater_.force_update(); } +void ExternalCmdConverterNode::onTimer() +{ + updater_.force_update(); +} void ExternalCmdConverterNode::onVelocity(const Odometry::ConstSharedPtr msg) { diff --git a/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp b/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp index eac6f630b9ecd..7e42551454e24 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp @@ -21,7 +21,10 @@ namespace raw_vehicle_cmd_converter { -CSVLoader::CSVLoader(const std::string & csv_path) { csv_path_ = csv_path; } +CSVLoader::CSVLoader(const std::string & csv_path) +{ + csv_path_ = csv_path; +} bool CSVLoader::readCSV(Table & result, const char delim) { diff --git a/vehicle/steer_offset_estimator/src/steer_offset_estimator_node.cpp b/vehicle/steer_offset_estimator/src/steer_offset_estimator_node.cpp index d7943623491a2..96a88cc7c1484 100644 --- a/vehicle/steer_offset_estimator/src/steer_offset_estimator_node.cpp +++ b/vehicle/steer_offset_estimator/src/steer_offset_estimator_node.cpp @@ -79,9 +79,15 @@ void SteerOffsetEstimatorNode::monitorSteerOffset(DiagnosticStatusWrapper & stat stat.summary(DiagStatus::OK, "Calibration OK"); } -void SteerOffsetEstimatorNode::onTwist(const TwistStamped::ConstSharedPtr msg) { twist_ptr_ = msg; } +void SteerOffsetEstimatorNode::onTwist(const TwistStamped::ConstSharedPtr msg) +{ + twist_ptr_ = msg; +} -void SteerOffsetEstimatorNode::onSteer(const Steering::ConstSharedPtr msg) { steer_ptr_ = msg; } +void SteerOffsetEstimatorNode::onSteer(const Steering::ConstSharedPtr msg) +{ + steer_ptr_ = msg; +} bool SteerOffsetEstimatorNode::updateSteeringOffset() {