Merge remote-tracking branch 'upstream/main' into remove-in-collision…

…-goals

Signed-off-by: Tony Najjar <[email protected]>

nav2_amcl/src/amcl_node.cpp

@@ -537,8 +537,8 @@ AmclNode::initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::Sha
       global_frame_id_.c_str());
     return;
   }
-  if (abs(msg->pose.pose.position.x) > map_->size_x ||
-    abs(msg->pose.pose.position.y) > map_->size_y)
+  if (first_map_received_ && (abs(msg->pose.pose.position.x) > map_->size_x ||
+    abs(msg->pose.pose.position.y) > map_->size_y))
   {
     RCLCPP_ERROR(
       get_logger(), "Received initialpose from message is out of the size of map. Rejecting.");

nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp

@@ -19,6 +19,7 @@
 #include <memory>
 #include <string>
 #include <mutex>
+#include <functional>
 
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_util/lifecycle_node.hpp"

nav2_docking/opennav_docking/include/opennav_docking/navigator.hpp

@@ -18,6 +18,7 @@
 #include <vector>
 #include <memory>
 #include <string>
+#include <functional>
 
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
@@ -67,11 +68,14 @@ class Navigator
    * May throw exception if fails to navigate or communicate
    * Blocks until completion.
    * @param pose Pose to go to
-   * @param max_staging_duration Maximum time to get to the staging pose
+   * @param remaining_staging_duration Remaining time to get to the staging pose
+   * @param isPreempted Function to check if preempted
+   * @param recursed True if recursed (used to retry once)
    */
   void goToPose(
     const geometry_msgs::msg::PoseStamped & pose,
-    const rclcpp::Duration & max_staging_duration,
+    rclcpp::Duration remaining_staging_duration,
+    std::function<bool()> isPreempted,
     bool recursed = false);
 
 protected:

nav2_docking/opennav_docking/src/docking_server.cpp

@@ -253,8 +253,15 @@ void DockingServer::dockRobot()
     {
       RCLCPP_INFO(get_logger(), "Robot already within pre-staging pose tolerance for dock");
     } else {
+      std::function<bool()> isPreempted = [this]() {
+          return checkAndWarnIfCancelled(docking_action_server_, "dock_robot") ||
+                 checkAndWarnIfPreempted(docking_action_server_, "dock_robot");
+        };
+
       navigator_->goToPose(
-        initial_staging_pose, rclcpp::Duration::from_seconds(goal->max_staging_time));
+        initial_staging_pose,
+        rclcpp::Duration::from_seconds(goal->max_staging_time),
+        isPreempted);
       RCLCPP_INFO(get_logger(), "Successful navigation to staging pose");
     }
 

nav2_docking/opennav_docking/src/navigator.cpp

