diff --git a/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt b/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt
index cbd7dba18d1b0e575a6fb725b20077e825cf2c55..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)
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)));
   }
 }