Skip to content

Commit 2b0de17

Browse files
a-maumaupre-commit-ci[bot]
a-maumau
and
pre-commit-ci[bot]
authored
refactor(gyro_odometer): apply static analysis (#7360)
* refactor based on linter Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com> * style(pre-commit): autofix --------- Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
1 parent fbe57b1 commit 2b0de17

7 files changed

+92
-96
lines changed

‎localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp

+8-8
Original file line numberDiff line numberDiff line change
@@ -30,14 +30,14 @@ class DiagnosticsModule
3030
public:
3131
DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name);
3232
void clear();
33-
void addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg);
33+
void add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg);
3434
template <typename T>
35-
void addKeyValue(const std::string & key, const T & value);
36-
void updateLevelAndMessage(const int8_t level, const std::string & message);
35+
void add_key_value(const std::string & key, const T & value);
36+
void update_level_and_message(const int8_t level, const std::string & message);
3737
void publish(const rclcpp::Time & publish_time_stamp);
3838

3939
private:
40-
diagnostic_msgs::msg::DiagnosticArray createDiagnosticsArray(
40+
[[nodiscard]] diagnostic_msgs::msg::DiagnosticArray create_diagnostics_array(
4141
const rclcpp::Time & publish_time_stamp) const;
4242

4343
rclcpp::Clock::SharedPtr clock_;
@@ -47,18 +47,18 @@ class DiagnosticsModule
4747
};
4848

4949
template <typename T>
50-
void DiagnosticsModule::addKeyValue(const std::string & key, const T & value)
50+
void DiagnosticsModule::add_key_value(const std::string & key, const T & value)
5151
{
5252
diagnostic_msgs::msg::KeyValue key_value;
5353
key_value.key = key;
5454
key_value.value = std::to_string(value);
55-
addKeyValue(key_value);
55+
add_key_value(key_value);
5656
}
5757

5858
template <>
59-
void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value);
59+
void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value);
6060
template <>
61-
void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value);
61+
void DiagnosticsModule::add_key_value(const std::string & key, const bool & value);
6262

6363
} // namespace autoware::gyro_odometer
6464

‎localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp

+4-5
Original file line numberDiff line numberDiff line change
@@ -47,14 +47,13 @@ class GyroOdometerNode : public rclcpp::Node
4747

4848
public:
4949
explicit GyroOdometerNode(const rclcpp::NodeOptions & node_options);
50-
~GyroOdometerNode();
5150

5251
private:
53-
void callbackVehicleTwist(
52+
void callback_vehicle_twist(
5453
const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_msg_ptr);
55-
void callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr);
56-
void concatGyroAndOdometer();
57-
void publishData(const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw);
54+
void callback_imu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr);
55+
void concat_gyro_and_odometer();
56+
void publish_data(const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw);
5857

5958
rclcpp::Subscription<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr
6059
vehicle_twist_sub_;

‎localization/gyro_odometer/src/diagnostics_module.cpp

+8-8
Original file line numberDiff line numberDiff line change
@@ -44,7 +44,7 @@ void DiagnosticsModule::clear()
4444
diagnostics_status_msg_.message = "";
4545
}
4646

47-
void DiagnosticsModule::addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg)
47+
void DiagnosticsModule::add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg)
4848
{
4949
auto it = std::find_if(
5050
std::begin(diagnostics_status_msg_.values), std::end(diagnostics_status_msg_.values),
@@ -58,24 +58,24 @@ void DiagnosticsModule::addKeyValue(const diagnostic_msgs::msg::KeyValue & key_v
5858
}
5959

6060
template <>
61-
void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value)
61+
void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value)
6262
{
6363
diagnostic_msgs::msg::KeyValue key_value;
6464
key_value.key = key;
6565
key_value.value = value;
66-
addKeyValue(key_value);
66+
add_key_value(key_value);
6767
}
6868

6969
template <>
70-
void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value)
70+
void DiagnosticsModule::add_key_value(const std::string & key, const bool & value)
7171
{
7272
diagnostic_msgs::msg::KeyValue key_value;
7373
key_value.key = key;
7474
key_value.value = value ? "True" : "False";
75-
addKeyValue(key_value);
75+
add_key_value(key_value);
7676
}
7777

