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

Updated azimut3 nav parameters, fixed last goal not sent when distance between last goal and transform to goal is < goalReachedRadius

Browse Source
This commit is contained in:
matlabbe
2015-03-18 18:00:30 -04:00
parent a0ae280b74
commit c58a11da0b
3 changed files with 29 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+4 -2
View File
@@ -1070,7 +1070,9 @@ void CoreWrapper::process(
bool lastPoseModified = false;
if(rtabmap_.getPathCurrentGoalId() == rtabmap_.getPath().back().first && rtabmap_.getLocalOptimizedPoses().size())
{
if(latestNodeWasReached_ || rtabmap_.getLocalOptimizedPoses().rbegin()->second.getDistance(currentMetricGoal_) < rtabmap_.getGoalReachedRadius())
if(latestNodeWasReached_ ||
rtabmap_.getLocalOptimizedPoses().rbegin()->second.getDistance(currentMetricGoal_) < rtabmap_.getGoalReachedRadius() ||
rtabmap_.getPathTransformToGoal().getNorm() < rtabmap_.getGoalReachedRadius())
{
if(!latestNodeWasReached_)
{
@@ -2372,7 +2374,7 @@ void CoreWrapper::goalDoneCb(const actionlib::SimpleClientGoalState& state,
{
if(rtabmap_.getPath().size() &&
rtabmap_.getPathCurrentGoalId() != rtabmap_.getPath().back().first &&
!uContains(rtabmap_.getLocalOptimizedPoses(), rtabmap_.getPath().back().first))
(!uContains(rtabmap_.getLocalOptimizedPoses(), rtabmap_.getPath().back().first) || !latestNodeWasReached_))
{
ROS_WARN("Planning: move_base reached current goal but it is not "
"the last one planned by rtabmap. A new goal should be sent when "