Skip to content

Commit

Permalink
enforce tolerance in bounds
Browse files Browse the repository at this point in the history
  • Loading branch information
@renan028
renan028 committed Aug 29, 2023
1 parent a9bc735 commit b8535f5
Show file tree
Hide file tree
Showing 2 changed files with 50 additions and 27 deletions.
43 changes: 18 additions & 25 deletions mbf_costmap_nav/src/mbf_costmap_nav/free_pose_search.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -250,39 +250,33 @@ SearchSolution FreePoseSearch::search() const
goal_cell.cost = costmap2d->getCost(goal_cell.x, goal_cell.y);
costmap2d->mapToWorld(goal_cell.x, goal_cell.y, goal_cell_pose.x, goal_cell_pose.y);

// if goal in bounds, we start the search from the goal pose (not goal_cell_pose) and continue from the goal cell
bool goal_pose_tested{ false };
bool test_goal_pose{ true };
unsigned int dummy_x, dummy_y;
if (costmap2d->worldToMap(config_.goal.x, config_.goal.y, dummy_x, dummy_y))
if (!costmap2d->worldToMap(config_.goal.x, config_.goal.y, dummy_x, dummy_y))
{
// check if the start cell is within tolerance and add it to the queue
if (std::hypot(goal_cell_pose.x - config_.goal.x, goal_cell_pose.y - config_.goal.y) <= config_.linear_tolerance)
{
queue.push(goal_cell);
in_queue_or_visited.insert(costmap2d->getIndex(goal_cell.x, goal_cell.y));
}
// if goal not in bounds, we don't start the search from the goal pose
test_goal_pose = false;
}
else

// add goal cell to queue if it is within linear tolerance
if (std::hypot(goal_cell_pose.x - config_.goal.x, goal_cell_pose.y - config_.goal.y) <= config_.linear_tolerance)
{
// if out of bounds, we start the search from the goal cell
goal_pose_tested = true;
queue.push(goal_cell);
in_queue_or_visited.insert(costmap2d->getIndex(goal_cell.x, goal_cell.y));
}

SearchSolution sol;
sol.pose.theta = config_.goal.theta;
SearchSolution unknow_outside_sol;
bool outside_or_unknown{ false };
while (!queue.empty() || !goal_pose_tested)
std::optional<SearchSolution> no_info_sol;
while (!queue.empty() || test_goal_pose)
{
Cell test_cell;
if (!goal_pose_tested)
if (test_goal_pose)
{
test_cell = goal_cell;
sol.pose.x = config_.goal.x;
sol.pose.y = config_.goal.y;
goal_pose_tested = true;
test_goal_pose = false;
}
else
{
Expand Down Expand Up @@ -314,13 +308,12 @@ SearchSolution FreePoseSearch::search() const
// if the state is outside or unknown, we save the first one we find
if ((tested_sol.search_state.state == SearchState::OUTSIDE ||
tested_sol.search_state.state == SearchState::UNKNOWN) &&
!outside_or_unknown)
!no_info_sol)
{
ROS_DEBUG_STREAM_NAMED(LOGNAME.data(), "Found unknown/outside pose: " << tested_sol.pose.x << ", "
<< tested_sol.pose.y << ", "
<< tested_sol.pose.theta);
unknow_outside_sol = tested_sol;
outside_or_unknown = true;
no_info_sol = tested_sol;
}

ROS_DEBUG_STREAM_COND_NAMED(tested_sol.search_state.state == SearchState::LETHAL, LOGNAME.data(),
Expand Down Expand Up @@ -361,19 +354,19 @@ SearchSolution FreePoseSearch::search() const
}
} // end while

if (outside_or_unknown)
if (no_info_sol)
{
// the solution is a no information pose or outside
ROS_DEBUG_STREAM_COND_NAMED(unknow_outside_sol.search_state.state == SearchState::UNKNOWN, LOGNAME.data(),
ROS_DEBUG_STREAM_COND_NAMED(no_info_sol->search_state.state == SearchState::UNKNOWN, LOGNAME.data(),
"The best solution found has NO_INFORMATION cost");
ROS_DEBUG_STREAM_COND_NAMED(unknow_outside_sol.search_state.state == SearchState::OUTSIDE, LOGNAME.data(),
ROS_DEBUG_STREAM_COND_NAMED(no_info_sol->search_state.state == SearchState::OUTSIDE, LOGNAME.data(),
"The best solution found is OUTSIDE the map");
if (viz_)
{
viz_->addSolution(unknow_outside_sol.pose, footprint);
viz_->addSolution(no_info_sol->pose, footprint);
viz_->publish();
}
return unknow_outside_sol;
return no_info_sol.value();
}

ROS_DEBUG_STREAM("No solution found within tolerance of goal; ending search");
Expand Down
34 changes: 32 additions & 2 deletions mbf_costmap_nav/test/free_pose_search_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -456,6 +456,36 @@ TEST_F(SearchHelperTest, service_test)
server.stop();
}

TEST_F(SearchHelperTest, enforce_bounds_no_tolerance)
{
ros::NodeHandle nh;
costmap_2d::Costmap2DROS cm("search/global", *tf_buffer_ptr);
FreePoseSearchViz viz(nh, cm.getGlobalFrameID());

printMap(*(cm.getCostmap()));

/*
y/x 0.5 1.5 2.5 3.5 4.5 5.5 6.5 7.5 8.5 9.5
----G-------------------------------------------------------
0.5 | 0 0 0 0 0 0 0 254 254 254
1.5 | 0 0 0 0 0 0 0 254 254 254
2.5 | 0 0 0 254 254 254 0 0 0 0
3.5 | 0 0 0 0 0 0 0 0 0 0
4.5 | 0 0 0 0 0 0 0 0 0 0
5.5 | 0 0 0 0 254 0 0 254 254 254
6.5 | 0 0 0 0 254 0 0 254 254 254
7.5 | 0 0 0 0 0 0 0 254 254 254
8.5 | 0 0 0 0 0 0 0 0 0 0
9.5 | 0 0 0 0 0 0 0 0 0 0
*/

SearchConfig config{ M_PI_4, M_PI, 0.5, false, 0.0, toPose2D(-1, -1, 0) };
FreePoseSearch sh(cm, config, std::nullopt, viz);

auto sol = sh.search();
EXPECT_EQ(sol.search_state.state, SearchState::LETHAL);
}

TEST_F(SearchHelperTest, enforce_bounds)
{
ros::NodeHandle nh;
Expand All @@ -479,7 +509,7 @@ TEST_F(SearchHelperTest, enforce_bounds)
9.5 | 0 0 0 0 0 0 0 0 0 0
*/

SearchConfig config{ M_PI_4, M_PI, 0.5, false, 0.0, toPose2D(-1, -1, 0) };
SearchConfig config{ M_PI_4, M_PI, std::sqrt(2 * 1.5 * 1.5), false, 0.0, toPose2D(-1, -1, 0) };
FreePoseSearch sh(cm, config, std::nullopt, viz);

auto sol = sh.search();
Expand All @@ -489,7 +519,7 @@ TEST_F(SearchHelperTest, enforce_bounds)
EXPECT_NEAR(sol.pose.theta, 0, 1e-6);
}

TEST_F(SearchHelperTest, enforce_bounds_with_tolerance)
TEST_F(SearchHelperTest, enforce_bounds_within_tolerance)
{
ros::NodeHandle nh;
costmap_2d::Costmap2DROS cm("search/global", *tf_buffer_ptr);
Expand Down

0 comments on commit b8535f5

Please sign in to comment.