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

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp

@@ -54,9 +54,13 @@ class SpinAction : public BtActionNode<nav2_msgs::action::Spin>
     return providedBasicPorts(
       {
         BT::InputPort<double>("spin_dist", 1.57, "Spin distance"),
-        BT::InputPort<double>("time_allowance", 10.0, "Allowed time for spinning")
+        BT::InputPort<double>("time_allowance", 10.0, "Allowed time for spinning"),
+        BT::InputPort<bool>("is_recovery", true, "True if recovery")
       });
   }
+
+private:
+  bool is_recovery_;
 };
 
 }  // namespace nav2_behavior_tree

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

nav2_behaviors/plugins/drive_on_heading.hpp

@@ -93,8 +93,6 @@ class DriveOnHeading : public TimedBehavior<ActionT>
   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(

nav2_bt_navigator/behavior_trees/odometry_calibration.xml

@@ -0,0 +1,20 @@
+<!--
+  his Behavior Tree drives in a square for odometry calibration experiments
+-->
+
+<root main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <Repeat num_cycles="3">
+      <Sequence name="Drive in a square">
+        <DriveOnHeading dist_to_travel="2.0" speed="0.2" time_allowance="12"/>
+        <Spin spin_dist="1.570796" is_recovery="false"/>
+        <DriveOnHeading dist_to_travel="2.0" speed="0.2" time_allowance="12"/>
+        <Spin spin_dist="1.570796" is_recovery="false"/>
+        <DriveOnHeading dist_to_travel="2.0" speed="0.2" time_allowance="12"/>
+        <Spin spin_dist="1.570796" is_recovery="false"/>
+        <DriveOnHeading dist_to_travel="2.0" speed="0.2" time_allowance="12"/>
+        <Spin spin_dist="1.570796" is_recovery="false"/>
+      </Sequence>
+    </Repeat>
+  </BehaviorTree>
+</root>

nav2_system_tests/src/behavior_tree/server_handler.cpp

@@ -40,6 +40,8 @@ ServerHandler::ServerHandler()
     node_, "wait");
   backup_server = std::make_unique<DummyActionServer<nav2_msgs::action::BackUp>>(
     node_, "backup");
+  drive_on_heading_server = std::make_unique<DummyActionServer<nav2_msgs::action::DriveOnHeading>>(
+    node_, "drive_on_heading");
   ntp_server = std::make_unique<DummyActionServer<nav2_msgs::action::ComputePathThroughPoses>>(
     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()

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<DummyActionServer<nav2_msgs::action::Spin>> spin_server;
   std::unique_ptr<DummyActionServer<nav2_msgs::action::Wait>> wait_server;
   std::unique_ptr<DummyActionServer<nav2_msgs::action::BackUp>> backup_server;
+  std::unique_ptr<DummyActionServer<nav2_msgs::action::DriveOnHeading>> drive_on_heading_server;
   std::unique_ptr<DummyActionServer<nav2_msgs::action::ComputePathThroughPoses>> ntp_server;
 
 private:

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));