diff --git a/include/air_navigation/py_interface.h b/include/air_navigation/py_interface.h
index e7d81e5dc9211563d7ddbf13e2cd35233d6a42b6..c48013cb40f15ae34d32c72fc4ffafc73215a34f 100644
--- a/include/air_navigation/py_interface.h
+++ b/include/air_navigation/py_interface.h
@@ -1,6 +1,7 @@
 #include <iostream>
 #include <string>
 #include <vector>
+#include <nav2_core/goal_checker.hpp>
 
 namespace air_navigation::py_interface
 {
@@ -50,16 +51,21 @@ namespace air_navigation::py_interface
     copy_vector3(s.angular, &d->angular);
   }
   template<typename _TS_, typename _TD_>
+  inline void copy_pose(const _TS_& s, _TD_* d)
+  {
+    d->position.x = s.position.x;
+    d->position.y = s.position.y;
+    d->position.z = s.position.z;
+    d->orientation.x = s.orientation.x;
+    d->orientation.y = s.orientation.y;
+    d->orientation.z = s.orientation.z;
+    d->orientation.w = s.orientation.w;
+  }
+  template<typename _TS_, typename _TD_>
   inline void copy_pose_stamped(const _TS_& s, _TD_* d)
   {
     copy_header(s.header, &d->header);
-    d->pose.position.x = s.pose.position.x;
-    d->pose.position.y = s.pose.position.y;
-    d->pose.position.z = s.pose.position.z;
-    d->pose.orientation.x = s.pose.orientation.x;
-    d->pose.orientation.y = s.pose.orientation.y;
-    d->pose.orientation.z = s.pose.orientation.z;
-    d->pose.orientation.w = s.pose.orientation.w;
+    copy_pose(s.pose, &d->pose);
   }
 
   template<typename _TPS_, typename _TS_, typename _TD_>
@@ -74,6 +80,51 @@ namespace air_navigation::py_interface
     }
   }
 
+  class GoalChecker
+  {
+  public:
+    GoalChecker(nav2_core::GoalChecker* _goalChecker) : m_goalChecker(_goalChecker)
+    {}
+    ~GoalChecker()
+    {}
+    void reset()
+    {
+      m_goalChecker->reset();
+    }
+
+    bool isGoalReached(
+      const Pose & query_pose, const Pose & goal_pose,
+      const Twist & velocity)
+    {
+      geometry_msgs::msg::Pose ros_query_pose;
+      geometry_msgs::msg::Pose ros_goal_pose;
+      geometry_msgs::msg::Twist ros_velocity;
+
+      copy_pose(query_pose, &ros_query_pose);
+      copy_pose(goal_pose, &ros_goal_pose);
+      copy_twist(velocity, &ros_velocity);
+
+      return m_goalChecker->isGoalReached(ros_query_pose, ros_goal_pose, ros_velocity);
+    }
+
+    bool getTolerances(
+      Pose* pose_tolerance,
+      Twist* vel_tolerance)
+    {
+      geometry_msgs::msg::Pose ros_pose_tolerance;
+      geometry_msgs::msg::Twist ros_vel_tolerance;
+
+      bool v = m_goalChecker->getTolerances(ros_pose_tolerance, ros_vel_tolerance);
+
+      copy_pose(ros_pose_tolerance, pose_tolerance);
+      copy_twist(ros_vel_tolerance, vel_tolerance);
+      return v;
+    }
+
+  private:
+    nav2_core::GoalChecker* m_goalChecker;
+  };
+
   class Controller
   {
   public:
@@ -86,7 +137,7 @@ namespace air_navigation::py_interface
     virtual void activate() = 0;
     virtual void deactivate() = 0;
 
-    virtual TwistStamped computeVelocityCommands(const PoseStamped& pose, Twist& velocity) = 0;
+    virtual TwistStamped computeVelocityCommands(const PoseStamped& pose, Twist& velocity, GoalChecker* goal) = 0;
 
     virtual void setPlan(const Path& path) = 0;
 
diff --git a/python/air_navigation/py_interface.sip b/python/air_navigation/py_interface.sip
index e2b16a288a85e3af076c4b9a88974489e97b27aa..3b64e4e9685594934dcb1f01bb03c2531488c9ee 100644
--- a/python/air_navigation/py_interface.sip
+++ b/python/air_navigation/py_interface.sip
@@ -30,6 +30,21 @@ namespace air_navigation
     struct Twist { Vector3 linear; Vector3 angular; };
     struct TwistStamped { Header header; Twist twist; };
     struct Path { Header header; std::vector<air_navigation::py_interface::PoseStamped> poses; };
+
+    class GoalChecker /NoDefaultCtors/
+    {
+    public:
+      void reset();
+
+      bool isGoalReached(
+        const Pose & query_pose, const Pose & goal_pose,
+        const Twist & velocity);
+
+      bool getTolerances(
+        Pose* pose_tolerance,
+        Twist* vel_tolerance);
+    };
+
     class Controller
     {
     public:
@@ -42,7 +57,7 @@ namespace air_navigation
       virtual void activate() = 0;
       virtual void deactivate() = 0;
 
-      virtual TwistStamped computeVelocityCommands(const PoseStamped& pose, Twist& velocity) = 0;
+      virtual TwistStamped computeVelocityCommands(const PoseStamped& pose, Twist& velocity, GoalChecker* goal) = 0;
 
       virtual void setPlan(const Path& path) = 0;
 
diff --git a/src/python_controller.cpp b/src/python_controller.cpp
index 98a06f88aa91500fac6a7320e2d1edbe51c892ed..c8369f4882ad7c202fc187feb9606fd856e34e80 100644
--- a/src/python_controller.cpp
+++ b/src/python_controller.cpp
@@ -64,7 +64,7 @@ void PythonController::deactivate()
 geometry_msgs::msg::TwistStamped PythonController::computeVelocityCommands(
   const geometry_msgs::msg::PoseStamped & pose,
   const geometry_msgs::msg::Twist & twist,
-    nav2_core::GoalChecker * )
+    nav2_core::GoalChecker * goal)
 {
   python_lock pl;
   if(m_python_controller_interface)
@@ -73,7 +73,8 @@ geometry_msgs::msg::TwistStamped PythonController::computeVelocityCommands(
     py_interface::copy_pose_stamped(pose, &py_pose);
     py_interface::Twist py_twis;
     py_interface::copy_twist(twist, &py_twis);
-    py_interface::TwistStamped py_twis_stamped = m_python_controller_interface->computeVelocityCommands(py_pose, py_twis);
+    py_interface::GoalChecker gc(goal);
+    py_interface::TwistStamped py_twis_stamped = m_python_controller_interface->computeVelocityCommands(py_pose, py_twis, &gc);
     geometry_msgs::msg::TwistStamped twist_stamped;
     py_interface::copy_header(py_twis_stamped.header, &twist_stamped.header);
     py_interface::copy_twist(py_twis_stamped.twist, &twist_stamped.twist);