diff --git a/nav2_bt_navigator/CMakeLists.txt b/nav2_bt_navigator/CMakeLists.txt index cfe6a0d486..99a1f7dff3 100644 --- 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 index 98287b5be8..bc7fbf4854 100644 --- 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 #include +#include #include #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 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 index 2540ec06a7..b8e5ceac1f 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -20,6 +20,7 @@ behaviortree_cpp_v3 std_msgs geometry_msgs + service_robot_msgs std_srvs nav2_util pluginlib @@ -35,6 +36,7 @@ std_msgs nav2_util geometry_msgs + service_robot_msgs pluginlib nav2_core diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index 4f9f8e546e..6892619454 100644 --- 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 +#include #include #include #include @@ -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>>( + "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;