78-
void DiagnosticsModule::updateLevelAndMessage(const int8_t level, const std::string & message)
78+
void DiagnosticsModule::update_level_and_message(const int8_t level, const std::string & message)
7979
{
8080
if ((level > diagnostic_msgs::msg::DiagnosticStatus::OK)) {
8181
if (!diagnostics_status_msg_.message.empty()) {
@@ -90,10 +90,10 @@ void DiagnosticsModule::updateLevelAndMessage(const int8_t level, const std::str
9090

9191
void DiagnosticsModule::publish(const rclcpp::Time & publish_time_stamp)
9292
{
93-
diagnostics_pub_->publish(createDiagnosticsArray(publish_time_stamp));
93+
diagnostics_pub_->publish(create_diagnostics_array(publish_time_stamp));
9494
}
9595

96-
diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::createDiagnosticsArray(
96+
diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::create_diagnostics_array(
9797
const rclcpp::Time & publish_time_stamp) const
9898
{
9999
diagnostic_msgs::msg::DiagnosticArray diagnostics_msg;

‎localization/gyro_odometer/src/gyro_odometer_core.cpp

+44-47
Original file line numberDiff line numberDiff line change
@@ -31,13 +31,13 @@
3131
namespace autoware::gyro_odometer
3232
{
3333

34-
std::array<double, 9> transformCovariance(const std::array<double, 9> & cov)
34+
std::array<double, 9> transform_covariance(const std::array<double, 9> & cov)
3535
{
3636
using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX;
3737

3838
double max_cov = std::max({cov[COV_IDX::X_X], cov[COV_IDX::Y_Y], cov[COV_IDX::Z_Z]});
3939

40-
std::array<double, 9> cov_transformed;
40+
std::array<double, 9> cov_transformed = {};
4141
cov_transformed.fill(0.);
4242
cov_transformed[COV_IDX::X_X] = max_cov;
4343
cov_transformed[COV_IDX::Y_Y] = max_cov;
@@ -57,11 +57,11 @@ GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options)
5757

5858
vehicle_twist_sub_ = create_subscription<geometry_msgs::msg::TwistWithCovarianceStamped>(
5959
"vehicle/twist_with_covariance", rclcpp::QoS{100},
60-
std::bind(&GyroOdometerNode::callbackVehicleTwist, this, std::placeholders::_1));
60+
std::bind(&GyroOdometerNode::callback_vehicle_twist, this, std::placeholders::_1));
6161

6262
imu_sub_ = create_subscription<sensor_msgs::msg::Imu>(
6363
"imu", rclcpp::QoS{100},
64-
std::bind(&GyroOdometerNode::callbackImu, this, std::placeholders::_1));
64+
std::bind(&GyroOdometerNode::callback_imu, this, std::placeholders::_1));
6565

6666
twist_raw_pub_ = create_publisher<geometry_msgs::msg::TwistStamped>("twist_raw", rclcpp::QoS{10});
6767
twist_with_covariance_raw_pub_ = create_publisher<geometry_msgs::msg::TwistWithCovarianceStamped>(
@@ -76,49 +76,46 @@ GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options)
7676
// TODO(YamatoAndo) createTimer
7777
}
7878

79-
GyroOdometerNode::~GyroOdometerNode()
80-
{
81-
}
82-
83-
void GyroOdometerNode::callbackVehicleTwist(
84-
const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_ptr)
79+
void GyroOdometerNode::callback_vehicle_twist(
80+
const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_msg_ptr)
8581
{
8682
diagnostics_->clear();
87-
diagnostics_->addKeyValue(
88-
"topic_time_stamp", static_cast<rclcpp::Time>(vehicle_twist_ptr->header.stamp).nanoseconds());
83+
diagnostics_->add_key_value(
84+
"topic_time_stamp",
85+
static_cast<rclcpp::Time>(vehicle_twist_msg_ptr->header.stamp).nanoseconds());
8986

9087
vehicle_twist_arrived_ = true;
91-
latest_vehicle_twist_ros_time_ = vehicle_twist_ptr->header.stamp;
92-
vehicle_twist_queue_.push_back(*vehicle_twist_ptr);
93-
concatGyroAndOdometer();
88+
latest_vehicle_twist_ros_time_ = vehicle_twist_msg_ptr->header.stamp;
89+
vehicle_twist_queue_.push_back(*vehicle_twist_msg_ptr);
90+
concat_gyro_and_odometer();
9491

95-
diagnostics_->publish(vehicle_twist_ptr->header.stamp);
92+
diagnostics_->publish(vehicle_twist_msg_ptr->header.stamp);
9693
}
9794

98-
void GyroOdometerNode::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr)
95+
void GyroOdometerNode::callback_imu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr)
9996
{
10097
diagnostics_->clear();
101-
diagnostics_->addKeyValue(
98+
diagnostics_->add_key_value(
10299
"topic_time_stamp", static_cast<rclcpp::Time>(imu_msg_ptr->header.stamp).nanoseconds());
103100

104101
imu_arrived_ = true;
105102
latest_imu_ros_time_ = imu_msg_ptr->header.stamp;
106103
gyro_queue_.push_back(*imu_msg_ptr);
107-
concatGyroAndOdometer();
104+
concat_gyro_and_odometer();
108105

109106
diagnostics_->publish(imu_msg_ptr->header.stamp);
110107
}
111108

112-
void GyroOdometerNode::concatGyroAndOdometer()
109+
void GyroOdometerNode::concat_gyro_and_odometer()
113110
{
114111
// check arrive first topic
115-
diagnostics_->addKeyValue("is_arrived_first_vehicle_twist", vehicle_twist_arrived_);
116-
diagnostics_->addKeyValue("is_arrived_first_imu", imu_arrived_);
112+
diagnostics_->add_key_value("is_arrived_first_vehicle_twist", vehicle_twist_arrived_);
113+
diagnostics_->add_key_value("is_arrived_first_imu", imu_arrived_);
117114
if (!vehicle_twist_arrived_) {
118115
std::stringstream message;
119116
message << "Twist msg is not subscribed";
120117
RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str());
121-
diagnostics_->updateLevelAndMessage(
118+
diagnostics_->update_level_and_message(
122119
diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
123120

124121
vehicle_twist_queue_.clear();
@@ -129,7 +126,7 @@ void GyroOdometerNode::concatGyroAndOdometer()
129126
std::stringstream message;
130127
message << "Imu msg is not subscribed";
131128
RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str());
132-
diagnostics_->updateLevelAndMessage(
129+
diagnostics_->update_level_and_message(
133130
diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
134131

135132
vehicle_twist_queue_.clear();
@@ -141,14 +138,14 @@ void GyroOdometerNode::concatGyroAndOdometer()
141138
const double vehicle_twist_dt =
142139
std::abs((this->now() - latest_vehicle_twist_ros_time_).seconds());
143140
const double imu_dt = std::abs((this->now() - latest_imu_ros_time_).seconds());
144-
diagnostics_->addKeyValue("vehicle_twist_time_stamp_dt", vehicle_twist_dt);
145-
diagnostics_->addKeyValue("imu_time_stamp_dt", imu_dt);
141+
diagnostics_->add_key_value("vehicle_twist_time_stamp_dt", vehicle_twist_dt);
142+
diagnostics_->add_key_value("imu_time_stamp_dt", imu_dt);
146143
if (vehicle_twist_dt > message_timeout_sec_) {
147144
const std::string message = fmt::format(
148145
"Vehicle twist msg is timeout. vehicle_twist_dt: {}[sec], tolerance {}[sec]",
149146
vehicle_twist_dt, message_timeout_sec_);
150147
RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message);
151-
diagnostics_->updateLevelAndMessage(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message);
148+
diagnostics_->update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message);
152149

153150
vehicle_twist_queue_.clear();
154151
gyro_queue_.clear();
@@ -158,16 +155,16 @@ void GyroOdometerNode::concatGyroAndOdometer()
158155
const std::string message = fmt::format(
159156
"Imu msg is timeout. imu_dt: {}[sec], tolerance {}[sec]", imu_dt, message_timeout_sec_);
160157
RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message);
161-
diagnostics_->updateLevelAndMessage(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message);
158+
diagnostics_->update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message);
162159

163160
vehicle_twist_queue_.clear();
164161
gyro_queue_.clear();
165162
return;
166163
}
167164

168165
// check queue size
169-
diagnostics_->addKeyValue("vehicle_twist_queue_size", vehicle_twist_queue_.size());
170-
diagnostics_->addKeyValue("imu_queue_size", gyro_queue_.size());
166+
diagnostics_->add_key_value("vehicle_twist_queue_size", vehicle_twist_queue_.size());
167+
diagnostics_->add_key_value("imu_queue_size", gyro_queue_.size());
171168
if (vehicle_twist_queue_.empty()) {
172169
// not output error and clear queue
173170
return;
@@ -182,13 +179,13 @@ void GyroOdometerNode::concatGyroAndOdometer()
182179
transform_listener_->getLatestTransform(gyro_queue_.front().header.frame_id, output_frame_);
183180

184181
const bool is_succeed_transform_imu = (tf_imu2base_ptr != nullptr);
185-
diagnostics_->addKeyValue("is_succeed_transform_imu", is_succeed_transform_imu);
182+
diagnostics_->add_key_value("is_succeed_transform_imu", is_succeed_transform_imu);
186183
if (!is_succeed_transform_imu) {
187184
std::stringstream message;
188185
message << "Please publish TF " << output_frame_ << " to "
189186
<< gyro_queue_.front().header.frame_id;
190187
RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str());
191-
diagnostics_->updateLevelAndMessage(
188+
diagnostics_->update_level_and_message(
192189
diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str());
193190

194191
vehicle_twist_queue_.clear();
@@ -208,7 +205,7 @@ void GyroOdometerNode::concatGyroAndOdometer()
208205

209206
gyro.header.frame_id = output_frame_;
210207
gyro.angular_velocity = transformed_angular_velocity.vector;
211-
gyro.angular_velocity_covariance = transformCovariance(gyro.angular_velocity_covariance);
208+
gyro.angular_velocity_covariance = transform_covariance(gyro.angular_velocity_covariance);
212209
}
213210

214211
using COV_IDX_XYZ = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX;
@@ -223,8 +220,8 @@ void GyroOdometerNode::concatGyroAndOdometer()
223220
vx_mean += vehicle_twist.twist.twist.linear.x;
224221
vx_covariance_original += vehicle_twist.twist.covariance[0 * 6 + 0];
225222
}
226-
vx_mean /= vehicle_twist_queue_.size();
227-
vx_covariance_original /= vehicle_twist_queue_.size();
223+
vx_mean /= static_cast<double>(vehicle_twist_queue_.size());
224+
vx_covariance_original /= static_cast<double>(vehicle_twist_queue_.size());
228225

229226
for (const auto & gyro : gyro_queue_) {
230227
gyro_mean.x += gyro.angular_velocity.x;
@@ -234,12 +231,12 @@ void GyroOdometerNode::concatGyroAndOdometer()
234231
gyro_covariance_original.y += gyro.angular_velocity_covariance[COV_IDX_XYZ::Y_Y];
235232
gyro_covariance_original.z += gyro.angular_velocity_covariance[COV_IDX_XYZ::Z_Z];
236233
}
237-
gyro_mean.x /= gyro_queue_.size();
238-
gyro_mean.y /= gyro_queue_.size();
239-
gyro_mean.z /= gyro_queue_.size();
240-
gyro_covariance_original.x /= gyro_queue_.size();
241-
gyro_covariance_original.y /= gyro_queue_.size();
242-
gyro_covariance_original.z /= gyro_queue_.size();
234+
gyro_mean.x /= static_cast<double>(gyro_queue_.size());
235+
gyro_mean.y /= static_cast<double>(gyro_queue_.size());
236+
gyro_mean.z /= static_cast<double>(gyro_queue_.size());
237+
gyro_covariance_original.x /= static_cast<double>(gyro_queue_.size());
238+
gyro_covariance_original.y /= static_cast<double>(gyro_queue_.size());
239+
gyro_covariance_original.z /= static_cast<double>(gyro_queue_.size());
243240

244241
// concat
245242
geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov;
@@ -257,23 +254,23 @@ void GyroOdometerNode::concatGyroAndOdometer()
257254
// From a statistical point of view, here we reduce the covariances according to the number of
258255
// observed data
259256
twist_with_cov.twist.covariance[COV_IDX_XYZRPY::X_X] =
260-
vx_covariance_original / vehicle_twist_queue_.size();
257+
vx_covariance_original / static_cast<double>(vehicle_twist_queue_.size());
261258
twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Y_Y] = 100000.0;
262259
twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Z_Z] = 100000.0;
263260
twist_with_cov.twist.covariance[COV_IDX_XYZRPY::ROLL_ROLL] =
264-
gyro_covariance_original.x / gyro_queue_.size();
261+
gyro_covariance_original.x / static_cast<double>(gyro_queue_.size());
265262
twist_with_cov.twist.covariance[COV_IDX_XYZRPY::PITCH_PITCH] =
266-
gyro_covariance_original.y / gyro_queue_.size();
263+
gyro_covariance_original.y / static_cast<double>(gyro_queue_.size());
267264
twist_with_cov.twist.covariance[COV_IDX_XYZRPY::YAW_YAW] =
268-
gyro_covariance_original.z / gyro_queue_.size();
265+
gyro_covariance_original.z / static_cast<double>(gyro_queue_.size());
269266

270-
publishData(twist_with_cov);
267+
publish_data(twist_with_cov);
271268

272269
vehicle_twist_queue_.clear();
273270
gyro_queue_.clear();
274271
}
275272

