Skip to content

Commit

Permalink
style(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
pre-commit-ci[bot] committed May 31, 2024
1 parent dd1423b commit b9fb1f6
Show file tree
Hide file tree
Showing 4 changed files with 27 additions and 24 deletions.
20 changes: 10 additions & 10 deletions localization/gyro_odometer/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -39,13 +39,13 @@

<img src="./media/diagnostic.png" alt="drawing" width="600"/>

| Name | Description | Transition condition to Warning | Transition condition to Error |
| ------------------------- | -------------------------------------------------- | ------------------------------- | ----------------------------- |
| `topic_time_stamp` | the time stamp of service calling. [nano second] | none | none |
| `is_arrived_first_vehicle_twist` | whether the vehicle twist topic has been received even once. | not arrive yet | none |
| `is_arrived_first_imu` | whether the imu topic has been received even once. | not arrive yet | none |
| `vehicle_twist_time_stamp_dt` | the time difference between the current time and the latest vehicle twist topic. [second] | none | the time is **longer** than `message_timeout_sec` |
| `imu_time_stamp_dt` | the time difference between the current time and the latest imu topic. [second] | none | the time is **longer** than `message_timeout_sec` |
| `vehicle_twist_queue_size` | the size of vehicle_twist_queue. | none | none |
| `imu_queue_size` | the size of gyro_queue. | none | none |
| `is_succeed_transform_imu` | whether transform imu is succeed or not. | none | failed |
| Name | Description | Transition condition to Warning | Transition condition to Error |
| -------------------------------- | ----------------------------------------------------------------------------------------- | ------------------------------- | ------------------------------------------------- |
| `topic_time_stamp` | the time stamp of service calling. [nano second] | none | none |
| `is_arrived_first_vehicle_twist` | whether the vehicle twist topic has been received even once. | not arrive yet | none |
| `is_arrived_first_imu` | whether the imu topic has been received even once. | not arrive yet | none |
| `vehicle_twist_time_stamp_dt` | the time difference between the current time and the latest vehicle twist topic. [second] | none | the time is **longer** than `message_timeout_sec` |
| `imu_time_stamp_dt` | the time difference between the current time and the latest imu topic. [second] | none | the time is **longer** than `message_timeout_sec` |
| `vehicle_twist_queue_size` | the size of vehicle_twist_queue. | none | none |
| `imu_queue_size` | the size of gyro_queue. | none | none |
| `is_succeed_transform_imu` | whether transform imu is succeed or not. | none | failed |
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,6 @@
#define GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_

#include "gyro_odometer/diagnostics_module.hpp"

#include "tier4_autoware_utils/ros/logger_level_configure.hpp"
#include "tier4_autoware_utils/ros/msg_covariance.hpp"
#include "tier4_autoware_utils/ros/transform_listener.hpp"
Expand Down
2 changes: 1 addition & 1 deletion localization/gyro_odometer/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -17,8 +17,8 @@
<buildtool_depend>ament_cmake_auto</buildtool_depend>
<buildtool_depend>autoware_cmake</buildtool_depend>

<depend>fmt</depend>
<depend>diagnostic_msgs</depend>
<depend>fmt</depend>
<depend>geometry_msgs</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
Expand Down
28 changes: 16 additions & 12 deletions localization/gyro_odometer/src/gyro_odometer_core.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -25,8 +25,8 @@

#include <cmath>
#include <memory>
#include <string>
#include <sstream>
#include <string>

namespace autoware::gyro_odometer
{
Expand Down Expand Up @@ -84,7 +84,8 @@ void GyroOdometerNode::callbackVehicleTwist(
const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_ptr)
{
diagnostics_->clear();
diagnostics_->addKeyValue("topic_time_stamp", static_cast<rclcpp::Time>(vehicle_twist_ptr->header.stamp).nanoseconds());
diagnostics_->addKeyValue(
"topic_time_stamp", static_cast<rclcpp::Time>(vehicle_twist_ptr->header.stamp).nanoseconds());

vehicle_twist_arrived_ = true;
latest_vehicle_twist_rostime_ = vehicle_twist_ptr->header.stamp;
Expand All @@ -94,11 +95,11 @@ void GyroOdometerNode::callbackVehicleTwist(
diagnostics_->publish();

Check warning on line 95 in localization/gyro_odometer/src/gyro_odometer_core.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Code Duplication

The module contains 2 functions with similar structure: GyroOdometerNode::callbackImu,GyroOdometerNode::callbackVehicleTwist. Avoid duplicated, aka copy-pasted, code inside the module. More duplication lowers the code health.
}


void GyroOdometerNode::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr)
{
diagnostics_->clear();
diagnostics_->addKeyValue("topic_time_stamp", static_cast<rclcpp::Time>(imu_msg_ptr->header.stamp).nanoseconds());
diagnostics_->addKeyValue(
"topic_time_stamp", static_cast<rclcpp::Time>(imu_msg_ptr->header.stamp).nanoseconds());

imu_arrived_ = true;
latest_imu_rostime_ = imu_msg_ptr->header.stamp;
Expand All @@ -108,7 +109,6 @@ void GyroOdometerNode::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr i
diagnostics_->publish();
}


void GyroOdometerNode::concatGyroAndOdometer()
{
// check arrive first topic
Expand All @@ -118,7 +118,8 @@ void GyroOdometerNode::concatGyroAndOdometer()
std::stringstream message;
message << "Twist msg is not subscribed";
RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str());
diagnostics_->updateLevelAndMessage(diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
diagnostics_->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());

vehicle_twist_queue_.clear();
gyro_queue_.clear();
Expand All @@ -128,7 +129,8 @@ 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(diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
diagnostics_->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());

vehicle_twist_queue_.clear();
gyro_queue_.clear();
Expand All @@ -142,7 +144,8 @@ void GyroOdometerNode::concatGyroAndOdometer()
diagnostics_->addKeyValue("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_);
"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);

Expand Down Expand Up @@ -181,9 +184,11 @@ void GyroOdometerNode::concatGyroAndOdometer()
diagnostics_->addKeyValue("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;
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(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str());
diagnostics_->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str());

vehicle_twist_queue_.clear();
gyro_queue_.clear();
Expand All @@ -202,8 +207,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 = transformCovariance(gyro.angular_velocity_covariance);
}

using COV_IDX_XYZ = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX;
Expand Down

0 comments on commit b9fb1f6

Please sign in to comment.