diff --git a/localization/gyro_odometer/CMakeLists.txt b/localization/gyro_odometer/CMakeLists.txt index a832383ff4761..1b142b254d2e1 100644 --- a/localization/gyro_odometer/CMakeLists.txt +++ b/localization/gyro_odometer/CMakeLists.txt @@ -6,6 +6,7 @@ autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED src/gyro_odometer_core.cpp + src/diagnostics_module.cpp ) target_link_libraries(${PROJECT_NAME} fmt) diff --git a/localization/gyro_odometer/README.md b/localization/gyro_odometer/README.md index e9e390010f084..40e7c67d60a87 100644 --- a/localization/gyro_odometer/README.md +++ b/localization/gyro_odometer/README.md @@ -34,3 +34,18 @@ - [Limitation] The frequency of the output messages depends on the frequency of the input IMU message. - [Limitation] We cannot produce reliable values for the lateral and vertical velocities. Therefore we assign large values to the corresponding elements in the output covariance matrix. + +## Diagnostics + +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 | diff --git a/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp b/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp new file mode 100644 index 0000000000000..f9da127803809 --- /dev/null +++ b/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp @@ -0,0 +1,64 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GYRO_ODOMETER__DIAGNOSTICS_MODULE_HPP_ +#define GYRO_ODOMETER__DIAGNOSTICS_MODULE_HPP_ + +#include + +#include + +#include +#include + +namespace autoware::gyro_odometer +{ + +class DiagnosticsModule +{ +public: + DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name); + void clear(); + void addKeyValue(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 publish(); + +private: + diagnostic_msgs::msg::DiagnosticArray createDiagnosticsArray() const; + + rclcpp::Clock::SharedPtr clock_; + rclcpp::Publisher::SharedPtr diagnostics_pub_; + + diagnostic_msgs::msg::DiagnosticStatus diagnostics_status_msg_; +}; + +template +void DiagnosticsModule::addKeyValue(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); +} + +template <> +void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value); +template <> +void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value); + +} // namespace autoware::gyro_odometer + +#endif // GYRO_ODOMETER__DIAGNOSTICS_MODULE_HPP_ 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 2926589bd2da9..61309ed3ac28e 100644 --- a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp +++ b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp @@ -15,6 +15,8 @@ #ifndef GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ #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" @@ -52,6 +54,7 @@ class GyroOdometerNode : public rclcpp::Node void callbackVehicleTwist( 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); rclcpp::Subscription::SharedPtr @@ -74,8 +77,12 @@ class GyroOdometerNode : public rclcpp::Node bool vehicle_twist_arrived_; bool imu_arrived_; + rclcpp::Time latest_vehicle_twist_rostime_; + rclcpp::Time latest_imu_rostime_; std::deque vehicle_twist_queue_; std::deque gyro_queue_; + + std::unique_ptr diagnostics_; }; } // namespace autoware::gyro_odometer diff --git a/localization/gyro_odometer/media/diagnostic.png b/localization/gyro_odometer/media/diagnostic.png new file mode 100644 index 0000000000000..f2324f4079d0d Binary files /dev/null and b/localization/gyro_odometer/media/diagnostic.png differ diff --git a/localization/gyro_odometer/package.xml b/localization/gyro_odometer/package.xml index 3c979eb69ac8a..1b5b9ea8805a2 100644 --- a/localization/gyro_odometer/package.xml +++ b/localization/gyro_odometer/package.xml @@ -18,6 +18,7 @@ autoware_cmake fmt + diagnostic_msgs geometry_msgs rclcpp rclcpp_components diff --git a/localization/gyro_odometer/src/diagnostics_module.cpp b/localization/gyro_odometer/src/diagnostics_module.cpp new file mode 100644 index 0000000000000..606f54027ffdb --- /dev/null +++ b/localization/gyro_odometer/src/diagnostics_module.cpp @@ -0,0 +1,109 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gyro_odometer/diagnostics_module.hpp" + +#include + +#include + +#include +#include + +namespace autoware::gyro_odometer +{ + +DiagnosticsModule::DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name) +: clock_(node->get_clock()) +{ + diagnostics_pub_ = + node->create_publisher("/diagnostics", 10); + + diagnostics_status_msg_.name = + std::string(node->get_name()) + std::string(": ") + diagnostic_name; + diagnostics_status_msg_.hardware_id = node->get_name(); +} + +void DiagnosticsModule::clear() +{ + diagnostics_status_msg_.values.clear(); + diagnostics_status_msg_.values.shrink_to_fit(); + + diagnostics_status_msg_.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + diagnostics_status_msg_.message = ""; +} + +void DiagnosticsModule::addKeyValue(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), + [key_value_msg](const auto & arg) { return arg.key == key_value_msg.key; }); + + if (it != std::cend(diagnostics_status_msg_.values)) { + it->value = key_value_msg.value; + } else { + diagnostics_status_msg_.values.push_back(key_value_msg); + } +} + +template <> +void DiagnosticsModule::addKeyValue(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); +} + +template <> +void DiagnosticsModule::addKeyValue(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); +} + +void DiagnosticsModule::updateLevelAndMessage(const int8_t level, const std::string & message) +{ + if ((level > diagnostic_msgs::msg::DiagnosticStatus::OK)) { + if (!diagnostics_status_msg_.message.empty()) { + diagnostics_status_msg_.message += "; "; + } + diagnostics_status_msg_.message += message; + } + if (level > diagnostics_status_msg_.level) { + diagnostics_status_msg_.level = level; + } +} + +void DiagnosticsModule::publish() +{ + diagnostics_pub_->publish(createDiagnosticsArray()); +} + +diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::createDiagnosticsArray() const +{ + diagnostic_msgs::msg::DiagnosticArray diagnostics_msg; + diagnostics_msg.header.stamp = clock_->now(); + diagnostics_msg.status.push_back(diagnostics_status_msg_); + + if (diagnostics_msg.status.at(0).level == diagnostic_msgs::msg::DiagnosticStatus::OK) { + diagnostics_msg.status.at(0).message = "OK"; + } + + return diagnostics_msg; +} + +} // namespace autoware::gyro_odometer diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp index bda7af74b8489..423e9c983a4ea 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -26,10 +26,25 @@ #include #include #include +#include namespace autoware::gyro_odometer { +std::array transformCovariance(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; + 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; +} + GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) : Node("gyro_odometer", node_options), output_frame_(declare_parameter("output_frame")), @@ -56,6 +71,8 @@ GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) twist_with_covariance_pub_ = create_publisher( "twist_with_covariance", rclcpp::QoS{10}); + diagnostics_ = std::make_unique(this, "gyro_odometer_status"); + // TODO(YamatoAndo) createTimer } @@ -63,186 +80,190 @@ GyroOdometerNode::~GyroOdometerNode() { } -std::array transformCovariance(const std::array & cov) +void GyroOdometerNode::callbackVehicleTwist( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_ptr) { - using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; + diagnostics_->clear(); + diagnostics_->addKeyValue("topic_time_stamp", static_cast(vehicle_twist_ptr->header.stamp).nanoseconds()); - double max_cov = std::max({cov[COV_IDX::X_X], cov[COV_IDX::Y_Y], cov[COV_IDX::Z_Z]}); + vehicle_twist_arrived_ = true; + latest_vehicle_twist_rostime_ = vehicle_twist_ptr->header.stamp; + vehicle_twist_queue_.push_back(*vehicle_twist_ptr); + concatGyroAndOdometer(); - std::array 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; + diagnostics_->publish(); } -geometry_msgs::msg::TwistWithCovarianceStamped concatGyroAndOdometer( - const std::deque & vehicle_twist_queue, - const std::deque & gyro_queue) -{ - using COV_IDX_XYZ = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; - using COV_IDX_XYZRPY = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - - double vx_mean = 0; - geometry_msgs::msg::Vector3 gyro_mean{}; - double vx_covariance_original = 0; - geometry_msgs::msg::Vector3 gyro_covariance_original{}; - for (const auto & vehicle_twist : vehicle_twist_queue) { - 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(); - - for (const auto & gyro : gyro_queue) { - gyro_mean.x += gyro.angular_velocity.x; - gyro_mean.y += gyro.angular_velocity.y; - gyro_mean.z += gyro.angular_velocity.z; - gyro_covariance_original.x += gyro.angular_velocity_covariance[COV_IDX_XYZ::X_X]; - 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(); - geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov; - const auto latest_vehicle_twist_stamp = rclcpp::Time(vehicle_twist_queue.back().header.stamp); - const auto latest_imu_stamp = rclcpp::Time(gyro_queue.back().header.stamp); - if (latest_vehicle_twist_stamp < latest_imu_stamp) { - twist_with_cov.header.stamp = latest_imu_stamp; - } else { - twist_with_cov.header.stamp = latest_vehicle_twist_stamp; - } - twist_with_cov.header.frame_id = gyro_queue.front().header.frame_id; - twist_with_cov.twist.twist.linear.x = vx_mean; - twist_with_cov.twist.twist.angular = gyro_mean; +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()); - // 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(); - 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(); - twist_with_cov.twist.covariance[COV_IDX_XYZRPY::PITCH_PITCH] = - gyro_covariance_original.y / gyro_queue.size(); - twist_with_cov.twist.covariance[COV_IDX_XYZRPY::YAW_YAW] = - gyro_covariance_original.z / gyro_queue.size(); + imu_arrived_ = true; + latest_imu_rostime_ = imu_msg_ptr->header.stamp; + gyro_queue_.push_back(*imu_msg_ptr); + concatGyroAndOdometer(); - return twist_with_cov; + diagnostics_->publish(); } -void GyroOdometerNode::callbackVehicleTwist( - const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_ptr) + +void GyroOdometerNode::concatGyroAndOdometer() { - vehicle_twist_arrived_ = true; - if (!imu_arrived_) { - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Imu msg is not subscribed"); + // check arrive first topic + diagnostics_->addKeyValue("is_arrived_first_vehicle_twist", vehicle_twist_arrived_); + diagnostics_->addKeyValue("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(diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + vehicle_twist_queue_.clear(); gyro_queue_.clear(); return; } + if (!imu_arrived_) { + 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()); - const double twist_dt = std::abs((this->now() - vehicle_twist_ptr->header.stamp).seconds()); - if (twist_dt > message_timeout_sec_) { - const std::string error_msg = fmt::format( - "Twist msg is timeout. twist_dt: {}[sec], tolerance {}[sec]", twist_dt, message_timeout_sec_); - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str()); vehicle_twist_queue_.clear(); gyro_queue_.clear(); return; } - vehicle_twist_queue_.push_back(*vehicle_twist_ptr); + // check timeout + const double vehicle_twist_dt = std::abs((this->now() - latest_vehicle_twist_rostime_).seconds()); + const double imu_dt = std::abs((this->now() - latest_imu_rostime_).seconds()); + diagnostics_->addKeyValue("vehicle_twist_time_stamp_dt", vehicle_twist_dt); + 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_); + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message); + diagnostics_->updateLevelAndMessage(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message); - if (gyro_queue_.empty()) return; - const double imu_dt = std::abs((this->now() - gyro_queue_.back().header.stamp).seconds()); - if (imu_dt > message_timeout_sec_) { - const std::string error_msg = fmt::format( - "Imu msg is timeout. twist_dt: {}[sec], tolerance {}[sec]", imu_dt, message_timeout_sec_); - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str()); vehicle_twist_queue_.clear(); gyro_queue_.clear(); return; } + if (imu_dt > message_timeout_sec_) { + 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); - const geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov_raw = - concatGyroAndOdometer(vehicle_twist_queue_, gyro_queue_); - publishData(twist_with_cov_raw); - vehicle_twist_queue_.clear(); - gyro_queue_.clear(); -} - -void GyroOdometerNode::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) -{ - imu_arrived_ = true; - if (!vehicle_twist_arrived_) { - RCLCPP_WARN_THROTTLE( - this->get_logger(), *this->get_clock(), 1000, "Twist msg is not subscribed"); vehicle_twist_queue_.clear(); gyro_queue_.clear(); return; } - const double imu_dt = std::abs((this->now() - imu_msg_ptr->header.stamp).seconds()); - if (imu_dt > message_timeout_sec_) { - const std::string error_msg = fmt::format( - "Imu msg is timeout. imu_dt: {}[sec], tolerance {}[sec]", imu_dt, message_timeout_sec_); - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str()); - vehicle_twist_queue_.clear(); - gyro_queue_.clear(); + // check queue size + diagnostics_->addKeyValue("vehicle_twist_queue_size", vehicle_twist_queue_.size()); + diagnostics_->addKeyValue("imu_queue_size", gyro_queue_.size()); + if (vehicle_twist_queue_.empty()) { + // not output error and clear queue + return; + } + if (gyro_queue_.empty()) { + // not output error and clear queue return; } + // get transformation geometry_msgs::msg::TransformStamped::ConstSharedPtr tf_imu2base_ptr = - transform_listener_->getLatestTransform(imu_msg_ptr->header.frame_id, output_frame_); - if (!tf_imu2base_ptr) { - RCLCPP_ERROR( - this->get_logger(), "Please publish TF %s to %s", output_frame_.c_str(), - (imu_msg_ptr->header.frame_id).c_str()); + 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); + 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(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); + vehicle_twist_queue_.clear(); gyro_queue_.clear(); return; } - geometry_msgs::msg::Vector3Stamped angular_velocity; - angular_velocity.header = imu_msg_ptr->header; - angular_velocity.vector = imu_msg_ptr->angular_velocity; - - geometry_msgs::msg::Vector3Stamped transformed_angular_velocity; - transformed_angular_velocity.header = tf_imu2base_ptr->header; - tf2::doTransform(angular_velocity, transformed_angular_velocity, *tf_imu2base_ptr); - - sensor_msgs::msg::Imu gyro_base_link; - gyro_base_link.header = imu_msg_ptr->header; - gyro_base_link.header.frame_id = output_frame_; - gyro_base_link.angular_velocity = transformed_angular_velocity.vector; - gyro_base_link.angular_velocity_covariance = - transformCovariance(imu_msg_ptr->angular_velocity_covariance); - - gyro_queue_.push_back(gyro_base_link); - - if (vehicle_twist_queue_.empty()) return; - const double twist_dt = - std::abs((this->now() - vehicle_twist_queue_.back().header.stamp).seconds()); - if (twist_dt > message_timeout_sec_) { - const std::string error_msg = fmt::format( - "Twist msg is timeout. twist_dt: {}[sec], tolerance {}[sec]", twist_dt, message_timeout_sec_); - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str()); - vehicle_twist_queue_.clear(); - gyro_queue_.clear(); - return; + // transform gyro frame + for (auto & gyro : gyro_queue_) { + geometry_msgs::msg::Vector3Stamped angular_velocity; + angular_velocity.header = gyro.header; + angular_velocity.vector = gyro.angular_velocity; + + geometry_msgs::msg::Vector3Stamped transformed_angular_velocity; + transformed_angular_velocity.header = tf_imu2base_ptr->header; + tf2::doTransform(angular_velocity, transformed_angular_velocity, *tf_imu2base_ptr); + + gyro.header.frame_id = output_frame_; + gyro.angular_velocity = transformed_angular_velocity.vector; + gyro.angular_velocity_covariance = + transformCovariance(gyro.angular_velocity_covariance); + } + + using COV_IDX_XYZ = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; + using COV_IDX_XYZRPY = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + + // calc mean, covariance + double vx_mean = 0; + geometry_msgs::msg::Vector3 gyro_mean{}; + double vx_covariance_original = 0; + geometry_msgs::msg::Vector3 gyro_covariance_original{}; + for (const auto & vehicle_twist : vehicle_twist_queue_) { + 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(); + + for (const auto & gyro : gyro_queue_) { + gyro_mean.x += gyro.angular_velocity.x; + gyro_mean.y += gyro.angular_velocity.y; + gyro_mean.z += gyro.angular_velocity.z; + gyro_covariance_original.x += gyro.angular_velocity_covariance[COV_IDX_XYZ::X_X]; + 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(); + + // concat + geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov; + const auto latest_vehicle_twist_stamp = rclcpp::Time(vehicle_twist_queue_.back().header.stamp); + const auto latest_imu_stamp = rclcpp::Time(gyro_queue_.back().header.stamp); + if (latest_vehicle_twist_stamp < latest_imu_stamp) { + twist_with_cov.header.stamp = latest_imu_stamp; + } else { + twist_with_cov.header.stamp = latest_vehicle_twist_stamp; } + twist_with_cov.header.frame_id = gyro_queue_.front().header.frame_id; + twist_with_cov.twist.twist.linear.x = vx_mean; + twist_with_cov.twist.twist.angular = gyro_mean; + + // 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(); + 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(); + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::PITCH_PITCH] = + gyro_covariance_original.y / gyro_queue_.size(); + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::YAW_YAW] = + gyro_covariance_original.z / gyro_queue_.size(); + + publishData(twist_with_cov); - const geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov_raw = - concatGyroAndOdometer(vehicle_twist_queue_, gyro_queue_); - publishData(twist_with_cov_raw); vehicle_twist_queue_.clear(); gyro_queue_.clear(); }