asgardius/virtualx-engine
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Merge pull request #84816 from Scony/add-obstacle-navserver-tests

Browse source 
Fix memory leak in 'NavigationServer3D' involving static obstacles
This commit is contained in:
Rémi Verschelde 2023-12-04 23:05:39 +01:00
commit 8a9e3ad8d5
No known key found for this signature in database
GPG key ID: C3336907360768E1
2 changed files with 111 additions and 1 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

13
modules/navigation/nav_map.cpp
View file

@ -1107,8 +1107,14 @@ void NavMap::_update_rvo_obstacles_tree_2d() {
obstacle_vertex_count += obstacle->get_vertices().size(); 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 // 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); raw_obstacles.reserve(obstacle_vertex_count);
// The following block is modified copy from RVO2D::AddObstacle() // The following block is modified copy from RVO2D::AddObstacle()
@ -1128,6 +1134,11 @@ void NavMap::_update_rvo_obstacles_tree_2d() {
real_t _obstacle_height = obstacle->get_height(); real_t _obstacle_height = obstacle->get_height();
for (const Vector3 &_obstacle_vertex : _obstacle_vertices) { 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)); rvo_2d_vertices.push_back(RVO2D::Vector2(_obstacle_vertex.x + _obstacle_position.x, _obstacle_vertex.z + _obstacle_position.z));
} }

99
tests/servers/test_navigation_server_3d.h
View file

@ -429,6 +429,105 @@ TEST_SUITE("[Navigation]") {
navigation_server->free(map); 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 #ifndef DISABLE_DEPRECATED
// This test case uses only public APIs on purpose - other test cases use simplified baking. // 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. // FIXME: Remove once deprecated `region_bake_navigation_mesh()` is removed.