Skip to content

Commit

Permalink
Merge pull request #150 from tier4/fix/deviation_estimator_fix_by_linter
Browse files Browse the repository at this point in the history
fix(deviation_estimator): fixed as pointed out by linter
  • Loading branch information
SakodaShintaro authored Jan 22, 2024
2 parents df312f4 + 838e882 commit 75f0371
Show file tree
Hide file tree
Showing 4 changed files with 13 additions and 9 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,8 @@
#include <fmt/core.h>

#include <limits>
#include <map>
#include <string>
#include <utility>
#include <vector>

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ class DeviationEvaluator : public rclcpp::Node
{
double lateral;
double long_radius;
Errors() : lateral(0), long_radius(0){};
Errors() : lateral(0), long_radius(0) {}
};

public:
Expand Down Expand Up @@ -89,6 +89,7 @@ class DeviationEvaluator : public rclcpp::Node

void callbackNDTPoseWithCovariance(const PoseWithCovarianceStamped::SharedPtr msg);

// cspell:words EKFDR, EKFGT
void callbackEKFDROdom(const Odometry::SharedPtr msg);

void callbackEKFGTOdom(const Odometry::SharedPtr msg);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,15 +15,14 @@
# See the License for the specific language governing permissions and
# limitations under the License.

import os
# cspell:words lowerbound

from pathlib import Path
from threading import Thread

from bag_load_utils import BagFileEvaluator
import matplotlib.pyplot as plt
import numpy as np
from plot_utils import plot_bag_compare
from plot_utils import plot_thresholds
import rclpy
from rclpy.node import Node

Expand All @@ -41,15 +40,15 @@ def validate_threshold(
recall: float, threshold: float, lowerbound: float, recall_threshold: float = 0.99
):
if threshold < lowerbound:
print(f"Threshold is too small for this vehicle.")
print("Threshold is too small for this vehicle.")
elif recall == np.inf:
print(
f"No error larger than {threshold:.3f} [m] observed. Provide more data on deviation_evaluator."
)
elif recall > recall_threshold:
print("Valid threshold!")
else:
print(f"Covariance seems to be too optimistic.")
print("Covariance seems to be too optimistic.")
print("Consider increasing the covariances of the dead reckoning sensors.")

if threshold > lowerbound:
Expand All @@ -58,7 +57,7 @@ def validate_threshold(
print(f" [NG] lowerbound = {lowerbound:.3f} > {threshold:.3f} = threshold [m]")

if recall == np.inf:
print(f" [NG] recall = INF")
print(f" [NG] recall = {recall}")
elif recall > recall_threshold:
print(f" [OK] recall = {recall:.3f} > {recall_threshold:.3f}")
else:
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.

// cspell:words EKFDR, EKFGT

#include "deviation_evaluator/deviation_evaluator.hpp"

#include "rclcpp/logging.hpp"
Expand All @@ -31,7 +33,7 @@
// clang-format on
using std::placeholders::_1;

using namespace std::chrono_literals;
using std::chrono_literals::operator""s;

double double_round(const double x, const int n) { return std::round(x * pow(10, n)) / pow(10, n); }

Expand Down Expand Up @@ -154,7 +156,7 @@ void DeviationEvaluator::callbackEKFDROdom(const Odometry::SharedPtr msg)
rclcpp::Time(dr_pose_queue_.back()->header.stamp).seconds() >
rclcpp::Time(pose_ptr->header.stamp).seconds()) {
dr_pose_queue_.clear();
RCLCPP_ERROR_STREAM(this->get_logger(), "Timestampe jump detected!");
RCLCPP_ERROR_STREAM(this->get_logger(), "Timestamp jump detected!");
}
}
dr_pose_queue_.push_back(pose_ptr);
Expand Down

0 comments on commit 75f0371

Please sign in to comment.