From 7c473cdadb71355fcdc8dd73bd4bbf40e75141ac Mon Sep 17 00:00:00 2001 From: Dan Rose Date: Tue, 20 Aug 2019 16:01:41 -0500 Subject: [PATCH] nav2_util::duration_from_seconds -> rclcpp::Duration::from_seconds (#1048) * nav2_util::duration_from_seconds -> rclcpp::Duration::from_seconds * Update nav2_costmap_2d/plugins/static_layer.cpp oops! Co-Authored-By: Carl Delsey --- nav2_amcl/src/amcl_node.cpp | 1 - nav2_costmap_2d/plugins/obstacle_layer.cpp | 3 +- nav2_costmap_2d/src/costmap_2d_ros.cpp | 3 +- .../test/integration/costmap_tester.cpp | 1 - .../dwb_controller/src/progress_checker.cpp | 3 +- .../dwb_core/src/dwb_local_planner.cpp | 3 +- .../dwb_critics/src/oscillation.cpp | 5 ++- .../src/limited_accel_generator.cpp | 3 +- .../src/standard_traj_generator.cpp | 3 +- .../nav2_util/duration_conversions.hpp | 32 ------------------- 10 files changed, 8 insertions(+), 49 deletions(-) delete mode 100644 nav2_util/include/nav2_util/duration_conversions.hpp diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 0a6e356b02..4691879f9c 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -30,7 +30,6 @@ #include "message_filters/subscriber.h" #include "nav2_util/angleutils.hpp" -#include "nav2_util/duration_conversions.hpp" #include "nav2_util/pf/pf.hpp" #include "nav2_util/string_utils.hpp" #include "nav2_util/sensors/laser/laser.hpp" diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index 815541af73..932c062a2d 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -45,7 +45,6 @@ #include "pluginlib/class_list_macros.hpp" #include "sensor_msgs/point_cloud2_iterator.hpp" -#include "nav2_util/duration_conversions.hpp" #include "nav2_costmap_2d/costmap_math.hpp" PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::ObstacleLayer, nav2_costmap_2d::Layer) @@ -212,7 +211,7 @@ void ObstacleLayer::onInitialize() observation_subscribers_.push_back(sub); observation_notifiers_.push_back(filter); - observation_notifiers_.back()->setTolerance(nav2_util::duration_from_seconds(0.05)); + observation_notifiers_.back()->setTolerance(rclcpp::Duration::from_seconds(0.05)); } else { std::shared_ptr> sub( diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index da31c67a3d..e34ef0e7cb 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -43,7 +43,6 @@ #include #include "nav2_costmap_2d/layered_costmap.hpp" -#include "nav2_util/duration_conversions.hpp" #include "nav2_util/execution_timer.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.h" #include "tf2_ros/create_timer_ros.h" @@ -299,7 +298,7 @@ Costmap2DROS::getParameters() // 2. The map publish frequency cannot be 0 (to avoid a divde-by-zero) if (map_publish_frequency_ > 0) { - publish_cycle_ = nav2_util::duration_from_seconds(1 / map_publish_frequency_); + publish_cycle_ = rclcpp::Duration::from_seconds(1 / map_publish_frequency_); } else { publish_cycle_ = rclcpp::Duration(-1); } diff --git a/nav2_costmap_2d/test/integration/costmap_tester.cpp b/nav2_costmap_2d/test/integration/costmap_tester.cpp index a272a2fd6c..a0265e1d79 100644 --- a/nav2_costmap_2d/test/integration/costmap_tester.cpp +++ b/nav2_costmap_2d/test/integration/costmap_tester.cpp @@ -41,7 +41,6 @@ #include "tf2_ros/transform_listener.h" #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "nav2_costmap_2d/cost_values.hpp" -#include "nav2_util/duration_conversions.hpp" namespace nav2_costmap_2d { diff --git a/nav2_dwb_controller/dwb_controller/src/progress_checker.cpp b/nav2_dwb_controller/dwb_controller/src/progress_checker.cpp index 481125d601..f7743d6389 100644 --- a/nav2_dwb_controller/dwb_controller/src/progress_checker.cpp +++ b/nav2_dwb_controller/dwb_controller/src/progress_checker.cpp @@ -15,7 +15,6 @@ #include "dwb_controller/progress_checker.hpp" #include #include "dwb_core/exceptions.hpp" -#include "nav2_util/duration_conversions.hpp" namespace dwb_controller { @@ -28,7 +27,7 @@ ProgressChecker::ProgressChecker(const rclcpp::Node::SharedPtr & node) nh_->get_parameter_or("required_movement_radius", radius_, 0.5); double time_allowance_param; nh_->get_parameter_or("movement_time_allowance_", time_allowance_param, 10.0); - time_allowance_ = nav2_util::duration_from_seconds(time_allowance_param); + time_allowance_ = rclcpp::Duration::from_seconds(time_allowance_param); } void ProgressChecker::check(nav_2d_msgs::msg::Pose2DStamped & current_pose) diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp index a3a5dd8d0b..cb8881c8fe 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -48,7 +48,6 @@ #include "nav_2d_utils/tf_help.hpp" #include "nav2_util/lifecycle_node.hpp" #include "pluginlib/class_list_macros.hpp" -#include "nav2_util/duration_conversions.hpp" namespace dwb_core { @@ -83,7 +82,7 @@ DWBLocalPlanner::on_configure(const rclcpp_lifecycle::State & state) double transform_tolerance; node_->get_parameter("transform_tolerance", transform_tolerance); - transform_tolerance_ = nav2_util::duration_from_seconds(transform_tolerance); + transform_tolerance_ = rclcpp::Duration::from_seconds(transform_tolerance); RCLCPP_INFO(node_->get_logger(), "Setting transform_tolerance to %f", transform_tolerance); node_->get_parameter("prune_plan", prune_plan_); diff --git a/nav2_dwb_controller/dwb_critics/src/oscillation.cpp b/nav2_dwb_controller/dwb_critics/src/oscillation.cpp index 708059b06e..1dd8d742b1 100644 --- a/nav2_dwb_controller/dwb_critics/src/oscillation.cpp +++ b/nav2_dwb_controller/dwb_critics/src/oscillation.cpp @@ -37,7 +37,6 @@ #include #include #include -#include "nav2_util/duration_conversions.hpp" #include "nav_2d_utils/parameters.hpp" #include "dwb_core/exceptions.hpp" #include "pluginlib/class_list_macros.hpp" @@ -94,7 +93,7 @@ void OscillationCritic::onInit() oscillation_reset_dist_ = nav_2d_utils::searchAndGetParam(nh_, "oscillation_reset_dist", 0.05); oscillation_reset_dist_sq_ = oscillation_reset_dist_ * oscillation_reset_dist_; oscillation_reset_angle_ = nav_2d_utils::searchAndGetParam(nh_, "oscillation_reset_angle", 0.2); - oscillation_reset_time_ = nav2_util::duration_from_seconds( + oscillation_reset_time_ = rclcpp::Duration::from_seconds( nav_2d_utils::searchAndGetParam(nh_, "oscillation_reset_time", -1.0)); nh_->declare_parameter(name_ + ".x_only_threshold", rclcpp::ParameterValue(0.05)); @@ -174,7 +173,7 @@ bool OscillationCritic::resetAvailable() return true; } } - if (oscillation_reset_time_ >= nav2_util::duration_from_seconds(0.0)) { + if (oscillation_reset_time_ >= rclcpp::Duration::from_seconds(0.0)) { auto t_diff = (nh_->now() - prev_reset_time_); if (t_diff > oscillation_reset_time_) { return true; diff --git a/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp b/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp index 46b03f82cb..4d6b8e4205 100644 --- a/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp @@ -38,7 +38,6 @@ #include "nav_2d_utils/parameters.hpp" #include "pluginlib/class_list_macros.hpp" #include "dwb_core/exceptions.hpp" -#include "nav2_util/duration_conversions.hpp" namespace dwb_plugins { @@ -87,7 +86,7 @@ dwb_msgs::msg::Trajectory2D LimitedAccelGenerator::generateTrajectory( { dwb_msgs::msg::Trajectory2D traj; traj.velocity = cmd_vel; - traj.duration = nav2_util::duration_from_seconds(sim_time_); + traj.duration = rclcpp::Duration::from_seconds(sim_time_); geometry_msgs::msg::Pose2D pose = start_pose; std::vector steps = getTimeSteps(cmd_vel); diff --git a/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp b/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp index fff2d6fdd0..5fe5eb985b 100644 --- a/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp @@ -41,7 +41,6 @@ #include "nav_2d_utils/parameters.hpp" #include "pluginlib/class_list_macros.hpp" #include "dwb_core/exceptions.hpp" -#include "nav2_util/duration_conversions.hpp" using nav_2d_utils::loadParameterWithDeprecation; @@ -148,7 +147,7 @@ dwb_msgs::msg::Trajectory2D StandardTrajectoryGenerator::generateTrajectory( { dwb_msgs::msg::Trajectory2D traj; traj.velocity = cmd_vel; - traj.duration = nav2_util::duration_from_seconds(sim_time_); + traj.duration = rclcpp::Duration::from_seconds(sim_time_); // simulate the trajectory geometry_msgs::msg::Pose2D pose = start_pose; nav_2d_msgs::msg::Twist2D vel = start_vel; diff --git a/nav2_util/include/nav2_util/duration_conversions.hpp b/nav2_util/include/nav2_util/duration_conversions.hpp deleted file mode 100644 index 89b993942d..0000000000 --- a/nav2_util/include/nav2_util/duration_conversions.hpp +++ /dev/null @@ -1,32 +0,0 @@ -// Copyright (c) 2018 Intel Corporation -// -// 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 NAV2_UTIL__DURATION_CONVERSIONS_HPP_ -#define NAV2_UTIL__DURATION_CONVERSIONS_HPP_ - -#include "rclcpp/rclcpp.hpp" - -namespace nav2_util -{ - -// TODO(crdelsey): This functionality should be part of the RCLCPP Duration -// interface. Once that gets integrated, this function can be removed. -inline rclcpp::Duration duration_from_seconds(double seconds) -{ - return rclcpp::Duration(static_cast(seconds * 1e9)); // convert to ns -} - -} // namespace nav2_util - -#endif // NAV2_UTIL__DURATION_CONVERSIONS_HPP_