Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix memory leak in 'NavigationServer3D' involving static obstacles #84816

Merged
merged 1 commit into from
Dec 4, 2023
+111 −1
Merged

Fix memory leak in 'NavigationServer3D' involving static obstacles #84816

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 12 additions & 1 deletion modules/navigation/nav_map.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1107,8 +1107,14 @@ void NavMap::_update_rvo_obstacles_tree_2d() {
obstacle_vertex_count += obstacle->get_vertices().size();
}

// Cleaning old obstacles.
for (size_t i = 0; i < rvo_simulation_2d.obstacles_.size(); ++i) {
delete rvo_simulation_2d.obstacles_[i];
}
rvo_simulation_2d.obstacles_.clear();

// Cannot use LocalVector here as RVO library expects std::vector to build KdTree
std::vector<RVO2D::Obstacle2D *> raw_obstacles;
std::vector<RVO2D::Obstacle2D *> &raw_obstacles = rvo_simulation_2d.obstacles_;
raw_obstacles.reserve(obstacle_vertex_count);

// The following block is modified copy from RVO2D::AddObstacle()
Expand All @@ -1127,6 +1133,11 @@ void NavMap::_update_rvo_obstacles_tree_2d() {
uint32_t _obstacle_avoidance_layers = obstacle->get_avoidance_layers();

for (const Vector3 &_obstacle_vertex : _obstacle_vertices) {
#ifdef TOOLS_ENABLED
if (_obstacle_vertex.y != 0) {
WARN_PRINT_ONCE("Y coordinates of static obstacle vertices are ignored. Please use obstacle position Y to change elevation of obstacle.");
}
#endif
rvo_2d_vertices.push_back(RVO2D::Vector2(_obstacle_vertex.x + _obstacle_position.x, _obstacle_vertex.z + _obstacle_position.z));
}

Expand Down
99 changes: 99 additions & 0 deletions tests/servers/test_navigation_server_3d.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -429,6 +429,105 @@ TEST_SUITE("[Navigation]") {
navigation_server->free(map);
}

TEST_CASE("[NavigationServer3D] Server should make agents avoid dynamic obstacles when avoidance enabled") {
NavigationServer3D *navigation_server = NavigationServer3D::get_singleton();

RID map = navigation_server->map_create();
RID agent_1 = navigation_server->agent_create();
RID obstacle_1 = navigation_server->obstacle_create();

navigation_server->map_set_active(map, true);

navigation_server->agent_set_map(agent_1, map);
navigation_server->agent_set_avoidance_enabled(agent_1, true);
navigation_server->agent_set_position(agent_1, Vector3(0, 0, 0));
navigation_server->agent_set_radius(agent_1, 1);
navigation_server->agent_set_velocity(agent_1, Vector3(1, 0, 0));
CallableMock agent_1_avoidance_callback_mock;
navigation_server->agent_set_avoidance_callback(agent_1, callable_mp(&agent_1_avoidance_callback_mock, &CallableMock::function1));

navigation_server->obstacle_set_map(obstacle_1, map);
navigation_server->obstacle_set_avoidance_enabled(obstacle_1, true);
navigation_server->obstacle_set_position(obstacle_1, Vector3(2.5, 0, 0.5));
navigation_server->obstacle_set_radius(obstacle_1, 1);

CHECK_EQ(agent_1_avoidance_callback_mock.function1_calls, 0);
navigation_server->process(0.0); // Give server some cycles to commit.
CHECK_EQ(agent_1_avoidance_callback_mock.function1_calls, 1);
Vector3 agent_1_safe_velocity = agent_1_avoidance_callback_mock.function1_latest_arg0;
CHECK_MESSAGE(agent_1_safe_velocity.x > 0, "Agent 1 should move a bit along desired velocity (+X).");
CHECK_MESSAGE(agent_1_safe_velocity.z < 0, "Agent 1 should move a bit to the side so that it avoids obstacle.");

navigation_server->free(obstacle_1);
navigation_server->free(agent_1);
navigation_server->free(map);
navigation_server->process(0.0); // Give server some cycles to commit.
}

TEST_CASE("[NavigationServer3D] Server should make agents avoid static obstacles when avoidance enabled") {
NavigationServer3D *navigation_server = NavigationServer3D::get_singleton();

RID map = navigation_server->map_create();
RID agent_1 = navigation_server->agent_create();
RID agent_2 = navigation_server->agent_create();
RID obstacle_1 = navigation_server->obstacle_create();

navigation_server->map_set_active(map, true);

navigation_server->agent_set_map(agent_1, map);
navigation_server->agent_set_avoidance_enabled(agent_1, true);
navigation_server->agent_set_radius(agent_1, 1.6); // Have hit the obstacle already.
navigation_server->agent_set_velocity(agent_1, Vector3(1, 0, 0));
CallableMock agent_1_avoidance_callback_mock;
navigation_server->agent_set_avoidance_callback(agent_1, callable_mp(&agent_1_avoidance_callback_mock, &CallableMock::function1));

navigation_server->agent_set_map(agent_2, map);
navigation_server->agent_set_avoidance_enabled(agent_2, true);
navigation_server->agent_set_radius(agent_2, 1.4); // Haven't hit the obstacle yet.
navigation_server->agent_set_velocity(agent_2, Vector3(1, 0, 0));
CallableMock agent_2_avoidance_callback_mock;
navigation_server->agent_set_avoidance_callback(agent_2, callable_mp(&agent_2_avoidance_callback_mock, &CallableMock::function1));

navigation_server->obstacle_set_map(obstacle_1, map);
navigation_server->obstacle_set_avoidance_enabled(obstacle_1, true);
PackedVector3Array obstacle_1_vertices;

SUBCASE("Static obstacles should work on ground level") {
navigation_server->agent_set_position(agent_1, Vector3(0, 0, 0));
navigation_server->agent_set_position(agent_2, Vector3(0, 0, 5));
obstacle_1_vertices.push_back(Vector3(1.5, 0, 0.5));
obstacle_1_vertices.push_back(Vector3(1.5, 0, 4.5));
}

SUBCASE("Static obstacles should work when elevated") {
navigation_server->agent_set_position(agent_1, Vector3(0, 5, 0));
navigation_server->agent_set_position(agent_2, Vector3(0, 5, 5));
obstacle_1_vertices.push_back(Vector3(1.5, 0, 0.5));
obstacle_1_vertices.push_back(Vector3(1.5, 0, 4.5));
navigation_server->obstacle_set_position(obstacle_1, Vector3(0, 5, 0));
}

navigation_server->obstacle_set_vertices(obstacle_1, obstacle_1_vertices);

CHECK_EQ(agent_1_avoidance_callback_mock.function1_calls, 0);
CHECK_EQ(agent_2_avoidance_callback_mock.function1_calls, 0);
navigation_server->process(0.0); // Give server some cycles to commit.
CHECK_EQ(agent_1_avoidance_callback_mock.function1_calls, 1);
CHECK_EQ(agent_2_avoidance_callback_mock.function1_calls, 1);
Vector3 agent_1_safe_velocity = agent_1_avoidance_callback_mock.function1_latest_arg0;
Vector3 agent_2_safe_velocity = agent_2_avoidance_callback_mock.function1_latest_arg0;
CHECK_MESSAGE(agent_1_safe_velocity.x > 0, "Agent 1 should move a bit along desired velocity (+X).");
CHECK_MESSAGE(agent_1_safe_velocity.z < 0, "Agent 1 should move a bit to the side so that it avoids obstacle.");
CHECK_MESSAGE(agent_2_safe_velocity.x > 0, "Agent 2 should move a bit along desired velocity (+X).");
CHECK_MESSAGE(agent_2_safe_velocity.z == 0, "Agent 2 should not move to the side.");

navigation_server->free(obstacle_1);
navigation_server->free(agent_2);
navigation_server->free(agent_1);
navigation_server->free(map);
navigation_server->process(0.0); // Give server some cycles to commit.
}

#ifndef DISABLE_DEPRECATED
// This test case uses only public APIs on purpose - other test cases use simplified baking.
// FIXME: Remove once deprecated `region_bake_navigation_mesh()` is removed.
Expand Down
Loading