diff --git a/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt b/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt index 6c57a2cba4d75e068a800a4516743d40e422b28a..272f38cf4cac4d770c9be50e3e55c694b333fd4b 100644 --- a/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt @@ -1,7 +1,7 @@ ament_add_gtest(vtest velocity_iterator_test.cpp) -#ament_add_gtest(goal_checker goal_checker.cpp) -#target_link_libraries(goal_checker simple_goal_checker stopped_goal_checker) +ament_add_gtest(goal_checker goal_checker.cpp) +target_link_libraries(goal_checker simple_goal_checker stopped_goal_checker) -#ament_add_gtest(twist_gen_test twist_gen.cpp) -#target_link_libraries(twist_gen_test standard_traj_generator) +ament_add_gtest(twist_gen_test twist_gen.cpp) +target_link_libraries(twist_gen_test standard_traj_generator) diff --git a/nav2_dwb_controller/dwb_plugins/test/goal_checker.cpp b/nav2_dwb_controller/dwb_plugins/test/goal_checker.cpp index 6e627e3282508c0e547965777a22d8a894977914..a2f7f7a3fff6cadabb37622a14eb1cad843bdd27 100644 --- a/nav2_dwb_controller/dwb_plugins/test/goal_checker.cpp +++ b/nav2_dwb_controller/dwb_plugins/test/goal_checker.cpp @@ -38,6 +38,8 @@ #include "gtest/gtest.h" #include "dwb_plugins/simple_goal_checker.hpp" #include "dwb_plugins/stopped_goal_checker.hpp" +#include "nav_2d_utils/conversions.hpp" +#include "nav2_util/lifecycle_node.hpp" using dwb_plugins::SimpleGoalChecker; using dwb_plugins::StoppedGoalChecker; @@ -61,9 +63,11 @@ void checkMacro( v.y = yv; v.theta = thetav; if (expected_result) { - EXPECT_TRUE(gc.isGoalReached(pose0, pose1, v)); + EXPECT_TRUE(gc.isGoalReached(nav_2d_utils::pose2DToPose(pose0), + nav_2d_utils::pose2DToPose(pose1), nav_2d_utils::twist2Dto3D(v))); } else { - EXPECT_FALSE(gc.isGoalReached(pose0, pose1, v)); + EXPECT_FALSE(gc.isGoalReached(nav_2d_utils::pose2DToPose(pose0), + nav_2d_utils::pose2DToPose(pose1), nav_2d_utils::twist2Dto3D(v))); } } diff --git a/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp b/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp index b1c90aedafc5e1878f70f4b521b42681964e7c02..1bee7fdda2c052ceaa0abc397bd69103efddd620 100644 --- a/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp +++ b/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp @@ -71,6 +71,8 @@ std::vector<rclcpp::Parameter> getDefaultKinematicParameters() parameters.push_back(rclcpp::Parameter("max_speed_xy", 0.55)); parameters.push_back(rclcpp::Parameter("min_speed_theta", 0.4)); + parameters.push_back(rclcpp::Parameter("use_dwa", false)); + return parameters; } @@ -237,17 +239,6 @@ TEST(VelocityIterator, dwa_gen) checkLimits(twists, 0.0, 0.125, -0.1, 0.1, -0.16, 0.16, hypot(0.125, 0.1), 0.0, 0.1); } -TEST(VelocityIterator, dwa_gen_no_param) -{ - auto nh = makeTestNode("dwa_gen_no_param"); - nh->set_parameters({rclcpp::Parameter("min_speed_theta", -1.0)}); - dwb_plugins::LimitedAccelGenerator gen; - gen.initialize(nh); - std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(zero); - EXPECT_EQ(twists.size(), 20u * 20u * 5u + 100u - 1u); - checkLimits(twists, 0.0, 0.125, -0.1, 0.1, -0.16, 0.16, hypot(0.125, 0.1), 0.0, 0.1); -} - TEST(VelocityIterator, nonzero) { auto nh = makeTestNode("nonzero");