From b27b65be5fcf77fe6b604f9302d68c3527424b2d Mon Sep 17 00:00:00 2001 From: a-maumau Date: Fri, 7 Jun 2024 15:06:19 +0900 Subject: [PATCH] refactor based on linter Signed-off-by: a-maumau --- .../gyro_odometer/diagnostics_module.hpp | 16 ++-- .../gyro_odometer/gyro_odometer_core.hpp | 9 +- .../gyro_odometer/src/diagnostics_module.cpp | 16 ++-- .../gyro_odometer/src/gyro_odometer_core.cpp | 92 +++++++++---------- .../test/test_gyro_odometer_helper.cpp | 6 +- .../test/test_gyro_odometer_helper.hpp | 9 +- .../test/test_gyro_odometer_pubsub.cpp | 40 ++++---- 7 files changed, 94 insertions(+), 94 deletions(-) diff --git a/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp b/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp index 49b881900b997..c5b7b7a592013 100644 --- a/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp +++ b/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp @@ -30,14 +30,14 @@ class DiagnosticsModule public: DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name); void clear(); - void addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg); + void add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg); template - void addKeyValue(const std::string & key, const T & value); - void updateLevelAndMessage(const int8_t level, const std::string & message); + void add_key_value(const std::string & key, const T & value); + void update_level_and_message(const int8_t level, const std::string & message); void publish(const rclcpp::Time & publish_time_stamp); private: - diagnostic_msgs::msg::DiagnosticArray createDiagnosticsArray( + [[nodiscard]] diagnostic_msgs::msg::DiagnosticArray create_diagnostics_array( const rclcpp::Time & publish_time_stamp) const; rclcpp::Clock::SharedPtr clock_; @@ -47,18 +47,18 @@ class DiagnosticsModule }; template -void DiagnosticsModule::addKeyValue(const std::string & key, const T & value) +void DiagnosticsModule::add_key_value(const std::string & key, const T & value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; key_value.value = std::to_string(value); - addKeyValue(key_value); + add_key_value(key_value); } template <> -void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value); +void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value); template <> -void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value); +void DiagnosticsModule::add_key_value(const std::string & key, const bool & value); } // namespace autoware::gyro_odometer diff --git a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp index 3b2da504f938a..f85640ab2c523 100644 --- a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp +++ b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp @@ -47,14 +47,13 @@ class GyroOdometerNode : public rclcpp::Node public: explicit GyroOdometerNode(const rclcpp::NodeOptions & node_options); - ~GyroOdometerNode(); private: - void callbackVehicleTwist( + void callback_vehicle_twist( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_msg_ptr); - void callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr); - void concatGyroAndOdometer(); - void publishData(const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw); + void callback_imu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr); + void concat_gyro_and_odometer(); + void publish_data(const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw); rclcpp::Subscription::SharedPtr vehicle_twist_sub_; diff --git a/localization/gyro_odometer/src/diagnostics_module.cpp b/localization/gyro_odometer/src/diagnostics_module.cpp index 9d88d8e6833ed..5d687943cef53 100644 --- a/localization/gyro_odometer/src/diagnostics_module.cpp +++ b/localization/gyro_odometer/src/diagnostics_module.cpp @@ -44,7 +44,7 @@ void DiagnosticsModule::clear() diagnostics_status_msg_.message = ""; } -void DiagnosticsModule::addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg) +void DiagnosticsModule::add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg) { auto it = std::find_if( 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 } template <> -void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value) +void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; key_value.value = value; - addKeyValue(key_value); + add_key_value(key_value); } template <> -void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value) +void DiagnosticsModule::add_key_value(const std::string & key, const bool & value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; key_value.value = value ? "True" : "False"; - addKeyValue(key_value); + add_key_value(key_value); } -void DiagnosticsModule::updateLevelAndMessage(const int8_t level, const std::string & message) +void DiagnosticsModule::update_level_and_message(const int8_t level, const std::string & message) { if ((level > diagnostic_msgs::msg::DiagnosticStatus::OK)) { if (!diagnostics_status_msg_.message.empty()) { @@ -90,10 +90,10 @@ void DiagnosticsModule::updateLevelAndMessage(const int8_t level, const std::str void DiagnosticsModule::publish(const rclcpp::Time & publish_time_stamp) { - diagnostics_pub_->publish(createDiagnosticsArray(publish_time_stamp)); + diagnostics_pub_->publish(create_diagnostics_array(publish_time_stamp)); } -diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::createDiagnosticsArray( +diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::create_diagnostics_array( const rclcpp::Time & publish_time_stamp) const { diagnostic_msgs::msg::DiagnosticArray diagnostics_msg; diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp index 1852d70bced71..81930841311cb 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -31,13 +31,13 @@ namespace autoware::gyro_odometer { -std::array transformCovariance(const std::array & cov) +std::array transform_covariance(const std::array & cov) { using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; double max_cov = std::max({cov[COV_IDX::X_X], cov[COV_IDX::Y_Y], cov[COV_IDX::Z_Z]}); - std::array cov_transformed; + std::array cov_transformed = {}; cov_transformed.fill(0.); cov_transformed[COV_IDX::X_X] = max_cov; cov_transformed[COV_IDX::Y_Y] = max_cov; @@ -57,11 +57,11 @@ GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) vehicle_twist_sub_ = create_subscription( "vehicle/twist_with_covariance", rclcpp::QoS{100}, - std::bind(&GyroOdometerNode::callbackVehicleTwist, this, std::placeholders::_1)); + std::bind(&GyroOdometerNode::callback_vehicle_twist, this, std::placeholders::_1)); imu_sub_ = create_subscription( "imu", rclcpp::QoS{100}, - std::bind(&GyroOdometerNode::callbackImu, this, std::placeholders::_1)); + std::bind(&GyroOdometerNode::callback_imu, this, std::placeholders::_1)); twist_raw_pub_ = create_publisher("twist_raw", rclcpp::QoS{10}); twist_with_covariance_raw_pub_ = create_publisher( @@ -76,49 +76,47 @@ GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) // TODO(YamatoAndo) createTimer } -GyroOdometerNode::~GyroOdometerNode() -{ -} - -void GyroOdometerNode::callbackVehicleTwist( - const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_ptr) +void GyroOdometerNode::callback_vehicle_twist( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_msg_ptr) { diagnostics_->clear(); - diagnostics_->addKeyValue( - "topic_time_stamp", static_cast(vehicle_twist_ptr->header.stamp).nanoseconds()); + diagnostics_->add_key_value( + "topic_time_stamp", + static_cast(vehicle_twist_msg_ptr->header.stamp).nanoseconds() + ); vehicle_twist_arrived_ = true; - latest_vehicle_twist_ros_time_ = vehicle_twist_ptr->header.stamp; - vehicle_twist_queue_.push_back(*vehicle_twist_ptr); - concatGyroAndOdometer(); + latest_vehicle_twist_ros_time_ = vehicle_twist_msg_ptr->header.stamp; + vehicle_twist_queue_.push_back(*vehicle_twist_msg_ptr); + concat_gyro_and_odometer(); - diagnostics_->publish(vehicle_twist_ptr->header.stamp); + diagnostics_->publish(vehicle_twist_msg_ptr->header.stamp); } -void GyroOdometerNode::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) +void GyroOdometerNode::callback_imu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) { diagnostics_->clear(); - diagnostics_->addKeyValue( + diagnostics_->add_key_value( "topic_time_stamp", static_cast(imu_msg_ptr->header.stamp).nanoseconds()); imu_arrived_ = true; latest_imu_ros_time_ = imu_msg_ptr->header.stamp; gyro_queue_.push_back(*imu_msg_ptr); - concatGyroAndOdometer(); + concat_gyro_and_odometer(); diagnostics_->publish(imu_msg_ptr->header.stamp); } -void GyroOdometerNode::concatGyroAndOdometer() +void GyroOdometerNode::concat_gyro_and_odometer() { // check arrive first topic - diagnostics_->addKeyValue("is_arrived_first_vehicle_twist", vehicle_twist_arrived_); - diagnostics_->addKeyValue("is_arrived_first_imu", imu_arrived_); + diagnostics_->add_key_value("is_arrived_first_vehicle_twist", vehicle_twist_arrived_); + diagnostics_->add_key_value("is_arrived_first_imu", imu_arrived_); if (!vehicle_twist_arrived_) { std::stringstream message; message << "Twist msg is not subscribed"; RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); - diagnostics_->updateLevelAndMessage( + diagnostics_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); vehicle_twist_queue_.clear(); @@ -129,7 +127,7 @@ void GyroOdometerNode::concatGyroAndOdometer() std::stringstream message; message << "Imu msg is not subscribed"; RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); - diagnostics_->updateLevelAndMessage( + diagnostics_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); vehicle_twist_queue_.clear(); @@ -141,14 +139,14 @@ void GyroOdometerNode::concatGyroAndOdometer() const double vehicle_twist_dt = std::abs((this->now() - latest_vehicle_twist_ros_time_).seconds()); const double imu_dt = std::abs((this->now() - latest_imu_ros_time_).seconds()); - diagnostics_->addKeyValue("vehicle_twist_time_stamp_dt", vehicle_twist_dt); - diagnostics_->addKeyValue("imu_time_stamp_dt", imu_dt); + diagnostics_->add_key_value("vehicle_twist_time_stamp_dt", vehicle_twist_dt); + diagnostics_->add_key_value("imu_time_stamp_dt", imu_dt); if (vehicle_twist_dt > message_timeout_sec_) { const std::string message = fmt::format( "Vehicle twist msg is timeout. vehicle_twist_dt: {}[sec], tolerance {}[sec]", vehicle_twist_dt, message_timeout_sec_); RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message); - diagnostics_->updateLevelAndMessage(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message); + diagnostics_->update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message); vehicle_twist_queue_.clear(); gyro_queue_.clear(); @@ -158,7 +156,7 @@ void GyroOdometerNode::concatGyroAndOdometer() const std::string message = fmt::format( "Imu msg is timeout. imu_dt: {}[sec], tolerance {}[sec]", imu_dt, message_timeout_sec_); RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message); - diagnostics_->updateLevelAndMessage(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message); + diagnostics_->update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message); vehicle_twist_queue_.clear(); gyro_queue_.clear(); @@ -166,8 +164,8 @@ void GyroOdometerNode::concatGyroAndOdometer() } // check queue size - diagnostics_->addKeyValue("vehicle_twist_queue_size", vehicle_twist_queue_.size()); - diagnostics_->addKeyValue("imu_queue_size", gyro_queue_.size()); + diagnostics_->add_key_value("vehicle_twist_queue_size", vehicle_twist_queue_.size()); + diagnostics_->add_key_value("imu_queue_size", gyro_queue_.size()); if (vehicle_twist_queue_.empty()) { // not output error and clear queue return; @@ -182,13 +180,13 @@ void GyroOdometerNode::concatGyroAndOdometer() transform_listener_->getLatestTransform(gyro_queue_.front().header.frame_id, output_frame_); const bool is_succeed_transform_imu = (tf_imu2base_ptr != nullptr); - diagnostics_->addKeyValue("is_succeed_transform_imu", is_succeed_transform_imu); + diagnostics_->add_key_value("is_succeed_transform_imu", is_succeed_transform_imu); if (!is_succeed_transform_imu) { std::stringstream message; message << "Please publish TF " << output_frame_ << " to " << gyro_queue_.front().header.frame_id; RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); - diagnostics_->updateLevelAndMessage( + diagnostics_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); vehicle_twist_queue_.clear(); @@ -208,7 +206,7 @@ void GyroOdometerNode::concatGyroAndOdometer() gyro.header.frame_id = output_frame_; gyro.angular_velocity = transformed_angular_velocity.vector; - gyro.angular_velocity_covariance = transformCovariance(gyro.angular_velocity_covariance); + gyro.angular_velocity_covariance = transform_covariance(gyro.angular_velocity_covariance); } using COV_IDX_XYZ = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; @@ -223,8 +221,8 @@ void GyroOdometerNode::concatGyroAndOdometer() vx_mean += vehicle_twist.twist.twist.linear.x; vx_covariance_original += vehicle_twist.twist.covariance[0 * 6 + 0]; } - vx_mean /= vehicle_twist_queue_.size(); - vx_covariance_original /= vehicle_twist_queue_.size(); + vx_mean /= static_cast(vehicle_twist_queue_.size()); + vx_covariance_original /= static_cast(vehicle_twist_queue_.size()); for (const auto & gyro : gyro_queue_) { gyro_mean.x += gyro.angular_velocity.x; @@ -234,12 +232,12 @@ void GyroOdometerNode::concatGyroAndOdometer() gyro_covariance_original.y += gyro.angular_velocity_covariance[COV_IDX_XYZ::Y_Y]; gyro_covariance_original.z += gyro.angular_velocity_covariance[COV_IDX_XYZ::Z_Z]; } - gyro_mean.x /= gyro_queue_.size(); - gyro_mean.y /= gyro_queue_.size(); - gyro_mean.z /= gyro_queue_.size(); - gyro_covariance_original.x /= gyro_queue_.size(); - gyro_covariance_original.y /= gyro_queue_.size(); - gyro_covariance_original.z /= gyro_queue_.size(); + gyro_mean.x /= static_cast(gyro_queue_.size()); + gyro_mean.y /= static_cast(gyro_queue_.size()); + gyro_mean.z /= static_cast(gyro_queue_.size()); + gyro_covariance_original.x /= static_cast(gyro_queue_.size()); + gyro_covariance_original.y /= static_cast(gyro_queue_.size()); + gyro_covariance_original.z /= static_cast(gyro_queue_.size()); // concat geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov; @@ -257,23 +255,23 @@ void GyroOdometerNode::concatGyroAndOdometer() // From a statistical point of view, here we reduce the covariances according to the number of // observed data twist_with_cov.twist.covariance[COV_IDX_XYZRPY::X_X] = - vx_covariance_original / vehicle_twist_queue_.size(); + vx_covariance_original / static_cast(vehicle_twist_queue_.size()); twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Y_Y] = 100000.0; twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Z_Z] = 100000.0; twist_with_cov.twist.covariance[COV_IDX_XYZRPY::ROLL_ROLL] = - gyro_covariance_original.x / gyro_queue_.size(); + gyro_covariance_original.x / static_cast(gyro_queue_.size()); twist_with_cov.twist.covariance[COV_IDX_XYZRPY::PITCH_PITCH] = - gyro_covariance_original.y / gyro_queue_.size(); + gyro_covariance_original.y / static_cast(gyro_queue_.size()); twist_with_cov.twist.covariance[COV_IDX_XYZRPY::YAW_YAW] = - gyro_covariance_original.z / gyro_queue_.size(); + gyro_covariance_original.z / static_cast(gyro_queue_.size()); - publishData(twist_with_cov); + publish_data(twist_with_cov); vehicle_twist_queue_.clear(); gyro_queue_.clear(); } -void GyroOdometerNode::publishData( +void GyroOdometerNode::publish_data( const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw) { geometry_msgs::msg::TwistStamped twist_raw; diff --git a/localization/gyro_odometer/test/test_gyro_odometer_helper.cpp b/localization/gyro_odometer/test/test_gyro_odometer_helper.cpp index 54e1d320319d3..f55278998f5f0 100644 --- a/localization/gyro_odometer/test/test_gyro_odometer_helper.cpp +++ b/localization/gyro_odometer/test/test_gyro_odometer_helper.cpp @@ -17,7 +17,7 @@ using geometry_msgs::msg::TwistWithCovarianceStamped; using sensor_msgs::msg::Imu; -Imu generateSampleImu() +Imu generate_sample_imu() { Imu imu; imu.header.frame_id = "base_link"; @@ -27,7 +27,7 @@ Imu generateSampleImu() return imu; } -TwistWithCovarianceStamped generateSampleVelocity() +TwistWithCovarianceStamped generate_sample_velocity() { TwistWithCovarianceStamped twist; twist.header.frame_id = "base_link"; @@ -35,7 +35,7 @@ TwistWithCovarianceStamped generateSampleVelocity() return twist; } -rclcpp::NodeOptions getNodeOptionsWithDefaultParams() +rclcpp::NodeOptions get_node_options_with_default_params() { rclcpp::NodeOptions node_options; diff --git a/localization/gyro_odometer/test/test_gyro_odometer_helper.hpp b/localization/gyro_odometer/test/test_gyro_odometer_helper.hpp index 6e3aff0b841a9..9da344e410e61 100644 --- a/localization/gyro_odometer/test/test_gyro_odometer_helper.hpp +++ b/localization/gyro_odometer/test/test_gyro_odometer_helper.hpp @@ -20,11 +20,8 @@ #include #include -using geometry_msgs::msg::TwistWithCovarianceStamped; -using sensor_msgs::msg::Imu; - -Imu generateSampleImu(); -TwistWithCovarianceStamped generateSampleVelocity(); -rclcpp::NodeOptions getNodeOptionsWithDefaultParams(); +sensor_msgs::msg::Imu generate_sample_imu(); +geometry_msgs::msg::TwistWithCovarianceStamped generate_sample_velocity(); +rclcpp::NodeOptions get_node_options_with_default_params(); #endif // TEST_GYRO_ODOMETER_HELPER_HPP_ diff --git a/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp b/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp index b7849ef03bfc5..f72499a28a947 100644 --- a/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp +++ b/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp @@ -50,19 +50,21 @@ class VelocityGenerator : public rclcpp::Node class GyroOdometerValidator : public rclcpp::Node { public: - GyroOdometerValidator() : Node("gyro_odometer_validator"), received_latest_twist_ptr(nullptr) - { - twist_sub = create_subscription( - "/twist_with_covariance", 1, [this](const TwistWithCovarianceStamped::ConstSharedPtr msg) { + GyroOdometerValidator() : Node("gyro_odometer_validator"), + twist_sub(create_subscription("/twist_with_covariance", 1, + [this](const TwistWithCovarianceStamped::ConstSharedPtr msg) { received_latest_twist_ptr = msg; - }); + }) + ), + received_latest_twist_ptr(nullptr) + { } rclcpp::Subscription::SharedPtr twist_sub; TwistWithCovarianceStamped::ConstSharedPtr received_latest_twist_ptr; }; -void spinSome(rclcpp::Node::SharedPtr node_ptr) +void wait_spin_some(rclcpp::Node::SharedPtr node_ptr) { for (int i = 0; i < 50; ++i) { rclcpp::spin_some(node_ptr); @@ -70,7 +72,7 @@ void spinSome(rclcpp::Node::SharedPtr node_ptr) } } -bool isTwistValid( +bool is_twist_valid( const TwistWithCovarianceStamped & twist, const TwistWithCovarianceStamped & twist_ground_truth) { if (twist.twist.twist.linear.x != twist_ground_truth.twist.twist.linear.x) { @@ -99,8 +101,8 @@ bool isTwistValid( // velocity data are provided TEST(GyroOdometer, TestGyroOdometerWithImuAndVelocity) { - Imu input_imu = generateSampleImu(); - TwistWithCovarianceStamped input_velocity = generateSampleVelocity(); + Imu input_imu = generate_sample_imu(); + TwistWithCovarianceStamped input_velocity = generate_sample_velocity(); TwistWithCovarianceStamped expected_output_twist; expected_output_twist.twist.twist.linear.x = input_velocity.twist.twist.linear.x; @@ -109,7 +111,9 @@ TEST(GyroOdometer, TestGyroOdometerWithImuAndVelocity) expected_output_twist.twist.twist.angular.z = input_imu.angular_velocity.z; auto gyro_odometer_node = - std::make_shared(getNodeOptionsWithDefaultParams()); + std::make_shared( + get_node_options_with_default_params() + ); auto imu_generator = std::make_shared(); auto velocity_generator = std::make_shared(); auto gyro_odometer_validator_node = std::make_shared(); @@ -120,13 +124,13 @@ TEST(GyroOdometer, TestGyroOdometerWithImuAndVelocity) velocity_generator->vehicle_velocity_pub->publish(input_velocity); // gyro_odometer receives IMU and velocity, and publishes the fused twist data. - spinSome(gyro_odometer_node); + wait_spin_some(gyro_odometer_node); // validator node receives the fused twist data and store in "received_latest_twist_ptr". - spinSome(gyro_odometer_validator_node); + wait_spin_some(gyro_odometer_validator_node); EXPECT_FALSE(gyro_odometer_validator_node->received_latest_twist_ptr == nullptr); - EXPECT_TRUE(isTwistValid( + EXPECT_TRUE(is_twist_valid( *(gyro_odometer_validator_node->received_latest_twist_ptr), expected_output_twist)); } @@ -134,20 +138,22 @@ TEST(GyroOdometer, TestGyroOdometerWithImuAndVelocity) // Verify that the gyro_odometer does NOT publish any outputs when only IMU is provided TEST(GyroOdometer, TestGyroOdometerImuOnly) { - Imu input_imu = generateSampleImu(); + Imu input_imu = generate_sample_imu(); auto gyro_odometer_node = - std::make_shared(getNodeOptionsWithDefaultParams()); + std::make_shared( + get_node_options_with_default_params() + ); auto imu_generator = std::make_shared(); auto gyro_odometer_validator_node = std::make_shared(); imu_generator->imu_pub->publish(input_imu); // gyro_odometer receives IMU - spinSome(gyro_odometer_node); + wait_spin_some(gyro_odometer_node); // validator node waits for the output fused twist from gyro_odometer - spinSome(gyro_odometer_validator_node); + wait_spin_some(gyro_odometer_validator_node); EXPECT_TRUE(gyro_odometer_validator_node->received_latest_twist_ptr == nullptr); }