Merge pull request #1350 from bpwilcox/fix_twist_gen_test

Re-enable dwb_plugin tests

nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt

 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)

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)));
   }
 }
 

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");