i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-09-13 23:00:19 +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/include/rtabmap_slam/CoreWrapper.h
+2 -1
View File
@@ -254,7 +254,7 @@ private:
#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 @@ private:
#endif
#ifdef WITH_NAV2_MSGS
rclcpp_action::Client<NavigateToPose>::SharedPtr nav2Client_;
rclcpp_action::GoalUUID lastGoalSent_;
#endif
std::thread* transformThread_;
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...",