From 28f997477aa80130da4c0915d4f0182280c1a9a7 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 9 Sep 2024 23:26:06 -0700 Subject: [PATCH] Fixed #1207 --- rtabmap_slam/include/rtabmap_slam/CoreWrapper.h | 3 ++- rtabmap_slam/src/CoreWrapper.cpp | 8 +++++++- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h index 7ac6825c4..97cabb605 100644 --- a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h +++ b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h @@ -254,7 +254,7 @@ class CoreWrapper : public rclcpp::Node, public rtabmap_sync::CommonDataSubscrib #ifdef NAV_MSGS_FOXY void goalResponseCallback(std::shared_future future); #else - void goalResponseCallback(const GoalHandleNav2::SharedPtr & goal_handle); + void goalResponseCallback(const GoalHandleNav2::SharedPtr & goal_handle); #endif void resultCallback(const GoalHandleNav2::WrappedResult & result); #endif @@ -381,6 +381,7 @@ class CoreWrapper : public rclcpp::Node, public rtabmap_sync::CommonDataSubscrib #endif #ifdef WITH_NAV2_MSGS rclcpp_action::Client::SharedPtr nav2Client_; + rclcpp_action::GoalUUID lastGoalSent_; #endif std::thread* transformThread_; diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index e3cd35489..a9da2ce59 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -4436,7 +4436,7 @@ void CoreWrapper::goalResponseCallback( { auto goal_handle = future.get(); #else - const GoalHandleNav2::SharedPtr & goal_handle) + const GoalHandleNav2::SharedPtr & goal_handle) { #endif if (!goal_handle) { @@ -4448,6 +4448,7 @@ void CoreWrapper::goalResponseCallback( latestNodeWasReached_ = false; } else { RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); + lastGoalSent_ = goal_handle->get_goal_id(); } } @@ -4473,6 +4474,11 @@ void CoreWrapper::resultCallback( RCLCPP_INFO(this->get_logger(), "Planning: nav2 success!"); } } + else if(result.code==rclcpp_action::ResultCode::ABORTED && result.goal_id != lastGoalSent_) + { + // Just ignored, it is from an old goal + ignore = true; + } else { RCLCPP_ERROR(this->get_logger(), "Planning: nav2 failed for some reason: %s. Aborting the plan...",