Skip to content

Commit

Permalink
refactor(ekf_localizer): simplify duration conversion (#2017)
Browse files Browse the repository at this point in the history
  • Loading branch information
IshitaTakeshi authored Oct 13, 2022
1 parent 48135dc commit 4be6931
Showing 1 changed file with 5 additions and 5 deletions.
10 changes: 5 additions & 5 deletions localization/ekf_localizer/src/ekf_localizer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
#include "ekf_localizer/state_transition.hpp"
#include "ekf_localizer/warning.hpp"

#include <rclcpp/duration.hpp>
#include <rclcpp/logging.hpp>
#include <tier4_autoware_utils/math/unit_conversion.hpp>
#include <tier4_autoware_utils/ros/msg_covariance.hpp>
Expand Down Expand Up @@ -80,14 +81,13 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti
is_initialized_ = false;

/* initialize ros system */
auto period_control_ns =
std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(ekf_dt_));
timer_control_ = rclcpp::create_timer(
this, get_clock(), period_control_ns, std::bind(&EKFLocalizer::timerCallback, this));
this, get_clock(), rclcpp::Duration::from_seconds(ekf_dt_),
std::bind(&EKFLocalizer::timerCallback, this));

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

pub_pose_ = create_publisher<geometry_msgs::msg::PoseStamped>("ekf_pose", 1);
pub_pose_cov_ =
Expand Down

0 comments on commit 4be6931

Please sign in to comment.