Skip to content

Commit

Permalink
Remove clock_
Browse files Browse the repository at this point in the history 
Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
  • Loading branch information
@kosuke55
kosuke55 committed Feb 19, 2022
1 parent abc283b commit ddc2f38
Show file tree
Hide file tree
Showing 2 changed files with 13 additions and 17 deletions.
2 changes: 0 additions & 2 deletions localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,8 +76,6 @@ class EKFLocalizer : public rclcpp::Node
//!< @brief measurement twist with covariance subscriber
rclcpp::Subscription<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr
sub_twist_with_cov_;
//!< @brief clock
rclcpp::Clock::SharedPtr clock_;
//!< @brief time for ekf calculation callback
rclcpp::TimerBase::SharedPtr timer_control_;
//!< @brief last predict time
Expand Down
28 changes: 13 additions & 15 deletions localization/ekf_localizer/src/ekf_localizer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,9 +33,7 @@
using std::placeholders::_1;

EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOptions & node_options)
: rclcpp::Node(node_name, node_options),
dim_x_(6 /* x, y, yaw, yaw_bias, vx, wz */),
clock_(get_clock())
: rclcpp::Node(node_name, node_options), dim_x_(6 /* x, y, yaw, yaw_bias, vx, wz */)
{
show_debug_info_ = declare_parameter("show_debug_info", false);
ekf_rate_ = declare_parameter("predict_frequency", 50.0);
Expand Down Expand Up @@ -75,11 +73,11 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti
auto period_control_ns =
std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(ekf_dt_));
timer_control_ = rclcpp::create_timer(
this, clock_, period_control_ns, std::bind(&EKFLocalizer::timerCallback, this));
this, get_clock(), period_control_ns, std::bind(&EKFLocalizer::timerCallback, this));

const auto period_tf_ns = rclcpp::Rate(tf_rate_).period();
timer_tf_ = rclcpp::create_timer(
this, clock_, period_tf_ns, std::bind(&EKFLocalizer::timerTFCallback, this));
this, get_clock(), period_tf_ns, std::bind(&EKFLocalizer::timerTFCallback, this));

pub_pose_ = create_publisher<geometry_msgs::msg::PoseStamped>("ekf_pose", 1);
pub_pose_cov_ =
Expand Down Expand Up @@ -118,7 +116,7 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti
void EKFLocalizer::updatePredictFrequency()
{
if (last_predict_time_) {
ekf_rate_ = 1.0 / (clock_->now() - *last_predict_time_).seconds();
ekf_rate_ = 1.0 / (get_clock()->now() - *last_predict_time_).seconds();
DEBUG_INFO(get_logger(), "[EKF] update ekf_rate_ to %f hz", ekf_rate_);
ekf_dt_ = 1.0 / std::max(ekf_rate_, 0.1);

Expand All @@ -128,7 +126,7 @@ void EKFLocalizer::updatePredictFrequency()
proc_cov_yaw_d_ = std::pow(proc_stddev_yaw_c_ * ekf_dt_, 2.0);
proc_cov_yaw_bias_d_ = std::pow(proc_stddev_yaw_bias_c_ * ekf_dt_, 2.0);
}
last_predict_time_ = std::make_shared<const rclcpp::Time>(clock_->now());
last_predict_time_ = std::make_shared<const rclcpp::Time>(get_clock()->now());
}

/*
Expand Down Expand Up @@ -435,7 +433,7 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseStamped &
{
if (pose.header.frame_id != pose_frame_id_) {
RCLCPP_WARN_THROTTLE(
get_logger(), *clock_, std::chrono::milliseconds(2000).count(),
get_logger(), *get_clock(), std::chrono::milliseconds(2000).count(),
"pose frame_id is %s, but pose_frame is set as %s. They must be same.",
pose.header.frame_id.c_str(), pose_frame_id_.c_str());
}
Expand All @@ -451,13 +449,13 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseStamped &
if (delay_time < 0.0) {
delay_time = 0.0;
RCLCPP_WARN_THROTTLE(
get_logger(), *clock_, std::chrono::milliseconds(1000).count(),
get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(),
"Pose time stamp is inappropriate, set delay to 0[s]. delay = %f", delay_time);
}
int delay_step = std::roundf(delay_time / ekf_dt_);
if (delay_step > extend_state_step_ - 1) {
RCLCPP_WARN_THROTTLE(
get_logger(), *clock_, std::chrono::milliseconds(1000).count(),
get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(),
"Pose delay exceeds the compensation limit, ignored. delay: %f[s], limit = "
"extend_state_step * ekf_dt : %f [s]",
delay_time, extend_state_step_ * ekf_dt_);
Expand Down Expand Up @@ -491,7 +489,7 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseStamped &
P_y = P_curr.block(0, 0, dim_y, dim_y);
if (!mahalanobisGate(pose_gate_dist_, y_ekf, y, P_y)) {
RCLCPP_WARN_THROTTLE(
get_logger(), *clock_, std::chrono::milliseconds(2000).count(),
get_logger(), *get_clock(), std::chrono::milliseconds(2000).count(),
"[EKF] Pose measurement update, mahalanobis distance is over limit. ignore "
"measurement data.");
return;
Expand Down Expand Up @@ -539,7 +537,7 @@ void EKFLocalizer::measurementUpdateTwist(const geometry_msgs::msg::TwistStamped
{
if (twist.header.frame_id != "base_link") {
RCLCPP_WARN_THROTTLE(
get_logger(), *clock_, std::chrono::milliseconds(2000).count(),
get_logger(), *get_clock(), std::chrono::milliseconds(2000).count(),
"twist frame_id must be base_link");
}

Expand All @@ -554,14 +552,14 @@ void EKFLocalizer::measurementUpdateTwist(const geometry_msgs::msg::TwistStamped
double delay_time = (t_curr - twist.header.stamp).seconds() + twist_additional_delay_;
if (delay_time < 0.0) {
RCLCPP_WARN_THROTTLE(
get_logger(), *clock_, std::chrono::milliseconds(1000).count(),
get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(),
"Twist time stamp is inappropriate (delay = %f [s]), set delay to 0[s].", delay_time);
delay_time = 0.0;
}
int delay_step = std::roundf(delay_time / ekf_dt_);
if (delay_step > extend_state_step_ - 1) {
RCLCPP_WARN_THROTTLE(
get_logger(), *clock_, std::chrono::milliseconds(1000).count(),
get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(),
"Twist delay exceeds the compensation limit, ignored. delay: %f[s], limit = "
"extend_state_step * ekf_dt : %f [s]",
delay_time, extend_state_step_ * ekf_dt_);
Expand Down Expand Up @@ -589,7 +587,7 @@ void EKFLocalizer::measurementUpdateTwist(const geometry_msgs::msg::TwistStamped
P_y = P_curr.block(4, 4, dim_y, dim_y);
if (!mahalanobisGate(twist_gate_dist_, y_ekf, y, P_y)) {
RCLCPP_WARN_THROTTLE(
get_logger(), *clock_, std::chrono::milliseconds(2000).count(),
get_logger(), *get_clock(), std::chrono::milliseconds(2000).count(),
"[EKF] Twist measurement update, mahalanobis distance is over limit. ignore "
"measurement data.");
return;
Expand Down

0 comments on commit ddc2f38

Please sign in to comment.