diff --git a/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt b/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt index 6c57a2cba4d75e068a800a4516743d40e422b28a..cbd7dba18d1b0e575a6fb725b20077e825cf2c55 100644 --- a/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt @@ -3,5 +3,5 @@ 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(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/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");