Fixed #1207

introlab · Sep 10, 2024 · 28f9974 · 28f9974
1 parent 9deb8a2
commit 28f9974
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 2 deletions.
diff --git 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<GoalHandleNav2::SharedPtr> 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<NavigateToPose>::SharedPtr nav2Client_;
+	rclcpp_action::GoalUUID lastGoalSent_;
 #endif
 
 	std::thread* transformThread_;

diff --git 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...",