Add wait time between localization and navigation

Author: LuisLechugaRuiz

src/task_behavior_tree/src/bt_navigate_and_find.cpp

@@ -39,7 +39,7 @@ BtNavigateAndFind::BtNavigateAndFind() : rclcpp::Node("bt_navigate_and_find") {
   for (auto& goal_pose_name : goal_poses_names_) {
     declare_parameter(goal_pose_name);
   }
-  declare_parameter("spin_angle", 0.7854);
+  declare_parameter("spin_angle", 0.785);
   declare_parameter("wait_time", 3.0);
 
   if (ConfigureBT()) {
@@ -87,7 +87,7 @@ bool BtNavigateAndFind::ConfigureBT() {
   double spin_angle;
   get_parameter("spin_angle", spin_angle);
   blackboard_->set<double>("spin_angle", spin_angle);
-  blackboard_->set<int>("spin_cycles", int(3.14 / spin_angle));
+  blackboard_->set<int>("spin_cycles", int(std::round(6.28 / spin_angle)));
   blackboard_->set<double>("wait_time", (get_parameter("wait_time")).get_value<double>());
 
   // Get the BT filename to use from the node parameter

src/task_monitor/src/task_monitor.cpp

@@ -121,6 +121,8 @@ void TaskMonitor::SetInitialPose() {
   while ((now() - start_time).seconds() < timeout_s_) {
     if (IsRobotAtPose(initial_pose_)) {
       LOG(INFO, "Robot is at initial pose");
+      // Wait until nav is properly initialized. TODO: Make it better subscribing to any topic
+      rclcpp::sleep_for(std::chrono::milliseconds(5000));
       initialize_navigation_action_server_->succeeded_current();
       return;
     }