adding additional constants and smoothers tests (#2038)

smac_planner/include/smac_planner/node_basic.hpp

@@ -52,7 +52,8 @@ public:
   explicit NodeBasic(const unsigned int index)
   : index(index),
     graph_node_ptr(nullptr)
-  {}
+  {
+  }
 
   typename NodeT::Coordinates pose;  // Used by NodeSE2
   NodeT * graph_node_ptr;

smac_planner/test/test_a_star.cpp

@@ -115,14 +115,6 @@ TEST(AStarTest, test_a_star_2d)
   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)
@@ -169,3 +161,23 @@ TEST(AStarTest, test_a_star_se2)
 
   delete costmapA;
 }
+
+TEST(AStarTest, test_constants)
+{
+  smac_planner::MotionModel mm = smac_planner::MotionModel::UNKNOWN;  // unknown
+  EXPECT_EQ(smac_planner::toString(mm), std::string("Unknown"));
+  mm = smac_planner::MotionModel::VON_NEUMANN;  // vonneumann
+  EXPECT_EQ(smac_planner::toString(mm), std::string("Von Neumann"));
+  mm = smac_planner::MotionModel::MOORE;  // moore
+  EXPECT_EQ(smac_planner::toString(mm), std::string("Moore"));
+  mm = smac_planner::MotionModel::DUBIN;  // dubin
+  EXPECT_EQ(smac_planner::toString(mm), std::string("Dubin"));
+  mm = smac_planner::MotionModel::REEDS_SHEPP;  // reeds-shepp
+  EXPECT_EQ(smac_planner::toString(mm), std::string("Reeds-Shepp"));
+
+  EXPECT_EQ(smac_planner::fromString("VON_NEUMANN"), smac_planner::MotionModel::VON_NEUMANN);
+  EXPECT_EQ(smac_planner::fromString("MOORE"), smac_planner::MotionModel::MOORE);
+  EXPECT_EQ(smac_planner::fromString("DUBIN"), smac_planner::MotionModel::DUBIN);
+  EXPECT_EQ(smac_planner::fromString("REEDS_SHEPP"), smac_planner::MotionModel::REEDS_SHEPP);
+  EXPECT_EQ(smac_planner::fromString("NONE"), smac_planner::MotionModel::UNKNOWN);
+}

smac_planner/test/test_smac_2d.cpp

@@ -50,6 +50,13 @@ TEST(SmacTest, test_smac_2d)
     std::make_shared<nav2_costmap_2d::Costmap2DROS>("global_costmap");
   costmap_ros->on_configure(rclcpp_lifecycle::State());
 
+  node2D->declare_parameter("test.smooth_path", true);
+  node2D->set_parameter(rclcpp::Parameter("test.smooth_path", true));
+  node2D->declare_parameter("test.downsample_costmap", true);
+  node2D->set_parameter(rclcpp::Parameter("test.downsample_costmap", true));
+  node2D->declare_parameter("test.downsampling_factor", 2);
+  node2D->set_parameter(rclcpp::Parameter("test.downsampling_factor", 2));
+
   geometry_msgs::msg::PoseStamped start, goal;
   start.pose.position.x = 0.0;
   start.pose.position.y = 0.0;

smac_planner/test/test_smac_se2.cpp

@@ -52,6 +52,10 @@ TEST(SmacTest, test_smac_se2)
 
   nodeSE2->declare_parameter("test.smooth_path", true);
   nodeSE2->set_parameter(rclcpp::Parameter("test.smooth_path", true));
+  nodeSE2->declare_parameter("test.downsample_costmap", true);
+  nodeSE2->set_parameter(rclcpp::Parameter("test.downsample_costmap", true));
+  nodeSE2->declare_parameter("test.downsampling_factor", 2);
+  nodeSE2->set_parameter(rclcpp::Parameter("test.downsampling_factor", 2));
 
   geometry_msgs::msg::PoseStamped start, goal;
   start.pose.position.x = 0.0;
@@ -75,3 +79,14 @@ TEST(SmacTest, test_smac_se2)
   costmap_ros.reset();
   nodeSE2.reset();
 }
+
+TEST(SmacTestSE2, test_dist)
+{
+  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);
+}