i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-06 17:57:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed #1207

Browse Source
This commit is contained in:
matlabbe
2024-09-09 23:26:06 -07:00
parent 9deb8a2a81
commit 28f997477a
2 changed files with 9 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_slam/src/CoreWrapper.cpp
+7 -1
View File
@@ -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...",