Skip to content

Commit

Permalink
refactor(imu_corrector): apply static analysis (autowarefoundation#7967)
Browse files Browse the repository at this point in the history
* 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>
Co-authored-by: Yamato Ando <yamato.ando@gmail.com>
  • Loading branch information
3 people committed Jul 22, 2024
1 parent 9bb228f commit 1381424
Show file tree
Hide file tree
Showing 4 changed files with 15 additions and 12 deletions.
2 changes: 1 addition & 1 deletion sensing/imu_corrector/src/gyro_bias_estimation_module.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ class GyroBiasEstimationModule
void update_bias(
const std::vector<geometry_msgs::msg::PoseStamped> & pose_list,
const std::vector<geometry_msgs::msg::Vector3Stamped> & gyro_list);
geometry_msgs::msg::Vector3 get_bias_base_link() const;
[[nodiscard]] geometry_msgs::msg::Vector3 get_bias_base_link() const;

private:
std::pair<geometry_msgs::msg::Vector3, geometry_msgs::msg::Vector3> gyro_bias_pair_;
Expand Down
19 changes: 10 additions & 9 deletions sensing/imu_corrector/src/imu_corrector_core.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@

#include <algorithm>

std::array<double, 9> transformCovariance(const std::array<double, 9> & cov)
std::array<double, 9> transform_covariance(const std::array<double, 9> & cov)
{
using COV_IDX = autoware::universe_utils::xyz_covariance_index::XYZ_COV_IDX;

Expand All @@ -32,15 +32,15 @@ std::array<double, 9> transformCovariance(const std::array<double, 9> & cov)
max_cov = std::max(max_cov, cov[COV_IDX::Y_Y]);
max_cov = std::max(max_cov, cov[COV_IDX::Z_Z]);

std::array<double, 9> cov_transformed;
std::array<double, 9> cov_transformed{};
cov_transformed.fill(0.);
cov_transformed[COV_IDX::X_X] = max_cov;
cov_transformed[COV_IDX::Y_Y] = max_cov;
cov_transformed[COV_IDX::Z_Z] = max_cov;
return cov_transformed;
}

geometry_msgs::msg::Vector3 transformVector3(
geometry_msgs::msg::Vector3 transform_vector3(
const geometry_msgs::msg::Vector3 & vec, const geometry_msgs::msg::TransformStamped & transform)
{
geometry_msgs::msg::Vector3Stamped vec_stamped;
Expand Down Expand Up @@ -73,12 +73,12 @@ ImuCorrector::ImuCorrector(const rclcpp::NodeOptions & options)
accel_stddev_imu_link_ = declare_parameter<double>("acceleration_stddev", 10000.0);

imu_sub_ = create_subscription<sensor_msgs::msg::Imu>(
"input", rclcpp::QoS{1}, std::bind(&ImuCorrector::callbackImu, this, std::placeholders::_1));
"input", rclcpp::QoS{1}, std::bind(&ImuCorrector::callback_imu, this, std::placeholders::_1));

imu_pub_ = create_publisher<sensor_msgs::msg::Imu>("output", rclcpp::QoS{10});
}

void ImuCorrector::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr)
void ImuCorrector::callback_imu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr)
{
sensor_msgs::msg::Imu imu_msg;
imu_msg = *imu_msg_ptr;
Expand Down Expand Up @@ -113,12 +113,13 @@ void ImuCorrector::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m
imu_msg_base_link.header.stamp = imu_msg_ptr->header.stamp;
imu_msg_base_link.header.frame_id = output_frame_;
imu_msg_base_link.linear_acceleration =
transformVector3(imu_msg.linear_acceleration, *tf_imu2base_ptr);
transform_vector3(imu_msg.linear_acceleration, *tf_imu2base_ptr);
imu_msg_base_link.linear_acceleration_covariance =
transformCovariance(imu_msg.linear_acceleration_covariance);
imu_msg_base_link.angular_velocity = transformVector3(imu_msg.angular_velocity, *tf_imu2base_ptr);
transform_covariance(imu_msg.linear_acceleration_covariance);
imu_msg_base_link.angular_velocity =
transform_vector3(imu_msg.angular_velocity, *tf_imu2base_ptr);
imu_msg_base_link.angular_velocity_covariance =
transformCovariance(imu_msg.angular_velocity_covariance);
transform_covariance(imu_msg.angular_velocity_covariance);

imu_pub_->publish(imu_msg_base_link);
}
Expand Down
2 changes: 1 addition & 1 deletion sensing/imu_corrector/src/imu_corrector_core.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ class ImuCorrector : public rclcpp::Node
explicit ImuCorrector(const rclcpp::NodeOptions & options);

private:
void callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr);
void callback_imu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr);

rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,9 @@ TEST_F(GyroBiasEstimationModuleTest, GetBiasEstimationWhenVehicleStopped)

TEST_F(GyroBiasEstimationModuleTest, GetInsufficientDataException)
{
ASSERT_THROW(module.get_bias_base_link(), std::runtime_error);
// for the case of method with [[nodiscard]] attribute
geometry_msgs::msg::Vector3 bias_base_link;
ASSERT_THROW(bias_base_link = module.get_bias_base_link(), std::runtime_error);
}

TEST_F(GyroBiasEstimationModuleTest, GetInsufficientDataExceptionWhenVehicleMoving)
Expand Down

0 comments on commit 1381424

Please sign in to comment.