From 98c877099d5b722012ea8a56bc92edc33ff892fb Mon Sep 17 00:00:00 2001 From: Luca Bonamini Date: Tue, 24 Aug 2021 21:21:24 +0200 Subject: [PATCH 1/5] Add new test for smac_planner_2d --- nav2_smac_planner/test/test_smac_2d.cpp | 45 +++++++++++++++++++++++++ 1 file changed, 45 insertions(+) diff --git a/nav2_smac_planner/test/test_smac_2d.cpp b/nav2_smac_planner/test/test_smac_2d.cpp index f3391d42404..7f65c8b4d9c 100644 --- a/nav2_smac_planner/test/test_smac_2d.cpp +++ b/nav2_smac_planner/test/test_smac_2d.cpp @@ -37,6 +37,7 @@ class RclCppFixture }; RclCppFixture g_rclcppfixture; + // SMAC smoke tests for plugin-level issues rather than algorithms // (covered by more extensively testing in other files) // System tests in nav2_system_tests will actually plan with this work @@ -78,3 +79,47 @@ TEST(SmacTest, test_smac_2d) node2D.reset(); costmap_ros.reset(); } + +TEST(SmacTest, test_smac_2d_reconfigure) +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr node2D = + std::make_shared("Smac2DTest"); + + std::shared_ptr costmap_ros; + costmap_ros->on_configure(rclcpp_lifecycle::State()); + + auto planner_2d = std::make_unique(); + planner_2d->configure(node2D, "test", nullptr, costmap_ros); + planner_2d->activate(); + + auto rec_param = std::make_shared( + node2D->get_node_base_interface(), + node2D->get_node_topics_interface(), + node2D->get_node_graph_interface(), + node2D->get_node_services_interface()); + + auto results = rec_param->set_parameters( + { + rclcpp::Parameter("test.tolerance", 1.0), + rclcpp::Parameter("test.cost_travel_multiplier", 1.0), + rclcpp::Parameter("test.max_planning_time", 2.0), + rclcpp::Parameter("test.downsample_costmap", false), + rclcpp::Parameter("test.allow_unknown", false), + rclcpp::Parameter("test.downsampling_factor", 2), + rclcpp::Parameter("test.max_iterations", -1), + rclcpp::Parameter("test.max_on_approach_iterations", -1), + rclcpp::Parameter("test.motion_model_for_search", "UNKNOWN") + }); + + rclcpp::spin_until_future_complete(node2D->get_node_base_interface(), results); + + EXPECT_EQ(node2D->get_parameter("test.tolerance").as_double(), 1.0); + EXPECT_EQ(node2D->get_parameter("test.cost_travel_multiplier").as_double(), 1.0); + EXPECT_EQ(node2D->get_parameter("test.max_planning_time").as_double(), 2.0); + EXPECT_EQ(node2D->get_parameter("test.downsample_costmap").as_bool(), false); + EXPECT_EQ(node2D->get_parameter("test.allow_unknown").as_bool(), false); + EXPECT_EQ(node2D->get_parameter("test.downsampling_factor").as_int(), 2); + EXPECT_EQ(node2D->get_parameter("test.max_iterations").as_int(), -1); + EXPECT_EQ(node2D->get_parameter("test.max_on_approach_iterations").as_int(), -1); + EXPECT_EQ(node2D->get_parameter("test.motion_model_for_search").as_string(), "UNKNOWN"); +} From 61988d0c648397249cc946efcf0e9021ff9370e6 Mon Sep 17 00:00:00 2001 From: Luca Bonamini Date: Wed, 25 Aug 2021 10:11:27 +0200 Subject: [PATCH 2/5] Fix costmap initialization --- nav2_smac_planner/test/test_smac_2d.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nav2_smac_planner/test/test_smac_2d.cpp b/nav2_smac_planner/test/test_smac_2d.cpp index 7f65c8b4d9c..dd9a9e65e98 100644 --- a/nav2_smac_planner/test/test_smac_2d.cpp +++ b/nav2_smac_planner/test/test_smac_2d.cpp @@ -85,7 +85,8 @@ TEST(SmacTest, test_smac_2d_reconfigure) rclcpp_lifecycle::LifecycleNode::SharedPtr node2D = std::make_shared("Smac2DTest"); - std::shared_ptr costmap_ros; + std::shared_ptr costmap_ros = + std::make_shared("global_costmap"); costmap_ros->on_configure(rclcpp_lifecycle::State()); auto planner_2d = std::make_unique(); From f0cc5b03cf8bd1c1744c454bb286cecbdafd7b73 Mon Sep 17 00:00:00 2001 From: Luca Bonamini Date: Wed, 25 Aug 2021 11:26:52 +0200 Subject: [PATCH 3/5] Change set_parameters() with set_parameters_atomically() --- nav2_smac_planner/test/test_smac_2d.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_smac_planner/test/test_smac_2d.cpp b/nav2_smac_planner/test/test_smac_2d.cpp index dd9a9e65e98..f204fa6a7ac 100644 --- a/nav2_smac_planner/test/test_smac_2d.cpp +++ b/nav2_smac_planner/test/test_smac_2d.cpp @@ -99,7 +99,7 @@ TEST(SmacTest, test_smac_2d_reconfigure) node2D->get_node_graph_interface(), node2D->get_node_services_interface()); - auto results = rec_param->set_parameters( + auto results = rec_param->set_parameters_atomically( { rclcpp::Parameter("test.tolerance", 1.0), rclcpp::Parameter("test.cost_travel_multiplier", 1.0), From 3d884c3ac0b575255fad638db60c118e5d08eaed Mon Sep 17 00:00:00 2001 From: Luca Bonamini Date: Wed, 25 Aug 2021 22:37:49 +0200 Subject: [PATCH 4/5] Improving coverage --- nav2_smac_planner/test/test_smac_2d.cpp | 72 +++++++++++++++---------- 1 file changed, 43 insertions(+), 29 deletions(-) diff --git a/nav2_smac_planner/test/test_smac_2d.cpp b/nav2_smac_planner/test/test_smac_2d.cpp index f204fa6a7ac..bda9fc50cf9 100644 --- a/nav2_smac_planner/test/test_smac_2d.cpp +++ b/nav2_smac_planner/test/test_smac_2d.cpp @@ -13,21 +13,22 @@ // limitations under the License. Reserved. #include + #include #include #include +#include "geometry_msgs/msg/pose_stamped.hpp" #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 "geometry_msgs/msg/pose_stamped.hpp" -#include "nav2_smac_planner/node_hybrid.hpp" #include "nav2_smac_planner/a_star.hpp" #include "nav2_smac_planner/collision_checker.hpp" -#include "nav2_smac_planner/smac_planner_hybrid.hpp" +#include "nav2_smac_planner/node_hybrid.hpp" #include "nav2_smac_planner/smac_planner_2d.hpp" +#include "nav2_smac_planner/smac_planner_hybrid.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "rclcpp/rclcpp.hpp" class RclCppFixture { @@ -37,13 +38,11 @@ class RclCppFixture }; RclCppFixture g_rclcppfixture; - // SMAC smoke tests for plugin-level issues rather than algorithms // (covered by more extensively testing in other files) // System tests in nav2_system_tests will actually plan with this work -TEST(SmacTest, test_smac_2d) -{ +TEST(SmacTest, test_smac_2d) { rclcpp_lifecycle::LifecycleNode::SharedPtr node2D = std::make_shared("Smac2DTest"); @@ -62,7 +61,10 @@ TEST(SmacTest, test_smac_2d) start.pose.position.x = 0.0; start.pose.position.y = 0.0; start.pose.orientation.w = 1.0; - goal = start; + // goal = start; + goal.pose.position.x = 7.0; + goal.pose.position.y = 0.0; + goal.pose.orientation.w = 1.0; auto planner_2d = std::make_unique(); planner_2d->configure(node2D, "test", nullptr, costmap_ros); planner_2d->activate(); @@ -78,10 +80,10 @@ TEST(SmacTest, test_smac_2d) costmap_ros->on_cleanup(rclcpp_lifecycle::State()); node2D.reset(); costmap_ros.reset(); + EXPECT_FALSE(true); } -TEST(SmacTest, test_smac_2d_reconfigure) -{ +TEST(SmacTest, test_smac_2d_reconfigure) { rclcpp_lifecycle::LifecycleNode::SharedPtr node2D = std::make_shared("Smac2DTest"); @@ -94,33 +96,45 @@ TEST(SmacTest, test_smac_2d_reconfigure) planner_2d->activate(); auto rec_param = std::make_shared( - node2D->get_node_base_interface(), - node2D->get_node_topics_interface(), + node2D->get_node_base_interface(), node2D->get_node_topics_interface(), node2D->get_node_graph_interface(), node2D->get_node_services_interface()); auto results = rec_param->set_parameters_atomically( - { - rclcpp::Parameter("test.tolerance", 1.0), - rclcpp::Parameter("test.cost_travel_multiplier", 1.0), - rclcpp::Parameter("test.max_planning_time", 2.0), - rclcpp::Parameter("test.downsample_costmap", false), - rclcpp::Parameter("test.allow_unknown", false), - rclcpp::Parameter("test.downsampling_factor", 2), - rclcpp::Parameter("test.max_iterations", -1), - rclcpp::Parameter("test.max_on_approach_iterations", -1), - rclcpp::Parameter("test.motion_model_for_search", "UNKNOWN") - }); - - rclcpp::spin_until_future_complete(node2D->get_node_base_interface(), results); + {rclcpp::Parameter("test.tolerance", 1.0), + rclcpp::Parameter("test.cost_travel_multiplier", 1.0), + rclcpp::Parameter("test.max_planning_time", 2.0), + rclcpp::Parameter("test.downsample_costmap", false), + rclcpp::Parameter("test.allow_unknown", false), + rclcpp::Parameter("test.downsampling_factor", 2), + rclcpp::Parameter("test.max_iterations", -1), + rclcpp::Parameter("test.max_on_approach_iterations", -1), + rclcpp::Parameter("test.motion_model_for_search", "UNKNOWN")}); + + rclcpp::spin_until_future_complete( + node2D->get_node_base_interface(), + results); EXPECT_EQ(node2D->get_parameter("test.tolerance").as_double(), 1.0); - EXPECT_EQ(node2D->get_parameter("test.cost_travel_multiplier").as_double(), 1.0); + EXPECT_EQ( + node2D->get_parameter("test.cost_travel_multiplier").as_double(), + 1.0); EXPECT_EQ(node2D->get_parameter("test.max_planning_time").as_double(), 2.0); EXPECT_EQ(node2D->get_parameter("test.downsample_costmap").as_bool(), false); EXPECT_EQ(node2D->get_parameter("test.allow_unknown").as_bool(), false); EXPECT_EQ(node2D->get_parameter("test.downsampling_factor").as_int(), 2); EXPECT_EQ(node2D->get_parameter("test.max_iterations").as_int(), -1); - EXPECT_EQ(node2D->get_parameter("test.max_on_approach_iterations").as_int(), -1); - EXPECT_EQ(node2D->get_parameter("test.motion_model_for_search").as_string(), "UNKNOWN"); + EXPECT_EQ( + node2D->get_parameter("test.max_on_approach_iterations").as_int(), + -1); + EXPECT_EQ( + node2D->get_parameter("test.motion_model_for_search").as_string(), + "UNKNOWN"); + + results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test.downsample_costmap", true)}); + + rclcpp::spin_until_future_complete( + node2D->get_node_base_interface(), + results); } From 17ebe4d33c9f21050d0b4dea1aa4d08777f58b6a Mon Sep 17 00:00:00 2001 From: Luca Bonamini Date: Wed, 25 Aug 2021 23:17:06 +0200 Subject: [PATCH 5/5] Remove debug helper method --- nav2_smac_planner/test/test_smac_2d.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_smac_planner/test/test_smac_2d.cpp b/nav2_smac_planner/test/test_smac_2d.cpp index bda9fc50cf9..0509a7cebd7 100644 --- a/nav2_smac_planner/test/test_smac_2d.cpp +++ b/nav2_smac_planner/test/test_smac_2d.cpp @@ -80,7 +80,6 @@ TEST(SmacTest, test_smac_2d) { costmap_ros->on_cleanup(rclcpp_lifecycle::State()); node2D.reset(); costmap_ros.reset(); - EXPECT_FALSE(true); } TEST(SmacTest, test_smac_2d_reconfigure) {