From 9bb37c61c4a2be0a836345db795b9f872ea7b4fd Mon Sep 17 00:00:00 2001 From: Joshua Wallace <47819219+jwallace42@users.noreply.github.com> Date: Wed, 1 Jun 2022 22:52:53 -0400 Subject: [PATCH] Odometery calibration (#2934) * added a BT for driving in a square * removed extra printouts * decreased timeout * do not increment recovery * code review * code review * test and linting fix * updated plugin list * fixed BT loading issue * bump up speed --- .../plugins/action/spin_action.hpp | 6 +++++- .../plugins/action/spin_action.cpp | 5 ++++- nav2_behaviors/plugins/drive_on_heading.hpp | 2 -- .../behavior_trees/odometry_calibration.xml | 20 +++++++++++++++++++ .../src/behavior_tree/server_handler.cpp | 3 +++ .../src/behavior_tree/server_handler.hpp | 2 ++ .../behavior_tree/test_behavior_tree_node.cpp | 11 +++++++--- 7 files changed, 42 insertions(+), 7 deletions(-) create mode 100644 nav2_bt_navigator/behavior_trees/odometry_calibration.xml diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp index dec29248ea..76496ba7ed 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp @@ -54,9 +54,13 @@ class SpinAction : public BtActionNode return providedBasicPorts( { BT::InputPort("spin_dist", 1.57, "Spin distance"), - BT::InputPort("time_allowance", 10.0, "Allowed time for spinning") + BT::InputPort("time_allowance", 10.0, "Allowed time for spinning"), + BT::InputPort("is_recovery", true, "True if recovery") }); } + +private: + bool is_recovery_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/action/spin_action.cpp b/nav2_behavior_tree/plugins/action/spin_action.cpp index 5682a2ae9d..73dc8c589f 100644 --- a/nav2_behavior_tree/plugins/action/spin_action.cpp +++ b/nav2_behavior_tree/plugins/action/spin_action.cpp @@ -32,11 +32,14 @@ SpinAction::SpinAction( getInput("time_allowance", time_allowance); goal_.target_yaw = dist; goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); + getInput("is_recovery", is_recovery_); } void SpinAction::on_tick() { - increment_recovery_count(); + if (is_recovery_) { + increment_recovery_count(); + } } } // namespace nav2_behavior_tree diff --git a/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/plugins/drive_on_heading.hpp index da04533267..65ab8f1157 100644 --- a/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/plugins/drive_on_heading.hpp @@ -93,8 +93,6 @@ class DriveOnHeading : public TimedBehavior Status onCycleUpdate() { rclcpp::Duration time_remaining = end_time_ - this->steady_clock_.now(); - RCLCPP_ERROR_STREAM(this->logger_, "Time remaining" << time_remaining.seconds()); - RCLCPP_ERROR_STREAM(this->logger_, "Time remaining" << command_time_allowance_.seconds()); if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) { this->stopRobot(); RCLCPP_WARN( diff --git a/nav2_bt_navigator/behavior_trees/odometry_calibration.xml b/nav2_bt_navigator/behavior_trees/odometry_calibration.xml new file mode 100644 index 0000000000..49000e14fa --- /dev/null +++ b/nav2_bt_navigator/behavior_trees/odometry_calibration.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + diff --git a/nav2_system_tests/src/behavior_tree/server_handler.cpp b/nav2_system_tests/src/behavior_tree/server_handler.cpp index 0952f60712..2108f37d58 100644 --- a/nav2_system_tests/src/behavior_tree/server_handler.cpp +++ b/nav2_system_tests/src/behavior_tree/server_handler.cpp @@ -40,6 +40,8 @@ ServerHandler::ServerHandler() node_, "wait"); backup_server = std::make_unique>( node_, "backup"); + drive_on_heading_server = std::make_unique>( + node_, "drive_on_heading"); ntp_server = std::make_unique>( node_, "compute_path_through_poses"); } @@ -85,6 +87,7 @@ void ServerHandler::reset() const spin_server->reset(); wait_server->reset(); backup_server->reset(); + drive_on_heading_server->reset(); } void ServerHandler::spinThread() diff --git a/nav2_system_tests/src/behavior_tree/server_handler.hpp b/nav2_system_tests/src/behavior_tree/server_handler.hpp index c9fc50f38c..f8869ea037 100644 --- a/nav2_system_tests/src/behavior_tree/server_handler.hpp +++ b/nav2_system_tests/src/behavior_tree/server_handler.hpp @@ -29,6 +29,7 @@ #include "nav2_msgs/action/spin.hpp" #include "nav2_msgs/action/back_up.hpp" #include "nav2_msgs/action/wait.hpp" +#include "nav2_msgs/action/drive_on_heading.hpp" #include "nav2_msgs/action/compute_path_through_poses.hpp" #include "geometry_msgs/msg/point_stamped.hpp" @@ -93,6 +94,7 @@ class ServerHandler std::unique_ptr> spin_server; std::unique_ptr> wait_server; std::unique_ptr> backup_server; + std::unique_ptr> drive_on_heading_server; std::unique_ptr> ntp_server; private: diff --git a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp index 39503f9f8e..2f76823ae4 100644 --- a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp +++ b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp @@ -56,16 +56,17 @@ class BehaviorTreeHandler "nav2_compute_path_through_poses_action_bt_node", "nav2_smooth_path_action_bt_node", "nav2_follow_path_action_bt_node", - "nav2_back_up_action_bt_node", "nav2_spin_action_bt_node", "nav2_wait_action_bt_node", + "nav2_back_up_action_bt_node", + "nav2_drive_on_heading_bt_node", "nav2_clear_costmap_service_bt_node", "nav2_is_stuck_condition_bt_node", - "nav2_is_path_valid_condition_bt_node", "nav2_goal_reached_condition_bt_node", "nav2_initial_pose_received_condition_bt_node", "nav2_goal_updated_condition_bt_node", "nav2_globally_updated_goal_condition_bt_node", + "nav2_is_path_valid_condition_bt_node", "nav2_reinitialize_global_localization_service_bt_node", "nav2_rate_controller_bt_node", "nav2_distance_controller_bt_node", @@ -89,7 +90,11 @@ class BehaviorTreeHandler "nav2_controller_selector_bt_node", "nav2_goal_checker_selector_bt_node", "nav2_controller_cancel_bt_node", - "nav2_path_longer_on_approach_bt_node" + "nav2_path_longer_on_approach_bt_node", + "nav2_wait_cancel_bt_node", + "nav2_spin_cancel_bt_node", + "nav2_back_up_cancel_bt_node", + "nav2_drive_on_heading_cancel_bt_node" }; for (const auto & p : plugin_libs) { factory_.registerFromPlugin(BT::SharedLibrary::getOSName(p));