diff --git a/smac_planner/include/smac_planner/a_star.hpp b/smac_planner/include/smac_planner/a_star.hpp index 02bf5421313..a53b872a913 100644 --- a/smac_planner/include/smac_planner/a_star.hpp +++ b/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 */ diff --git a/smac_planner/include/smac_planner/costmap_downsampler.hpp b/smac_planner/include/smac_planner/costmap_downsampler.hpp index e67a5efa48d..6f23c696e21 100644 --- a/smac_planner/include/smac_planner/costmap_downsampler.hpp +++ b/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 diff --git a/smac_planner/include/smac_planner/node_basic.hpp b/smac_planner/include/smac_planner/node_basic.hpp index 4258608a5da..fb648983aa5 100644 --- a/smac_planner/include/smac_planner/node_basic.hpp +++ b/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; diff --git a/smac_planner/test/test_a_star.cpp b/smac_planner/test/test_a_star.cpp index 09989d0fb8f..741a8fd1ee7 100644 --- a/smac_planner/test/test_a_star.cpp +++ b/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) diff --git a/smac_planner/test/test_costmap_downsampler.cpp b/smac_planner/test/test_costmap_downsampler.cpp index 6fb2b846275..040a2eb760c 100644 --- a/smac_planner/test/test_costmap_downsampler.cpp +++ b/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(); } diff --git a/smac_planner/test/test_node2d.cpp b/smac_planner/test/test_node2d.cpp index 32b10cd33d9..bc42988aca4 100644 --- a/smac_planner/test/test_node2d.cpp +++ b/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) diff --git a/smac_planner/test/test_nodebasic.cpp b/smac_planner/test/test_nodebasic.cpp index 996d98f572b..0ec90a112f3 100644 --- a/smac_planner/test/test_nodebasic.cpp +++ b/smac_planner/test/test_nodebasic.cpp @@ -39,11 +39,11 @@ TEST(NodeBasicTest, test_node_basic) { smac_planner::NodeBasic node(50); - EXPECT_EQ(node.getIndex(), 50u); + EXPECT_EQ(node.index, 50u); EXPECT_EQ(node.graph_node_ptr, nullptr); smac_planner::NodeBasic node2(100); - EXPECT_EQ(node2.getIndex(), 100u); + EXPECT_EQ(node2.index, 100u); EXPECT_EQ(node2.graph_node_ptr, nullptr); } diff --git a/smac_planner/test/test_smoother.cpp b/smac_planner/test/test_smoother.cpp index 5543c6dc333..ab05a93959f 100644 --- a/smac_planner/test/test_smoother.cpp +++ b/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;