UAV-develop/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-07-24 15:27:41 +08:00
Code Issues Packages Projects Releases Wiki Activity

test(navigator): cover corridor path solve and battery budget

Browse Source 
Unit tests for CorridorGraph::findPath/findPathToRallyPoint (nest-first, rally fallback, directed-edge correctness) and RTL::isWithinBatteryBudget (threshold and NaN handling).

Signed-off-by: Anil Kircaliali <anil.kircaliali@auterion.com>
This commit is contained in:
Anil Kircaliali
2026-07-22 20:48:16 -07:00
parent fa767cecbc
commit afd69f4a9c
2 changed files with 335 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
src/modules/navigator/test/CMakeLists.txt
View File

@@ -34,3 +34,4 @@
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/..) include_directories(${CMAKE_CURRENT_SOURCE_DIR}/..)
px4_add_functional_gtest(SRC test_mission_base.cpp LINKLIBS modules__navigator) px4_add_functional_gtest(SRC test_mission_base.cpp LINKLIBS modules__navigator)
px4_add_functional_gtest(SRC test_corridor_graph.cpp LINKLIBS modules__navigator)

334
src/modules/navigator/test/test_corridor_graph.cpp Normal file
View File

@@ -0,0 +1,334 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file test_corridor_graph.cpp
*
* Unit tests for CorridorGraph::findPath(): nest-first / rally-fallback goal
* selection and directed-edge correctness.
*/
#include <gtest/gtest.h>
#include "corridor_graph.h"
#include "rtl.h"
#include <dataman_client/DatamanClient.hpp>
#include <lib/parameters/param.h>
#include <px4_platform_common/px4_work_queue/WorkQueueManager.hpp>
#include <cmath>
#include <unistd.h>
#include <vector>
extern "C" int dataman_main(int argc, char *argv[]);
namespace
{
class CorridorGraphDatamanRuntime
{
public:
CorridorGraphDatamanRuntime()
{
param_control_autosave(false);
px4::WorkQueueManagerStart();
char name[] = "dataman";
char start[] = "start";
char ram[] = "-r";
char *argv[] = {name, start, ram};
dataman_main(3, argv);
}
~CorridorGraphDatamanRuntime()
{
param_control_autosave(true);
char name[] = "dataman";
char stop[] = "stop";
char *argv[] = {name, stop};
dataman_main(2, argv);
px4::WorkQueueManagerStop();
}
};
CorridorGraphDatamanRuntime &corridorGraphDatamanRuntime()
{
static CorridorGraphDatamanRuntime runtime{};
return runtime;
}
// CorridorGraph's constructor creates DatamanClient members that register with the
// dataman work queue immediately; dataman must already be running before that happens,
// not just before the first read/write. A global environment guarantees this runs once,
// before any TEST body (and its CorridorGraph objects) executes.
class CorridorGraphDatamanEnvironment : public ::testing::Environment
{
public:
void SetUp() override { corridorGraphDatamanRuntime(); }
};
testing::Environment *const kCorridorGraphDatamanEnv =
testing::AddGlobalTestEnvironment(new CorridorGraphDatamanEnvironment());
mission_corridor_node_s makeNode(double lat, double lon, float alt, CorridorNodeType type)
{
mission_corridor_node_s node{};
node.lat = lat;
node.lon = lon;
node.alt = alt;
node.type = static_cast<uint8_t>(type);
return node;
}
mission_corridor_edge_s makeEdge(uint16_t from, uint16_t to, float static_cost = 1.0f)
{
mission_corridor_edge_s edge{};
edge.node_from = from;
edge.node_to = to;
edge.static_cost = static_cost;
return edge;
}
// Writes a corridor graph into dataman buffer 0 and returns once CorridorGraph
// has finished (re)loading it.
void loadGraph(CorridorGraph &graph, const std::vector<mission_corridor_node_s> &nodes,
const std::vector<mission_corridor_edge_s> &edges)
{
corridorGraphDatamanRuntime();
DatamanClient client;
for (size_t i = 0; i < nodes.size(); i++) {
mission_corridor_node_s node = nodes[i];
ASSERT_TRUE(client.writeSync(DM_KEY_CORRIDOR_NODES_0, i, reinterpret_cast<uint8_t *>(&node),
sizeof(node)));
}
for (size_t i = 0; i < edges.size(); i++) {
mission_corridor_edge_s edge = edges[i];
ASSERT_TRUE(client.writeSync(DM_KEY_CORRIDOR_EDGES_0, i, reinterpret_cast<uint8_t *>(&edge),
sizeof(edge)));
}
static uint32_t graph_id = 0;
graph_id++;
mission_stats_entry_s node_stats{};
node_stats.num_items = static_cast<uint16_t>(nodes.size());
node_stats.dataman_id = static_cast<uint8_t>(DM_KEY_CORRIDOR_NODES_0);
node_stats.opaque_id = graph_id;
ASSERT_TRUE(client.writeSync(DM_KEY_CORRIDOR_NODES_STATE, 0, reinterpret_cast<uint8_t *>(&node_stats),
sizeof(node_stats)));
mission_stats_entry_s edge_stats{};
edge_stats.num_items = static_cast<uint16_t>(edges.size());
edge_stats.dataman_id = static_cast<uint8_t>(DM_KEY_CORRIDOR_EDGES_0);
edge_stats.opaque_id = graph_id;
ASSERT_TRUE(client.writeSync(DM_KEY_CORRIDOR_EDGES_STATE, 0, reinterpret_cast<uint8_t *>(&edge_stats),
sizeof(edge_stats)));
graph.updateGraph();
for (int i = 0; i < 2000 && !graph.isLoaded(); i++) {
graph.run();
usleep(1000);
}
ASSERT_TRUE(graph.isLoaded());
ASSERT_EQ(graph.numNodes(), nodes.size());
ASSERT_EQ(graph.numEdges(), edges.size());
}
} // namespace
// Small triangle: 0 = waypoint (start), 1 = nest, 2 = rally point.
// Coordinates are ~100m apart so nearest-node search is unambiguous.
static const mission_corridor_node_s kWaypoint = makeNode(47.000, 8.000, 50.f, CorridorNodeType::Waypoint);
static const mission_corridor_node_s kNest = makeNode(47.001, 8.000, 50.f, CorridorNodeType::Nest);
static const mission_corridor_node_s kRally = makeNode(47.002, 8.000, 50.f, CorridorNodeType::RallyPoint);
TEST(CorridorGraphTest, ReturnsFalseWhenNotLoaded)
{
CorridorGraph graph(nullptr);
mission_corridor_node_s waypoints[8];
uint8_t num_waypoints = 0;
EXPECT_FALSE(graph.findPath(47.0, 8.0, 50.f, waypoints, num_waypoints, 8));
EXPECT_EQ(num_waypoints, 0);
}
TEST(CorridorGraphTest, FindsNestWhenReachable)
{
CorridorGraph graph(nullptr);
loadGraph(graph, {kWaypoint, kNest, kRally}, {makeEdge(0, 1, 10.f)});
mission_corridor_node_s waypoints[8];
uint8_t num_waypoints = 0;
ASSERT_TRUE(graph.findPath(kWaypoint.lat, kWaypoint.lon, kWaypoint.alt, waypoints, num_waypoints, 8));
ASSERT_EQ(num_waypoints, 2);
EXPECT_EQ(waypoints[0].type, static_cast<uint8_t>(CorridorNodeType::Waypoint));
EXPECT_EQ(waypoints[1].type, static_cast<uint8_t>(CorridorNodeType::Nest));
}
TEST(CorridorGraphTest, FallsBackToNearestReachableRallyPointWhenNestUnreachable)
{
CorridorGraph graph(nullptr);
// No edge leads to the nest (index 1); only the rally point (index 2) is reachable.
loadGraph(graph, {kWaypoint, kNest, kRally}, {makeEdge(0, 2, 10.f)});
mission_corridor_node_s waypoints[8];
uint8_t num_waypoints = 0;
ASSERT_TRUE(graph.findPath(kWaypoint.lat, kWaypoint.lon, kWaypoint.alt, waypoints, num_waypoints, 8));
ASSERT_EQ(num_waypoints, 2);
EXPECT_EQ(waypoints[0].type, static_cast<uint8_t>(CorridorNodeType::Waypoint));
EXPECT_EQ(waypoints[1].type, static_cast<uint8_t>(CorridorNodeType::RallyPoint));
}
TEST(CorridorGraphTest, ReturnsFalseWhenNothingReachable)
{
CorridorGraph graph(nullptr);
// Edges only lead back to the waypoint, not out of it.
loadGraph(graph, {kWaypoint, kNest, kRally}, {makeEdge(1, 0, 10.f), makeEdge(2, 0, 10.f)});
mission_corridor_node_s waypoints[8];
uint8_t num_waypoints = 0;
EXPECT_FALSE(graph.findPath(kWaypoint.lat, kWaypoint.lon, kWaypoint.alt, waypoints, num_waypoints, 8));
EXPECT_EQ(num_waypoints, 0);
}
TEST(CorridorGraphTest, RespectsDirectedEdgeDirection)
{
CorridorGraph graph(nullptr);
// Only a nest -> waypoint edge exists (the reverse of what RTL needs); there is no
// waypoint -> nest edge. A direction-agnostic (or backwards) Dijkstra call would
// incorrectly report the nest as reachable from the waypoint here.
loadGraph(graph, {kWaypoint, kNest}, {makeEdge(1, 0, 5.f)});
mission_corridor_node_s waypoints[8];
uint8_t num_waypoints = 0;
EXPECT_FALSE(graph.findPath(kWaypoint.lat, kWaypoint.lon, kWaypoint.alt, waypoints, num_waypoints, 8));
EXPECT_EQ(num_waypoints, 0);
}
TEST(CorridorGraphTest, FindPathToRallyPointIgnoresReachableNest)
{
CorridorGraph graph(nullptr);
// Both the nest and the rally point are reachable; findPathToRallyPoint() must
// still target the rally point, unlike findPath()'s nest-preferred behavior.
loadGraph(graph, {kWaypoint, kNest, kRally}, {makeEdge(0, 1, 10.f), makeEdge(0, 2, 5.f)});
mission_corridor_node_s waypoints[8];
uint8_t num_waypoints = 0;
ASSERT_TRUE(graph.findPathToRallyPoint(kWaypoint.lat, kWaypoint.lon, kWaypoint.alt, waypoints, num_waypoints, 8));
ASSERT_EQ(num_waypoints, 2);
EXPECT_EQ(waypoints[0].type, static_cast<uint8_t>(CorridorNodeType::Waypoint));
EXPECT_EQ(waypoints[1].type, static_cast<uint8_t>(CorridorNodeType::RallyPoint));
}
TEST(CorridorGraphTest, FindPathToRallyPointReturnsFalseWhenNoneReachable)
{
CorridorGraph graph(nullptr);
// Only the nest has an inbound edge; no rally point is reachable from the waypoint.
loadGraph(graph, {kWaypoint, kNest, kRally}, {makeEdge(0, 1, 10.f)});
mission_corridor_node_s waypoints[8];
uint8_t num_waypoints = 0;
EXPECT_FALSE(graph.findPathToRallyPoint(kWaypoint.lat, kWaypoint.lon, kWaypoint.alt, waypoints, num_waypoints, 8));
EXPECT_EQ(num_waypoints, 0);
}
TEST(CorridorGraphTest, MultiHopPathIncludesIntermediateWaypoints)
{
CorridorGraph graph(nullptr);
// The nest (index 3) is only reachable via a two-hop path through an intermediate
// waypoint (index 1); a direct, cheaper edge to the rally point (index 2) exists
// too, but the nest must still win since it's reachable. This pins down that the
// reconstructed path includes the intermediate hop rather than just start+goal.
const mission_corridor_node_s waypoint2 = makeNode(47.0005, 8.000, 50.f, CorridorNodeType::Waypoint);
loadGraph(graph, {kWaypoint, waypoint2, kRally, kNest}, {
makeEdge(0, 1, 1.f),
makeEdge(1, 3, 1.f),
makeEdge(0, 2, 1.f), // reaches only the rally point, but not the nest
});
mission_corridor_node_s waypoints[8];
uint8_t num_waypoints = 0;
ASSERT_TRUE(graph.findPath(kWaypoint.lat, kWaypoint.lon, kWaypoint.alt, waypoints, num_waypoints, 8));
ASSERT_EQ(num_waypoints, 3);
EXPECT_EQ(waypoints[0].type, static_cast<uint8_t>(CorridorNodeType::Waypoint));
EXPECT_EQ(waypoints[1].type, static_cast<uint8_t>(CorridorNodeType::Waypoint));
EXPECT_EQ(waypoints[2].type, static_cast<uint8_t>(CorridorNodeType::Nest));
}
// RTL::isWithinBatteryBudget() is the pure decision that gates RTL_TYPE=7's nest-vs-rally
// choice on remaining battery time (see RTL::tryFindCorridorPath()). It's covered here
// directly with fabricated inputs since driving a precise battery time estimate through
// SITL isn't practical.
TEST(RtlIsWithinBatteryBudgetTest, WithinBudgetWhenEstimateIsLessThanRemaining)
{
rtl_time_estimate_s estimate{};
estimate.valid = true;
estimate.safe_time_estimate = 100.f;
EXPECT_TRUE(RTL::isWithinBatteryBudget(estimate, 150.f));
}
TEST(RtlIsWithinBatteryBudgetTest, NotWithinBudgetWhenEstimateExceedsRemaining)
{
rtl_time_estimate_s estimate{};
estimate.valid = true;
estimate.safe_time_estimate = 200.f;
EXPECT_FALSE(RTL::isWithinBatteryBudget(estimate, 150.f));
}
TEST(RtlIsWithinBatteryBudgetTest, NotWithinBudgetWhenEstimateIsInvalid)
{
rtl_time_estimate_s estimate{};
estimate.valid = false;
estimate.safe_time_estimate = 10.f;
EXPECT_FALSE(RTL::isWithinBatteryBudget(estimate, 150.f));
}
TEST(RtlIsWithinBatteryBudgetTest, NotWithinBudgetWhenBatteryRemainingIsNan)
{
rtl_time_estimate_s estimate{};
estimate.valid = true;
estimate.safe_time_estimate = 10.f;
EXPECT_FALSE(RTL::isWithinBatteryBudget(estimate, std::nanf("")));
}