Adding additional SmacPlanner tests (#2036)

* adding some more tests to smac_planner

* addtl smac tests

* remove unused functions

Author: SteveMacenski

smac_planner/include/smac_planner/a_star.hpp

@@ -173,19 +173,49 @@ class AStarAlgorithm
    */
   int & getMaxIterations();
 
-private:
   /**
    * @brief Get pointer reference to starting node
    * @return Node pointer reference to starting node
    */
-  inline NodePtr & getStart();
+  NodePtr & getStart();
 
   /**
    * @brief Get pointer reference to goal node
    * @return Node pointer reference to goal node
    */
-  inline NodePtr & getGoal();
+  NodePtr & getGoal();
 
+  /**
+   * @brief Get maximum number of on-approach iterations after within threshold
+   * @return Reference to Maximum on-appraoch iterations parameter
+   */
+  int & getOnApproachMaxIterations();
+
+  /**
+   * @brief Get tolerance, in node nodes
+   * @return Reference to tolerance parameter
+   */
+  float & getToleranceHeuristic();
+
+  /**
+   * @brief Get size of graph in X
+   * @return Size in X
+   */
+  unsigned int & getSizeX();
+
+  /**
+   * @brief Get size of graph in Y
+   * @return Size in Y
+   */
+  unsigned int & getSizeY();
+
+  /**
+   * @brief Get number of angle quantization bins (SE2) or Z coordinate  (XYZ)
+   * @return Number of angle bins / Z dimension
+   */
+  unsigned int & getSizeDim3();
+
+protected:
   /**
    * @brief Get pointer to next goal in open set
    * @return Node pointer reference to next heuristically scored node
@@ -242,36 +272,6 @@ class AStarAlgorithm
    */
   inline bool areInputsValid();
 
-  /**
-   * @brief Get maximum number of on-approach iterations after within threshold
-   * @return Reference to Maximum on-appraoch iterations parameter
-   */
-  inline int & getOnApproachMaxIterations();
-
-  /**
-   * @brief Get tolerance, in node nodes
-   * @return Reference to tolerance parameter
-   */
-  inline float & getToleranceHeuristic();
-
-  /**
-   * @brief Get size of graph in X
-   * @return Size in X
-   */
-  inline unsigned int & getSizeX();
-
-  /**
-   * @brief Get size of graph in Y
-   * @return Size in Y
-   */
-  inline unsigned int & getSizeY();
-
-  /**
-   * @brief Get number of angle quantization bins (SE2) or Z coordinate  (XYZ)
-   * @return Number of angle bins / Z dimension
-   */
-  inline unsigned int & getSizeDim3();
-
   /**
    * @brief Clear hueristic queue of nodes to search
    */

smac_planner/include/smac_planner/costmap_downsampler.hpp

@@ -79,15 +79,17 @@ class CostmapDownsampler
    */
   nav2_costmap_2d::Costmap2D * downsample(const unsigned int & downsampling_factor);
 
-private:
-  /**
-   * @brief Update the sizes X-Y of the costmap and its downsampled version
-   */
-  void updateCostmapSize();
   /**
    * @brief Resize the downsampled costmap. Used in case the costmap changes and we need to update the downsampled version
    */
   void resizeCostmap();
+
+protected:
+  /**
+   * @brief Update the sizes X-Y of the costmap and its downsampled version
+   */
+  void updateCostmapSize();
+
   /**
    * @brief Explore all subcells of the original costmap and assign the max cost to the new (downsampled) cell
    * @param new_mx The X-coordinate of the cell in the new costmap

smac_planner/include/smac_planner/node_basic.hpp

@@ -50,29 +50,13 @@ class NodeBasic
    * @param index The index of this node for self-reference
    */
   explicit NodeBasic(const unsigned int index)
-  : _index(index),
+  : index(index),
     graph_node_ptr(nullptr)
   {}
 
-  /**
-   * @brief A destructor for smac_planner::NodeBasic
-   */
-  ~NodeBasic() {}
-
-  /**
-   * @brief Gets cell index
-   * @return Reference to cell index
-   */
-  inline unsigned int & getIndex()
-  {
-    return _index;
-  }
-
   typename NodeT::Coordinates pose;  // Used by NodeSE2
   NodeT * graph_node_ptr;
-
-protected:
-  unsigned int _index;
+  unsigned int index;
 };
 
 template class NodeBasic<Node2D>;