@@ -51,13 +51,16 @@ void Navigator::deactivate()
 
 void Navigator::goToPose(
   const geometry_msgs::msg::PoseStamped & pose,
-  const rclcpp::Duration & max_staging_duration,
+  rclcpp::Duration remaining_staging_duration,
+  std::function<bool()> isPreempted,
   bool recursed)
 {
+  auto node = node_.lock();
+
   Nav2Pose::Goal goal;
   goal.pose = pose;
   goal.behavior_tree = navigator_bt_xml_;
-  const auto timeout = max_staging_duration.to_chrono<std::chrono::milliseconds>();
+  const auto start_time = node->now();
 
   // Wait for server to be active
   nav_to_pose_client_->wait_for_action_server(1s);
@@ -66,19 +69,41 @@ void Navigator::goToPose(
       future_goal_handle, 2s) == rclcpp::FutureReturnCode::SUCCESS)
   {
     auto future_result = nav_to_pose_client_->async_get_result(future_goal_handle.get());
-    if (executor_.spin_until_future_complete(
-        future_result, timeout) == rclcpp::FutureReturnCode::SUCCESS)
-    {
-      auto result = future_result.get();
-      if (result.code == rclcpp_action::ResultCode::SUCCEEDED && result.result->error_code == 0) {
-        return;  // Success!
+
+    while (rclcpp::ok()) {
+      if (isPreempted()) {
+        auto cancel_future = nav_to_pose_client_->async_cancel_goal(future_goal_handle.get());
+        executor_.spin_until_future_complete(cancel_future, 1s);
+        throw opennav_docking_core::FailedToStage("Navigation request to staging pose preempted.");
+      }
+
+      if (node->now() - start_time > remaining_staging_duration) {
+        auto cancel_future = nav_to_pose_client_->async_cancel_goal(future_goal_handle.get());
+        executor_.spin_until_future_complete(cancel_future, 1s);
+        throw opennav_docking_core::FailedToStage("Navigation request to staging pose timed out.");
+      }
+
+      if (executor_.spin_until_future_complete(
+          future_result, 10ms) == rclcpp::FutureReturnCode::SUCCESS)
+      {
+        auto result = future_result.get();
+        if (result.code == rclcpp_action::ResultCode::SUCCEEDED &&
+          result.result->error_code == 0)
+        {
+          return;  // Success!
+        } else {
+          RCLCPP_WARN(node->get_logger(), "Navigation request to staging pose failed.");
+          break;
+        }
       }
     }
   }
 
   // Attempt to retry once using single iteration recursion
   if (!recursed) {
-    goToPose(pose, max_staging_duration, true);
+    auto elapsed_time = node->now() - start_time;
+    remaining_staging_duration = remaining_staging_duration - elapsed_time;
+    goToPose(pose, remaining_staging_duration, isPreempted, true);
     return;
   }
 

nav2_docking/opennav_docking/test/test_navigator.cpp

@@ -12,6 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <functional>
+
 #include "gtest/gtest.h"
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_util/simple_action_server.hpp"
@@ -94,26 +96,40 @@ TEST(NavigatorTests, TestNavigator)
   auto navigator = std::make_unique<Navigator>(node);
   navigator->activate();
 
+  std::function<bool()> is_preempted_true = []() {return true;};
+  std::function<bool()> is_preempted_false = []() {return false;};
+
   // Should succeed, action server set to succeed
   dummy_navigator_node->setReturn(true);
   EXPECT_NO_THROW(
-    navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0)));
+    navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0),
+      is_preempted_false));
 
   // Should fail, timeout exceeded
   EXPECT_THROW(
-    navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(0.0, 0.0)),
+    navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(0.0, 0.0),
+      is_preempted_false),
     opennav_docking_core::FailedToStage);
 
   // Should fail, action server set to succeed
   dummy_navigator_node->setReturn(false);
   EXPECT_THROW(
-    navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0)),
+    navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0),
+      is_preempted_false),
     opennav_docking_core::FailedToStage);
 
   // First should fail, recursion should succeed
   dummy_navigator_node->setToggle();
   EXPECT_NO_THROW(
-    navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0)));
+    navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0),
+      is_preempted_false));
+
+  // Should fail, preempted
+  dummy_navigator_node->setReturn(true);
+  EXPECT_THROW(
+    navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0),
+      is_preempted_true),
+    opennav_docking_core::FailedToStage);
 
   navigator->deactivate();
   navigator.reset();

nav2_planner/CMakeLists.txt

