Add doors to distance remaining

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

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

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>
 

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;