Add doors to distance remaining

redvinaa · turtlewizard73 · commit e0334ea57a66 · 2023-10-24T10:44:21.000Z
diff --git a/nav2_bt_navigator/CMakeLists.txt b/nav2_bt_navigator/CMakeLists.txt
@@ -9,6 +9,7 @@ find_package(rclcpp_lifecycle REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(service_robot_msgs REQUIRED)
 find_package(nav2_behavior_tree REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(nav2_msgs REQUIRED)
@@ -40,6 +41,7 @@ set(dependencies
   rclcpp_components
   std_msgs
   geometry_msgs
+  service_robot_msgs
   nav2_behavior_tree
   nav_msgs
   nav2_msgs
diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp
@@ -17,10 +17,12 @@
 
 #include <string>
 #include <vector>
+#include <tuple>
 #include <memory>
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
+#include "service_robot_msgs/msg/door.hpp"
 #include "nav2_core/behavior_tree_navigator.hpp"
 #include "nav2_msgs/action/navigate_to_pose.hpp"
 #include "nav2_util/geometry_utils.hpp"
@@ -31,6 +33,9 @@
 namespace nav2_bt_navigator
 {
 
+using Door = service_robot_msgs::msg::Door;
+using Direction = uint16_t;
+
 /**
  * @class NavigateToPoseNavigator
  * @brief A navigator for navigating to a specified pose
@@ -126,6 +131,7 @@ class NavigateToPoseNavigator
 
   std::string goal_blackboard_id_;
   std::string path_blackboard_id_;
+  std::string doors_list_blackboard_id_;
 
   // Odometry smoother object
   std::shared_ptr<nav2_util::OdomSmoother> odom_smoother_;
@@ -134,6 +140,8 @@ class NavigateToPoseNavigator
   double avg_linear_vel_;
   // Robot's angular velocity (to calculate time remaining, if set)
   double avg_angular_vel_;
+  // Expected time to cross door, used to calculate remaining time
+  double time_to_cross_door_;
 };
 
 }  // namespace nav2_bt_navigator
diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml
@@ -20,6 +20,7 @@
   <build_depend>behaviortree_cpp_v3</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>geometry_msgs</build_depend>
+  <build_depend>service_robot_msgs</build_depend>
   <build_depend>std_srvs</build_depend>
   <build_depend>nav2_util</build_depend>
   <build_depend>pluginlib</build_depend>
@@ -35,6 +36,7 @@
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>nav2_util</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>service_robot_msgs</exec_depend>
   <exec_depend>pluginlib</exec_depend>
   <exec_depend>nav2_core</exec_depend>
 
diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp
@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include <vector>
+#include <tuple>
 #include <string>
 #include <memory>
 #include <limits>
@@ -43,6 +44,18 @@ NavigateToPoseNavigator::configure(
 
   path_blackboard_id_ = node->get_parameter("path_blackboard_id").as_string();
 
+  if (!node->has_parameter("doors_list_blackboard_id")) {
+    node->declare_parameter("doors_list_blackboard_id", std::string("doors_list"));
+  }
+
+  doors_list_blackboard_id_ = node->get_parameter("doors_list_blackboard_id").as_string();
+
+  if (!node->has_parameter("time_to_cross_door")) {
+    node->declare_parameter("time_to_cross_door", 10.0);
+  }
+
+  time_to_cross_door_ = node->get_parameter("time_to_cross_door").as_double();
+
   if (!node->has_parameter("average_linear_speed")) {
     node->declare_parameter("average_linear_speed", 0.0);
   }
@@ -205,10 +218,23 @@ NavigateToPoseNavigator::onLoop()
         rclcpp::Duration::from_seconds(distance_remaining / std::abs(current_linear_speed));
     }
 
+    // If doors_lists is set, add doors to time remaining
+    try {
+      auto doors_list = blackboard->get<std::vector<std::tuple<Door, Direction>>>(
+        "doors_list");
+      estimated_time_remaining = rclcpp::Duration::from_seconds(
+        estimated_time_remaining.seconds() +
+        doors_list.size() * time_to_cross_door_);
+    } catch (BT::RuntimeError &) {
+      RCLCPP_DEBUG(
+        logger_, "Doors list not found in blackboard. Doors will not be added to time remaining");
+    }
+
     feedback_msg->distance_remaining = distance_remaining;
     feedback_msg->estimated_time_remaining = estimated_time_remaining;
-  } catch (...) {
-    // Ignore
+  } catch (std::exception & e) {
+    RCLCPP_ERROR(
+      logger_, "Exception caught while calculating distance and time remaining: %s", e.what());
   }
 
   int recovery_count = 0;
