diff --git a/localization/gyro_odometer/README.md b/localization/gyro_odometer/README.md index 40e7c67d60a87..aa6f2a96f4838 100644 --- a/localization/gyro_odometer/README.md +++ b/localization/gyro_odometer/README.md @@ -39,13 +39,13 @@ drawing -| 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 | 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 61309ed3ac28e..883c06f1363f0 100644 --- a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp +++ b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp @@ -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" diff --git a/localization/gyro_odometer/package.xml b/localization/gyro_odometer/package.xml index 1b5b9ea8805a2..575f6a2d74837 100644 --- a/localization/gyro_odometer/package.xml +++ b/localization/gyro_odometer/package.xml @@ -17,8 +17,8 @@ ament_cmake_auto autoware_cmake - fmt diagnostic_msgs + fmt geometry_msgs rclcpp rclcpp_components diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp index 423e9c983a4ea..8fe978f8d6ec0 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -25,8 +25,8 @@ #include #include -#include #include +#include namespace autoware::gyro_odometer { @@ -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(vehicle_twist_ptr->header.stamp).nanoseconds()); + diagnostics_->addKeyValue( + "topic_time_stamp", static_cast(vehicle_twist_ptr->header.stamp).nanoseconds()); vehicle_twist_arrived_ = true; latest_vehicle_twist_rostime_ = vehicle_twist_ptr->header.stamp; @@ -94,11 +95,11 @@ void GyroOdometerNode::callbackVehicleTwist( diagnostics_->publish(); } - void GyroOdometerNode::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) { diagnostics_->clear(); - diagnostics_->addKeyValue("topic_time_stamp", static_cast(imu_msg_ptr->header.stamp).nanoseconds()); + diagnostics_->addKeyValue( + "topic_time_stamp", static_cast(imu_msg_ptr->header.stamp).nanoseconds()); imu_arrived_ = true; latest_imu_rostime_ = imu_msg_ptr->header.stamp; @@ -108,7 +109,6 @@ void GyroOdometerNode::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr i diagnostics_->publish(); } - void GyroOdometerNode::concatGyroAndOdometer() { // check arrive first topic @@ -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(); @@ -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(); @@ -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); @@ -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(); @@ -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;