diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp index 072c774aab2..f31a2e740a9 100644 --- a/modules/navigation/nav_map.cpp +++ b/modules/navigation/nav_map.cpp @@ -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 raw_obstacles; + std::vector &raw_obstacles = rvo_simulation_2d.obstacles_; raw_obstacles.reserve(obstacle_vertex_count); // The following block is modified copy from RVO2D::AddObstacle() @@ -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)); } diff --git a/tests/servers/test_navigation_server_3d.h b/tests/servers/test_navigation_server_3d.h index 691536da8e7..5ab2975b740 100644 --- a/tests/servers/test_navigation_server_3d.h +++ b/tests/servers/test_navigation_server_3d.h @@ -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.