276-
void GyroOdometerNode::publishData(
273+
void GyroOdometerNode::publish_data(
277274
const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw)
278275
{
279276
geometry_msgs::msg::TwistStamped twist_raw;

‎localization/gyro_odometer/test/test_gyro_odometer_helper.cpp

+3-3
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,7 @@
1717
using geometry_msgs::msg::TwistWithCovarianceStamped;
1818
using sensor_msgs::msg::Imu;
1919

20-
Imu generateSampleImu()
20+
Imu generate_sample_imu()
2121
{
2222
Imu imu;
2323
imu.header.frame_id = "base_link";
@@ -27,15 +27,15 @@ Imu generateSampleImu()
2727
return imu;
2828
}
2929

30-
TwistWithCovarianceStamped generateSampleVelocity()
30+
TwistWithCovarianceStamped generate_sample_velocity()
3131
{
3232
TwistWithCovarianceStamped twist;
3333
twist.header.frame_id = "base_link";
3434
twist.twist.twist.linear.x = 1.0;
3535
return twist;
3636
}
3737

38-
rclcpp::NodeOptions getNodeOptionsWithDefaultParams()
38+
rclcpp::NodeOptions get_node_options_with_default_params()
3939
{
4040
rclcpp::NodeOptions node_options;
4141

‎localization/gyro_odometer/test/test_gyro_odometer_helper.hpp

+3-6
Original file line numberDiff line numberDiff line change
@@ -20,11 +20,8 @@
2020
#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
2121
#include <sensor_msgs/msg/imu.hpp>
2222

23-
using geometry_msgs::msg::TwistWithCovarianceStamped;
24-
using sensor_msgs::msg::Imu;
25-
26-
Imu generateSampleImu();
27-
TwistWithCovarianceStamped generateSampleVelocity();
28-
rclcpp::NodeOptions getNodeOptionsWithDefaultParams();
23+
sensor_msgs::msg::Imu generate_sample_imu();
24+
geometry_msgs::msg::TwistWithCovarianceStamped generate_sample_velocity();
25+
rclcpp::NodeOptions get_node_options_with_default_params();
2926

3027
#endif // TEST_GYRO_ODOMETER_HELPER_HPP_

0 commit comments

Comments
 (0)