Skip to content

Commit

Permalink
fix units of time objects (autowarefoundation#195)
Browse files Browse the repository at this point in the history
* fix units of time objects

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* fix tests

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* fix test

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>
  • Loading branch information
mitsudome-r authored Dec 21, 2020
1 parent d52efda commit a433049
Show file tree
Hide file tree
Showing 5 changed files with 14 additions and 7 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -183,7 +183,7 @@ void VelocityController::blockUntilVehiclePositionAvailable(const tf2::Duration
{
RCLCPP_INFO(
get_logger(), "waiting %d ms for %s->%s transform to become available",
std::chrono::duration_cast<std::chrono::seconds>(duration).count(), input, output);
std::chrono::duration_cast<std::chrono::milliseconds>(duration).count(), input, output);
rclcpp::sleep_for(duration);
}
RCLCPP_INFO(get_logger(), "transform available");
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -84,15 +84,15 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti

/* initialize ros system */
auto timer_control_callback = std::bind(&EKFLocalizer::timerCallback, this);
auto period_control = std::chrono::duration_cast<std::chrono::seconds>(
auto period_control = std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(ekf_dt_));
timer_control_ = std::make_shared<rclcpp::GenericTimer<decltype(timer_control_callback)>>(
this->get_clock(), period_control, std::move(timer_control_callback),
this->get_node_base_interface()->get_context());
this->get_node_timers_interface()->add_timer(timer_control_, nullptr);

auto timer_tf_callback = std::bind(&EKFLocalizer::timerTFCallback, this);
auto period_tf = std::chrono::duration_cast<std::chrono::seconds>(
auto period_tf = std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(1.0 / tf_rate_));
timer_tf_ = std::make_shared<rclcpp::GenericTimer<decltype(timer_tf_callback)>>(
this->get_clock(), period_tf, std::move(timer_tf_callback),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,10 @@ ament_auto_add_executable(motion_velocity_optimizer

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# ignore python
set(ament_cmake_flake8_FOUND TRUE)
set(ament_cmake_pyflake_FOUND TRUE)

ament_lint_auto_find_test_dependencies()
endif()

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -133,7 +133,7 @@ MotionVelocityOptimizer::MotionVelocityOptimizer()
{
external_velocity_limit_update_rate_ = get_parameter("over_a_weight").as_double();
auto timer_callback = std::bind(&MotionVelocityOptimizer::timerCallback, this);
auto period = std::chrono::duration_cast<std::chrono::seconds>(
auto period = std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(1.0 / external_velocity_limit_update_rate_));
timer_ = std::make_shared<rclcpp::GenericTimer<decltype(timer_callback)>>(
get_clock(), period, std::move(timer_callback), get_node_base_interface()->get_context());
Expand Down Expand Up @@ -742,7 +742,8 @@ void MotionVelocityOptimizer::blockUntilVehiclePositionAvailable(const tf2::Dura
{
RCLCPP_INFO(
get_logger(), "waiting %d ms for %s->%s transform to become available",
std::chrono::duration_cast<std::chrono::seconds>(duration).count(), input, output);
std::chrono::duration_cast<std::chrono::milliseconds>(duration).count(),
input, output);
rclcpp::sleep_for(duration);
}
RCLCPP_INFO(get_logger(), "transform available");
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -172,13 +172,15 @@ void LaneChanger::run()
{
// wait until mandatory data is ready
if (!route_handler_ptr_->isHandlerReady()) {
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5.0, "waiting for route to be ready");
RCLCPP_WARN_THROTTLE(
get_logger(), *get_clock(), 5000 /* ms */,
"waiting for route to be ready");
return;
}
if (!data_manager_ptr_->isDataReady()) {
RCLCPP_WARN_THROTTLE(
get_logger(),
*get_clock(), 5.0, "waiting for vehicle pose, vehicle_velocity, and obstacles");
*get_clock(), 5000 /* ms */, "waiting for vehicle pose, vehicle_velocity, and obstacles");
return;
}

Expand Down

0 comments on commit a433049

Please sign in to comment.