add humble-navigation2

2025-05-27 19:03:40 +08:00
parent 974abb5e1e
commit e74ec539c2
1280 changed files with 204114 additions and 0 deletions
@@ -0,0 +1,349 @@
+// Copyright (c) 2020, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include <math.h>
+#include <cmath>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_costmap_2d/costmap_subscriber.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_smac_planner/node_hybrid.hpp"
+#include "nav2_smac_planner/collision_checker.hpp"
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+TEST(NodeHybridTest, test_node_hybrid)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  nav2_smac_planner::SearchInfo info;
+  info.change_penalty = 0.1;
+  info.non_straight_penalty = 1.1;
+  info.reverse_penalty = 2.0;
+  info.minimum_turning_radius = 8;  // 0.4m/5cm resolution costmap
+  info.cost_penalty = 1.7;
+  info.retrospective_penalty = 0.1;
+  unsigned int size_x = 10;
+  unsigned int size_y = 10;
+  unsigned int size_theta = 72;
+
+  // Check defaulted constants
+  nav2_smac_planner::NodeHybrid testA(49);
+  EXPECT_EQ(testA.travel_distance_cost, sqrt(2));
+
+  nav2_smac_planner::NodeHybrid::initMotionModel(
+    nav2_smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info);
+
+  nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(
+    10, 10, 0.05, 0.0, 0.0, 0);
+  std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
+    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmapA, 72, node);
+  checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
+
+  // test construction
+  nav2_smac_planner::NodeHybrid testB(49);
+  EXPECT_TRUE(std::isnan(testA.getCost()));
+
+  // test node valid and cost
+  testA.pose.x = 5;
+  testA.pose.y = 5;
+  testA.pose.theta = 0;
+  EXPECT_EQ(testA.isNodeValid(true, checker.get()), true);
+  EXPECT_EQ(testA.isNodeValid(false, checker.get()), true);
+  EXPECT_EQ(testA.getCost(), 0.0f);
+
+  // test reset
+  testA.reset();
+  EXPECT_TRUE(std::isnan(testA.getCost()));
+
+  // Check motion-specific constants
+  EXPECT_NEAR(testA.travel_distance_cost, 2.08842, 0.1);
+
+  // check collision checking
+  EXPECT_EQ(testA.isNodeValid(false, checker.get()), true);
+
+  // check traversal cost computation
+  // simulated first node, should return neutral cost
+  EXPECT_NEAR(testB.getTraversalCost(&testA), 2.088, 0.1);
+  // now with straight motion, cost is 0, so will be neutral as well
+  // but now reduced by retrospective penalty (10%)
+  testB.setMotionPrimitiveIndex(1);
+  testA.setMotionPrimitiveIndex(0);
+  EXPECT_NEAR(testB.getTraversalCost(&testA), 2.088 * 0.9, 0.1);
+  // same direction as parent, testB
+  testA.setMotionPrimitiveIndex(1);
+  EXPECT_NEAR(testB.getTraversalCost(&testA), 2.297f * 0.9, 0.01);
+  // opposite direction as parent, testB
+  testA.setMotionPrimitiveIndex(2);
+  EXPECT_NEAR(testB.getTraversalCost(&testA), 2.506f * 0.9, 0.01);
+
+  // will throw because never collision checked testB
+  EXPECT_THROW(testA.getTraversalCost(&testB), std::runtime_error);
+
+  // check motion primitives
+  EXPECT_EQ(testA.getMotionPrimitiveIndex(), 2u);
+
+  // check operator== works on index
+  nav2_smac_planner::NodeHybrid testC(49);
+  EXPECT_TRUE(testA == testC);
+
+  // check accumulated costs are set
+  testC.setAccumulatedCost(100);
+  EXPECT_EQ(testC.getAccumulatedCost(), 100.0f);
+
+  // check visiting state
+  EXPECT_EQ(testC.wasVisited(), false);
+  testC.visited();
+  EXPECT_EQ(testC.wasVisited(), true);
+
+  // check index
+  EXPECT_EQ(testC.getIndex(), 49u);
+
+  // check set pose and pose
+  testC.setPose(nav2_smac_planner::NodeHybrid::Coordinates(10.0, 5.0, 4));
+  EXPECT_EQ(testC.pose.x, 10.0);
+  EXPECT_EQ(testC.pose.y, 5.0);
+  EXPECT_EQ(testC.pose.theta, 4);
+
+  // check static index functions
+  EXPECT_EQ(nav2_smac_planner::NodeHybrid::getIndex(1u, 1u, 4u, 10u, 72u), 796u);
+  EXPECT_EQ(nav2_smac_planner::NodeHybrid::getCoords(796u, 10u, 72u).x, 1u);
+  EXPECT_EQ(nav2_smac_planner::NodeHybrid::getCoords(796u, 10u, 72u).y, 1u);
+  EXPECT_EQ(nav2_smac_planner::NodeHybrid::getCoords(796u, 10u, 72u).theta, 4u);
+
+  delete costmapA;
+}
+
+TEST(NodeHybridTest, test_obstacle_heuristic)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  nav2_smac_planner::SearchInfo info;
+  info.change_penalty = 0.1;
+  info.non_straight_penalty = 1.1;
+  info.reverse_penalty = 2.0;
+  info.minimum_turning_radius = 8;  // 0.4m/5cm resolution costmap
+  info.cost_penalty = 1.7;
+  info.retrospective_penalty = 0.0;
+  unsigned int size_x = 100;
+  unsigned int size_y = 100;
+  unsigned int size_theta = 72;
+
+  nav2_smac_planner::NodeHybrid::initMotionModel(
+    nav2_smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info);
+
+  nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(
+    100, 100, 0.1, 0.0, 0.0, 0);
+  // island in the middle of lethal cost to cross
+  for (unsigned int i = 20; i <= 80; ++i) {
+    for (unsigned int j = 40; j <= 60; ++j) {
+      costmapA->setCost(i, j, 254);
+    }
+  }
+  // path on the right is narrow and thus with high cost
+  for (unsigned int i = 20; i <= 80; ++i) {
+    for (unsigned int j = 61; j <= 70; ++j) {
+      costmapA->setCost(i, j, 250);
+    }
+  }
+  for (unsigned int i = 20; i <= 80; ++i) {
+    for (unsigned int j = 71; j < 100; ++j) {
+      costmapA->setCost(i, j, 254);
+    }
+  }
+  std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
+    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmapA, 72, node);
+  checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
+
+  nav2_smac_planner::NodeHybrid testA(0);
+  testA.pose.x = 10;
+  testA.pose.y = 50;
+  testA.pose.theta = 0;
+
+  nav2_smac_planner::NodeHybrid testB(1);
+  testB.pose.x = 90;
+  testB.pose.y = 51;  // goal is a bit closer to the high-cost passage
+  testB.pose.theta = 0;
+
+  // first block the high-cost passage to make sure the cost spreads through the better path
+  for (unsigned int j = 61; j <= 70; ++j) {
+    costmapA->setCost(50, j, 254);
+  }
+  nav2_smac_planner::NodeHybrid::resetObstacleHeuristic(
+    costmapA, testA.pose.x, testA.pose.y, testB.pose.x, testB.pose.y);
+  float wide_passage_cost = nav2_smac_planner::NodeHybrid::getObstacleHeuristic(
+    testA.pose,
+    testB.pose,
+    info.cost_penalty);
+
+  EXPECT_NEAR(wide_passage_cost, 91.1f, 0.1f);
+
+  // then unblock it to check if cost remains the same
+  // (it should, since the unblocked narrow path will have higher cost than the wide one
+  //  and thus lower bound of the path cost should be unchanged)
+  for (unsigned int j = 61; j <= 70; ++j) {
+    costmapA->setCost(50, j, 250);
+  }
+  nav2_smac_planner::NodeHybrid::resetObstacleHeuristic(
+    costmapA,
+    testA.pose.x, testA.pose.y, testB.pose.x, testB.pose.y);
+  float two_passages_cost = nav2_smac_planner::NodeHybrid::getObstacleHeuristic(
+    testA.pose,
+    testB.pose,
+    info.cost_penalty);
+
+  EXPECT_EQ(wide_passage_cost, two_passages_cost);
+
+  delete costmapA;
+}
+
+TEST(NodeHybridTest, test_node_debin_neighbors)
+{
+  nav2_smac_planner::SearchInfo info;
+  info.change_penalty = 1.2;
+  info.non_straight_penalty = 1.4;
+  info.reverse_penalty = 2.1;
+  info.minimum_turning_radius = 4;  // 0.2 in grid coordinates
+  info.retrospective_penalty = 0.0;
+  unsigned int size_x = 100;
+  unsigned int size_y = 100;
+  unsigned int size_theta = 72;
+  nav2_smac_planner::NodeHybrid::initMotionModel(
+    nav2_smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info);
+
+  // test neighborhood computation
+  EXPECT_EQ(nav2_smac_planner::NodeHybrid::motion_table.projections.size(), 3u);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[0]._x, 1.731517, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[0]._y, 0, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[0]._theta, 0, 0.01);
+
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[1]._x, 1.69047, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[1]._y, 0.3747, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[1]._theta, 5, 0.01);
+
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[2]._x, 1.69047, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[2]._y, -0.3747, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[2]._theta, -5, 0.01);
+}
+
+TEST(NodeHybridTest, test_node_reeds_neighbors)
+{
+  auto lnode = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  nav2_smac_planner::SearchInfo info;
+  info.change_penalty = 1.2;
+  info.non_straight_penalty = 1.4;
+  info.reverse_penalty = 2.1;
+  info.minimum_turning_radius = 8;  // 0.4 in grid coordinates
+  info.retrospective_penalty = 0.0;
+  unsigned int size_x = 100;
+  unsigned int size_y = 100;
+  unsigned int size_theta = 72;
+  nav2_smac_planner::NodeHybrid::initMotionModel(
+    nav2_smac_planner::MotionModel::REEDS_SHEPP, size_x, size_y, size_theta, info);
+
+  EXPECT_EQ(nav2_smac_planner::NodeHybrid::motion_table.projections.size(), 6u);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[0]._x, 2.088, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[0]._y, 0, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[0]._theta, 0, 0.01);
+
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[1]._x, 2.070, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[1]._y, 0.272, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[1]._theta, 3, 0.01);
+
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[2]._x, 2.070, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[2]._y, -0.272, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[2]._theta, -3, 0.01);
+
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[3]._x, -2.088, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[3]._y, 0, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[3]._theta, 0, 0.01);
+
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[4]._x, -2.07, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[4]._y, 0.272, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[4]._theta, -3, 0.01);
+
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[5]._x, -2.07, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[5]._y, -0.272, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[5]._theta, 3, 0.01);
+
+  nav2_costmap_2d::Costmap2D costmapA(100, 100, 0.05, 0.0, 0.0, 0);
+  std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
+    std::make_unique<nav2_smac_planner::GridCollisionChecker>(&costmapA, 72, lnode);
+  checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
+  nav2_smac_planner::NodeHybrid * node = new nav2_smac_planner::NodeHybrid(49);
+  std::function<bool(const unsigned int &, nav2_smac_planner::NodeHybrid * &)> neighborGetter =
+    [&, this](const unsigned int & index, nav2_smac_planner::NodeHybrid * & neighbor_rtn) -> bool
+    {
+      // because we don't return a real object
+      return false;
+    };
+
+  nav2_smac_planner::NodeHybrid::NodeVector neighbors;
+  node->getNeighbors(neighborGetter, checker.get(), false, neighbors);
+  delete node;
+
+  // should be empty since totally invalid
+  EXPECT_EQ(neighbors.size(), 0u);
+}
+
+TEST(NodeHybridTest, basic_get_closest_angular_bin_test)
+{
+  // Tests to check getClosestAngularBin behavior for different input types
+  nav2_smac_planner::HybridMotionTable motion_table;
+
+  {
+    motion_table.bin_size = 3.1415926;
+    motion_table.num_angle_quantization = 2;
+    double test_theta = 3.1415926;
+    unsigned int expected_angular_bin = 1;
+    unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta);
+    EXPECT_EQ(expected_angular_bin, calculated_angular_bin);
+  }
+
+  {
+    motion_table.bin_size = M_PI;
+    motion_table.num_angle_quantization = 2;
+    double test_theta = M_PI / 2.0 - 0.000001;
+    unsigned int expected_angular_bin = 0;
+    unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta);
+    EXPECT_EQ(expected_angular_bin, calculated_angular_bin);
+  }
+
+  {
+    motion_table.bin_size = M_PI;
+    motion_table.num_angle_quantization = 2;
+    float test_theta = M_PI;
+    unsigned int expected_angular_bin = 1;
+    unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta);
+    EXPECT_EQ(expected_angular_bin, calculated_angular_bin);
+  }
+
+  {
+    motion_table.bin_size = 0.0872664675;
+    motion_table.num_angle_quantization = 72;
+    double test_theta = 6.28317530718;  // 0.0001 less than 2 pi
+    unsigned int expected_angular_bin = 0;  // should be closer to wrap around
+    unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta);
+    EXPECT_EQ(expected_angular_bin, calculated_angular_bin);
+  }
+}