diff --git a/.circleci/config.yml b/.circleci/config.yml index 9d3e0f490d..ab6312a368 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -33,12 +33,12 @@ _commands: - restore_cache: name: Restore Cache << parameters.key >> keys: - - "<< parameters.key >>-v16\ + - "<< parameters.key >>-v17\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" - - "<< parameters.key >>-v16\ + - "<< parameters.key >>-v17\ -{{ arch }}\ -main\ -\ @@ -58,7 +58,7 @@ _commands: steps: - save_cache: name: Save Cache << parameters.key >> - key: "<< parameters.key >>-v16\ + key: "<< parameters.key >>-v17\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp index 596fda9791..daf609d067 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp @@ -25,6 +25,7 @@ #include "behaviortree_cpp_v3/xml_parsing.h" #include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h" +#include "rclcpp/rclcpp.hpp" namespace nav2_behavior_tree { @@ -46,7 +47,8 @@ class BehaviorTreeEngine * @brief A constructor for nav2_behavior_tree::BehaviorTreeEngine * @param plugin_libraries vector of BT plugin library names to load */ - explicit BehaviorTreeEngine(const std::vector & plugin_libraries); + explicit BehaviorTreeEngine( + const std::vector & plugin_libraries); virtual ~BehaviorTreeEngine() {} /** diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index 947778da5d..429368ea96 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -25,7 +25,8 @@ namespace nav2_behavior_tree { -BehaviorTreeEngine::BehaviorTreeEngine(const std::vector & plugin_libraries) +BehaviorTreeEngine::BehaviorTreeEngine( + const std::vector & plugin_libraries) { BT::SharedLibrary loader; for (const auto & p : plugin_libraries) { diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp index 4fb0a459af..58999ffe18 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp @@ -76,7 +76,7 @@ class DriveOnHeading : public TimedBehavior command_speed_ = command->speed; command_time_allowance_ = command->time_allowance; - end_time_ = this->steady_clock_.now() + command_time_allowance_; + end_time_ = this->clock_->now() + command_time_allowance_; if (!nav2_util::getCurrentPose( initial_pose_, *this->tf_, this->local_frame_, this->robot_base_frame_, @@ -95,7 +95,7 @@ class DriveOnHeading : public TimedBehavior */ ResultStatus onCycleUpdate() override { - rclcpp::Duration time_remaining = end_time_ - this->steady_clock_.now(); + rclcpp::Duration time_remaining = end_time_ - this->clock_->now(); if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) { this->stopRobot(); RCLCPP_WARN( diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp index 6af6490b61..d01e14179f 100644 --- a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp +++ b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp @@ -119,8 +119,8 @@ class TimedBehavior : public nav2_core::Behavior { node_ = parent; auto node = node_.lock(); - logger_ = node->get_logger(); + clock_ = node->get_clock(); RCLCPP_INFO(logger_, "Configuring %s", name.c_str()); @@ -200,7 +200,7 @@ class TimedBehavior : public nav2_core::Behavior rclcpp::Duration elasped_time_{0, 0}; // Clock - rclcpp::Clock steady_clock_{RCL_STEADY_TIME}; + rclcpp::Clock::SharedPtr clock_; // Logger rclcpp::Logger logger_{rclcpp::get_logger("nav2_behaviors")}; @@ -231,11 +231,11 @@ class TimedBehavior : public nav2_core::Behavior return; } - auto start_time = steady_clock_.now(); + auto start_time = clock_->now(); rclcpp::WallRate loop_rate(cycle_frequency_); while (rclcpp::ok()) { - elasped_time_ = steady_clock_.now() - start_time; + elasped_time_ = clock_->now() - start_time; if (action_server_->is_cancel_requested()) { RCLCPP_INFO(logger_, "Canceling %s", behavior_name_.c_str()); stopRobot(); @@ -252,7 +252,7 @@ class TimedBehavior : public nav2_core::Behavior " however feature is currently not implemented. Aborting and stopping.", behavior_name_.c_str()); stopRobot(); - result->total_elapsed_time = steady_clock_.now() - start_time; + result->total_elapsed_time = clock_->now() - start_time; onActionCompletion(result); action_server_->terminate_current(result); return; @@ -264,14 +264,14 @@ class TimedBehavior : public nav2_core::Behavior RCLCPP_INFO( logger_, "%s completed successfully", behavior_name_.c_str()); - result->total_elapsed_time = steady_clock_.now() - start_time; + result->total_elapsed_time = clock_->now() - start_time; onActionCompletion(result); action_server_->succeeded_current(result); return; case Status::FAILED: RCLCPP_WARN(logger_, "%s failed", behavior_name_.c_str()); - result->total_elapsed_time = steady_clock_.now() - start_time; + result->total_elapsed_time = clock_->now() - start_time; result->error_code = on_cycle_update_result.error_code; onActionCompletion(result); action_server_->terminate_current(result); diff --git a/nav2_behaviors/plugins/assisted_teleop.cpp b/nav2_behaviors/plugins/assisted_teleop.cpp index 4628010a3d..a0fc972999 100644 --- a/nav2_behaviors/plugins/assisted_teleop.cpp +++ b/nav2_behaviors/plugins/assisted_teleop.cpp @@ -67,7 +67,7 @@ ResultStatus AssistedTeleop::onRun(const std::shared_ptrtime_allowance; - end_time_ = steady_clock_.now() + command_time_allowance_; + end_time_ = this->clock_->now() + command_time_allowance_; return ResultStatus{Status::SUCCEEDED, AssistedTeleopActionResult::NONE}; } @@ -82,7 +82,7 @@ ResultStatus AssistedTeleop::onCycleUpdate() feedback_->current_teleop_duration = elasped_time_; action_server_->publish_feedback(feedback_); - rclcpp::Duration time_remaining = end_time_ - steady_clock_.now(); + rclcpp::Duration time_remaining = end_time_ - this->clock_->now(); if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) { stopRobot(); RCLCPP_WARN_STREAM( @@ -125,7 +125,7 @@ ResultStatus AssistedTeleop::onCycleUpdate() if (time == simulation_time_step_) { RCLCPP_DEBUG_STREAM_THROTTLE( logger_, - steady_clock_, + *clock_, 1000, behavior_name_.c_str() << " collided on first time step, setting velocity to zero"); scaled_twist.linear.x = 0.0f; @@ -135,7 +135,7 @@ ResultStatus AssistedTeleop::onCycleUpdate() } else { RCLCPP_DEBUG_STREAM_THROTTLE( logger_, - steady_clock_, + *clock_, 1000, behavior_name_.c_str() << " collision approaching in " << time << " seconds"); double scale_factor = time / projection_time_; diff --git a/nav2_behaviors/plugins/back_up.cpp b/nav2_behaviors/plugins/back_up.cpp index 1ec9e83264..7a194874c7 100644 --- a/nav2_behaviors/plugins/back_up.cpp +++ b/nav2_behaviors/plugins/back_up.cpp @@ -31,7 +31,7 @@ ResultStatus BackUp::onRun(const std::shared_ptr comma command_speed_ = -std::fabs(command->speed); command_time_allowance_ = command->time_allowance; - end_time_ = steady_clock_.now() + command_time_allowance_; + end_time_ = this->clock_->now() + command_time_allowance_; if (!nav2_util::getCurrentPose( initial_pose_, *tf_, local_frame_, robot_base_frame_, diff --git a/nav2_behaviors/plugins/spin.cpp b/nav2_behaviors/plugins/spin.cpp index caa22941b3..c2767ecc5b 100644 --- a/nav2_behaviors/plugins/spin.cpp +++ b/nav2_behaviors/plugins/spin.cpp @@ -91,14 +91,14 @@ ResultStatus Spin::onRun(const std::shared_ptr command) cmd_yaw_); command_time_allowance_ = command->time_allowance; - end_time_ = steady_clock_.now() + command_time_allowance_; + end_time_ = this->clock_->now() + command_time_allowance_; return ResultStatus{Status::SUCCEEDED, SpinActionResult::NONE}; } ResultStatus Spin::onCycleUpdate() { - rclcpp::Duration time_remaining = end_time_ - steady_clock_.now(); + rclcpp::Duration time_remaining = end_time_ - this->clock_->now(); if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) { stopRobot(); RCLCPP_WARN( diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index c54b9ae829..2413e2eb3f 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -241,9 +241,6 @@ class PlannerServer : public nav2_util::LifecycleNode double max_planner_duration_; std::string planner_ids_concat_; - // Clock - rclcpp::Clock steady_clock_{RCL_STEADY_TIME}; - // TF buffer std::shared_ptr tf_; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index fea40ea405..d087cbef75 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -361,7 +361,7 @@ void PlannerServer::computePlanThroughPoses() { std::lock_guard lock(dynamic_params_lock_); - auto start_time = steady_clock_.now(); + auto start_time = this->now(); // Initialize the ComputePathThroughPoses goal and result auto goal = action_server_poses_->get_current_goal(); @@ -421,7 +421,7 @@ void PlannerServer::computePlanThroughPoses() result->path = concat_path; publishPlan(result->path); - auto cycle_duration = steady_clock_.now() - start_time; + auto cycle_duration = this->now() - start_time; result->planning_time = cycle_duration; if (max_planner_duration_ && cycle_duration.seconds() > max_planner_duration_) { @@ -480,7 +480,7 @@ PlannerServer::computePlan() { std::lock_guard lock(dynamic_params_lock_); - auto start_time = steady_clock_.now(); + auto start_time = this->now(); // Initialize the ComputePathToPose goal and result auto goal = action_server_pose_->get_current_goal(); @@ -517,7 +517,7 @@ PlannerServer::computePlan() // Publish the plan for visualization purposes publishPlan(result->path); - auto cycle_duration = steady_clock_.now() - start_time; + auto cycle_duration = this->now() - start_time; result->planning_time = cycle_duration; if (max_planner_duration_ && cycle_duration.seconds() > max_planner_duration_) { diff --git a/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp b/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp index 35eaadb861..fac9ffe3af 100644 --- a/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp @@ -166,8 +166,6 @@ class SmootherServer : public nav2_util::LifecycleNode std::shared_ptr costmap_sub_; std::shared_ptr footprint_sub_; std::shared_ptr collision_checker_; - - rclcpp::Clock steady_clock_; }; } // namespace nav2_smoother diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index 92d567a057..4e7f7242de 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -259,7 +259,7 @@ bool SmootherServer::findSmootherId( void SmootherServer::smoothPlan() { - auto start_time = steady_clock_.now(); + auto start_time = this->now(); RCLCPP_INFO(get_logger(), "Received a path to smooth."); @@ -283,7 +283,7 @@ void SmootherServer::smoothPlan() result->was_completed = smoothers_[current_smoother_]->smooth( result->path, goal->max_smoothing_duration); - result->smoothing_duration = steady_clock_.now() - start_time; + result->smoothing_duration = this->now() - start_time; if (!result->was_completed) { RCLCPP_INFO( diff --git a/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp b/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp index a270c8ab08..974c2d60b2 100644 --- a/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp +++ b/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp @@ -35,7 +35,9 @@ BackupBehaviorTester::BackupBehaviorTester() : is_active_(false), initial_pose_received_(false) { - node_ = rclcpp::Node::make_shared("backup_behavior_test"); + rclcpp::NodeOptions options; + options.parameter_overrides({{"use_sim_time", true}}); + node_ = rclcpp::Node::make_shared("backup_behavior_test", options); tf_buffer_ = std::make_shared(node_->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); diff --git a/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py b/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py index 6f4a6c3c16..bd4c5432fe 100755 --- a/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py @@ -102,7 +102,7 @@ def main(argv=sys.argv[1:]): testExecutable = os.getenv('TEST_EXECUTABLE') test1_action = ExecuteProcess( - cmd=[testExecutable], name='test_backup_behavior_node', output='screen' + cmd=[testExecutable], name='test_backup_behavior_node', output='screen', ) lts = LaunchTestService() diff --git a/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.cpp b/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.cpp index 8db6ef6406..967c6beb04 100644 --- a/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.cpp +++ b/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.cpp @@ -35,7 +35,9 @@ DriveOnHeadingBehaviorTester::DriveOnHeadingBehaviorTester() : is_active_(false), initial_pose_received_(false) { - node_ = rclcpp::Node::make_shared("DriveOnHeading_behavior_test"); + rclcpp::NodeOptions options; + options.parameter_overrides({{"use_sim_time", true}}); + node_ = rclcpp::Node::make_shared("DriveOnHeading_behavior_test", options); tf_buffer_ = std::make_shared(node_->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py index 4552f51768..c9df3d3243 100755 --- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py @@ -102,7 +102,7 @@ def main(argv=sys.argv[1:]): testExecutable = os.getenv('TEST_EXECUTABLE') test1_action = ExecuteProcess( - cmd=[testExecutable], name='test_spin_behavior_node', output='screen' + cmd=[testExecutable], name='test_spin_behavior_node', output='screen', ) lts = LaunchTestService() diff --git a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py index f4c4e5bc16..80f1529820 100755 --- a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py @@ -102,7 +102,7 @@ def main(argv=sys.argv[1:]): testExecutable = os.getenv('TEST_EXECUTABLE') test1_action = ExecuteProcess( - cmd=[testExecutable], name='test_wait_behavior_node', output='screen' + cmd=[testExecutable], name='test_wait_behavior_node', output='screen', ) lts = LaunchTestService() diff --git a/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp b/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp index ea6e58afb7..dffd52f911 100644 --- a/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp +++ b/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp @@ -164,7 +164,6 @@ bool WaitBehaviorTester::behaviorTest( RCLCPP_INFO(node_->get_logger(), "result received"); - if ((node_->now() - start_time).seconds() < static_cast(wait_time)) { return false; } diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index 6393a465a5..62ce6d12ad 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -158,7 +158,7 @@ VelocitySmoother::on_activate(const rclcpp_lifecycle::State &) RCLCPP_INFO(get_logger(), "Activating"); smoothed_cmd_pub_->on_activate(); double timer_duration_ms = 1000.0 / smoothing_frequency_; - timer_ = create_wall_timer( + timer_ = this->create_wall_timer( std::chrono::milliseconds(static_cast(timer_duration_ms)), std::bind(&VelocitySmoother::smootherTimer, this)); @@ -378,7 +378,7 @@ VelocitySmoother::dynamicParametersCallback(std::vector param } double timer_duration_ms = 1000.0 / smoothing_frequency_; - timer_ = create_wall_timer( + timer_ = this->create_wall_timer( std::chrono::milliseconds(static_cast(timer_duration_ms)), std::bind(&VelocitySmoother::smootherTimer, this)); } else if (name == "velocity_timeout") { diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp index 911ae441a2..cbd2700246 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp @@ -73,6 +73,7 @@ class WaitAtWaypoint : public nav2_core::WaypointTaskExecutor int waypoint_pause_duration_; bool is_enabled_; rclcpp::Logger logger_{rclcpp::get_logger("nav2_waypoint_follower")}; + rclcpp::Clock::SharedPtr clock_; }; } // namespace nav2_waypoint_follower diff --git a/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp b/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp index 917655e21a..77e20bcf76 100644 --- a/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp +++ b/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp @@ -42,6 +42,7 @@ void WaitAtWaypoint::initialize( throw std::runtime_error{"Failed to lock node in wait at waypoint plugin!"}; } logger_ = node->get_logger(); + clock_ = node->get_clock(); nav2_util::declare_parameter_if_not_declared( node, plugin_name + ".waypoint_pause_duration", @@ -77,7 +78,7 @@ bool WaitAtWaypoint::processAtWaypoint( logger_, "Arrived at %i'th waypoint, sleeping for %i milliseconds", curr_waypoint_index, waypoint_pause_duration_); - rclcpp::sleep_for(std::chrono::milliseconds(waypoint_pause_duration_)); + clock_->sleep_for(std::chrono::milliseconds(waypoint_pause_duration_)); return true; } } // namespace nav2_waypoint_follower