smac_planner/test/test_a_star.cpp

@@ -106,7 +106,23 @@ TEST(AStarTest, test_a_star_2d)
     EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0);
   }
 
+  EXPECT_TRUE(a_star_2.getStart() != nullptr);
+  EXPECT_TRUE(a_star_2.getGoal() != nullptr);
+  EXPECT_EQ(a_star_2.getSizeX(), 100u);
+  EXPECT_EQ(a_star_2.getSizeY(), 100u);
+  EXPECT_EQ(a_star_2.getSizeDim3(), 1u);
+  EXPECT_EQ(a_star_2.getToleranceHeuristic(), 1000.0);
+  EXPECT_EQ(a_star_2.getOnApproachMaxIterations(), 10);
+
   delete costmapA;
+
+  Eigen::Vector2d p1;
+  p1[0] = 0.0;
+  p1[1] = 0.0;
+  Eigen::Vector2d p2;
+  p2[0] = 0.0;
+  p2[1] = 1.0;
+  EXPECT_NEAR(smac_planner::squaredDistance(p1, p2), 1.0, 0.001);
 }
 
 TEST(AStarTest, test_a_star_se2)

smac_planner/test/test_costmap_downsampler.cpp

@@ -54,7 +54,7 @@ TEST(CostmapDownsampler, costmap_downsample_test)
   EXPECT_EQ(downsampledCostmapA->getSizeInCellsY(), 5u);
 
   // give it another costmap of another size
-  nav2_costmap_2d::Costmap2D costmapB(4, 4, 0.05, 0.0, 0.0, 0);
+  nav2_costmap_2d::Costmap2D costmapB(4, 4, 0.10, 0.0, 0.0, 0);
 
   // downsample it
   downsampler.initialize("map", "unused_topic", &costmapB, 4);
@@ -65,4 +65,6 @@ TEST(CostmapDownsampler, costmap_downsample_test)
   // validate size
   EXPECT_EQ(downsampledCostmapB->getSizeInCellsX(), 1u);
   EXPECT_EQ(downsampledCostmapB->getSizeInCellsY(), 1u);
+
+  downsampler.resizeCostmap();
 }

smac_planner/test/test_node2d.cpp

@@ -92,6 +92,7 @@ TEST(Node2DTest, test_node_2d)
   EXPECT_EQ(smac_planner::Node2D::getIndex(6u, 43u, 10u), 436u);
   EXPECT_EQ(smac_planner::Node2D::getCoords(436u, 10u, 1u).x, 6u);
   EXPECT_EQ(smac_planner::Node2D::getCoords(436u, 10u, 1u).y, 43u);
+  EXPECT_THROW(smac_planner::Node2D::getCoords(436u, 10u, 10u), std::runtime_error);
 }
 
 TEST(Node2DTest, test_node_2d_neighbors)

smac_planner/test/test_nodebasic.cpp

@@ -39,11 +39,11 @@ TEST(NodeBasicTest, test_node_basic)
 {
   smac_planner::NodeBasic<smac_planner::NodeSE2> node(50);
 
-  EXPECT_EQ(node.getIndex(), 50u);
+  EXPECT_EQ(node.index, 50u);
   EXPECT_EQ(node.graph_node_ptr, nullptr);
 
   smac_planner::NodeBasic<smac_planner::Node2D> node2(100);
 
-  EXPECT_EQ(node2.getIndex(), 100u);
+  EXPECT_EQ(node2.index, 100u);
   EXPECT_EQ(node2.graph_node_ptr, nullptr);
 }

smac_planner/test/test_smoother.cpp

@@ -76,6 +76,7 @@ TEST(SmootherTest, test_smoother)
   }
 
   smac_planner::OptimizerParams params;
+  params.debug = true;
   params.get(node2D.get(), "test");
 
   smac_planner::SmootherParams smoother_params;