@@ -2,71 +2,62 @@ cmake_minimum_required(VERSION 3.5)
 project(nav2_planner)
 
 find_package(ament_cmake REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(lifecycle_msgs REQUIRED)
 find_package(nav2_common REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(rclcpp_action REQUIRED)
-find_package(rclcpp_lifecycle REQUIRED)
-find_package(rclcpp_components REQUIRED)
-find_package(std_msgs REQUIRED)
-find_package(visualization_msgs REQUIRED)
-find_package(nav2_util REQUIRED)
+find_package(nav2_core REQUIRED)
+find_package(nav2_costmap_2d REQUIRED)
 find_package(nav2_msgs REQUIRED)
+find_package(nav2_util REQUIRED)
 find_package(nav_msgs REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(builtin_interfaces REQUIRED)
-find_package(tf2_ros REQUIRED)
-find_package(nav2_costmap_2d REQUIRED)
 find_package(pluginlib REQUIRED)
-find_package(nav2_core REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
 
 nav2_package()
 
-include_directories(
-  include
-)
-
 set(executable_name planner_server)
 set(library_name ${executable_name}_core)
 
-set(dependencies
-  rclcpp
-  rclcpp_action
-  rclcpp_lifecycle
-  rclcpp_components
-  std_msgs
-  visualization_msgs
-  nav2_util
-  nav2_msgs
-  nav_msgs
-  geometry_msgs
-  builtin_interfaces
-  tf2_ros
-  nav2_costmap_2d
-  pluginlib
-  nav2_core
-)
-
 add_library(${library_name} SHARED
   src/planner_server.cpp
 )
-
-ament_target_dependencies(${library_name}
-  ${dependencies}
+target_include_directories(${library_name}
+  PUBLIC
+    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+)
+target_link_libraries(${library_name} PUBLIC
+  ${geometry_msgs_TARGETS}
+  nav2_core::nav2_core
+  nav2_costmap_2d::nav2_costmap_2d_core
+  ${nav2_msgs_TARGETS}
+  nav2_util::nav2_util_core
+  ${nav_msgs_TARGETS}
+  pluginlib::pluginlib
+  rclcpp::rclcpp
+  rclcpp_lifecycle::rclcpp_lifecycle
+  ${rcl_interfaces_TARGETS}
+  tf2_ros::tf2_ros
+)
+target_link_libraries(${library_name} PRIVATE
+  ${lifecycle_msgs_TARGETS}
+  rclcpp_components::component
+  tf2::tf2
 )
 
 add_executable(${executable_name}
   src/main.cpp
 )
-
-target_link_libraries(${executable_name} ${library_name})
-
-ament_target_dependencies(${executable_name}
-  ${dependencies}
-)
+target_link_libraries(${executable_name} PRIVATE ${library_name} rclcpp::rclcpp)
 
 rclcpp_components_register_nodes(${library_name} "nav2_planner::PlannerServer")
 
 install(TARGETS ${library_name}
+  EXPORT ${library_name}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
@@ -77,17 +68,31 @@ install(TARGETS ${executable_name}
 )
 
 install(DIRECTORY include/
-  DESTINATION include/
+  DESTINATION include/${PROJECT_NAME}
 )
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   find_package(ament_cmake_gtest REQUIRED)
   ament_lint_auto_find_test_dependencies()
+
+  ament_find_gtest()
   add_subdirectory(test)
 endif()
 
-ament_export_include_directories(include)
+ament_export_include_directories(include/${PROJECT_NAME})
 ament_export_libraries(${library_name})
-ament_export_dependencies(${dependencies})
+ament_export_dependencies(
+  geometry_msgs
+  nav2_core
+  nav2_costmap_2d
+  nav2_msgs
+  nav2_util
+  nav_msgs
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  tf2_ros
+)
+ament_export_targets(${library_name})
 ament_package()

nav2_planner/include/nav2_planner/planner_server.hpp

@@ -31,7 +31,6 @@
 #include "nav2_msgs/msg/costmap.hpp"
 #include "nav2_util/robot_utils.hpp"
 #include "nav2_util/simple_action_server.hpp"
-#include "visualization_msgs/msg/marker.hpp"
 #include "tf2_ros/transform_listener.h"
 #include "tf2_ros/create_timer_ros.h"
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"

nav2_planner/package.xml

@@ -8,22 +8,23 @@
   <license>Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>nav2_common</build_depend>
 
-  <depend>rclcpp</depend>
-  <depend>rclcpp_action</depend>
-  <depend>rclcpp_lifecycle</depend>
-  <depend>visualization_msgs</depend>
-  <depend>nav2_util</depend>
-  <depend>nav2_msgs</depend>
-  <depend>nav_msgs</depend>
   <depend>geometry_msgs</depend>
-  <depend>builtin_interfaces</depend>
-  <depend>nav2_common</depend>
-  <depend>tf2_ros</depend>
+  <depend>lifecycle_msgs</depend>
+  <depend>nav2_core</depend>
   <depend>nav2_costmap_2d</depend>
+  <depend>nav2_msgs</depend>
+  <depend>nav2_util</depend>
+  <depend>nav_msgs</depend>
   <depend>pluginlib</depend>
-  <depend>nav2_core</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
 
+  <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>
 

nav2_planner/src/planner_server.cpp

@@ -24,7 +24,6 @@
 #include <vector>
 #include <utility>
 
-#include "builtin_interfaces/msg/duration.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
 #include "nav2_util/costmap.hpp"
 #include "nav2_util/node_utils.hpp"

nav2_planner/test/CMakeLists.txt

@@ -2,9 +2,9 @@
 ament_add_gtest(test_dynamic_parameters
   test_dynamic_parameters.cpp
 )
-ament_target_dependencies(test_dynamic_parameters
-  ${dependencies}
-)
 target_link_libraries(test_dynamic_parameters
   ${library_name}
+  nav2_util::nav2_util_core
+  rclcpp::rclcpp
+  ${rcl_interfaces_TARGETS}
 )