(refactor) Adds type hints, typo fix and formatting

Author: aleph-ra

src/kompass_core/control/_base_.py

@@ -1,5 +1,5 @@
 from abc import abstractmethod
-from typing import Optional, List
+from typing import Optional, List, Union
 import numpy as np
 import kompass_cpp
 from ..models import RobotState
@@ -95,7 +95,7 @@ def logging_info(self) -> str:
 
     @property
     @abstractmethod
-    def linear_x_control(self):
+    def linear_x_control(self) -> Union[List[float], np.ndarray]:
         """
         Getter of the last linear forward velocity control computed by the controller
 
@@ -106,7 +106,7 @@ def linear_x_control(self):
 
     @property
     @abstractmethod
-    def linear_y_control(self):
+    def linear_y_control(self) -> Union[List[float], np.ndarray]:
         """
         Getter the last linear velocity lateral control computed by the controller
 
@@ -117,7 +117,7 @@ def linear_y_control(self):
 
     @property
     @abstractmethod
-    def angular_control(self):
+    def angular_control(self) -> Union[List[float], np.ndarray]:
         """
         Getter of the last angular velocity control computed by the controller
 
@@ -255,7 +255,7 @@ def tracked_state(self) -> Optional[RobotState]:
 
     @property
     @abstractmethod
-    def linear_x_control(self) -> List[float]:
+    def linear_x_control(self) -> Union[List[float], np.ndarray]:
         """
         Getter of the last linear forward velocity control computed by the controller
 
@@ -266,7 +266,7 @@ def linear_x_control(self) -> List[float]:
 
     @property
     @abstractmethod
-    def linear_y_control(self) -> List[float]:
+    def linear_y_control(self) -> Union[List[float], np.ndarray]:
         """
         Getter the last linear velocity lateral control computed by the controller
 
@@ -277,7 +277,7 @@ def linear_y_control(self) -> List[float]:
 
     @property
     @abstractmethod
-    def angular_control(self) -> List[float]:
+    def angular_control(self) -> Union[List[float], np.ndarray]:
         """
         Getter of the last angular velocity control computed by the controller
 
@@ -298,7 +298,7 @@ def distance_error(self) -> float:
         return target.crosstrack_error
 
     @property
-    def orientation_error(self) -> float:
+    def orientation_error(self) -> Union[float, np.ndarray]:
         """
         Getter of the path tracking orientation error (rad)
 

src/kompass_core/control/dvz.py

@@ -130,7 +130,7 @@ def interpolated_path(self) -> kompass_cpp.types.Path:
         return self.__reference_cmd_generator.interpolated_path()
 
     @property
-    def tracked_state(self) -> RobotState:
+    def tracked_state(self) -> Optional[RobotState]:
         """
         Tracked state on the path
 

src/kompass_core/control/dwa.py

@@ -1,5 +1,5 @@
 import logging
-from typing import Optional
+from typing import Optional, Union, List
 import numpy as np
 from attrs import Factory, define, field
 from ..datatypes.laserscan import LaserScanData
@@ -386,7 +386,7 @@ def result_cost(self) -> Optional[float]:
         return None
 
     @property
-    def linear_x_control(self) -> np.ndarray:
+    def linear_x_control(self) -> Union[List[float], np.ndarray]:
         """
         Getter of the last linear forward velocity control computed by the controller
 
@@ -398,7 +398,7 @@ def linear_x_control(self) -> np.ndarray:
         return [0.0]
 
     @property
-    def linear_y_control(self) -> np.ndarray:
+    def linear_y_control(self) -> Union[List[float], np.ndarray]:
         """
         Getter the last linear velocity lateral control computed by the controller
 
@@ -410,7 +410,7 @@ def linear_y_control(self) -> np.ndarray:
         return [0.0]
 
     @property
-    def angular_control(self) -> np.ndarray:
+    def angular_control(self) -> Union[List[float], np.ndarray]:
         """
         Getter of the last angular velocity control computed by the controller
 

src/kompass_core/control/rgbd_follower.py

@@ -12,7 +12,7 @@
     TrajectoryVelocities2D,
     TrajectoryPath,
 )
-from typing import Optional, List
+from typing import Optional, List, Union
 import numpy as np
 import logging
 from ._base_ import ControllerTemplate
@@ -521,7 +521,7 @@ def result_cost(self) -> Optional[float]:
         return None
 
     @property
-    def linear_x_control(self) -> np.ndarray:
+    def linear_x_control(self) -> Union[List[float], np.ndarray]:
         """
         Getter of the last linear forward velocity control computed by the controller
 
@@ -533,7 +533,7 @@ def linear_x_control(self) -> np.ndarray:
         return [0.0]
 
     @property
-    def linear_y_control(self) -> np.ndarray:
+    def linear_y_control(self) -> Union[List[float], np.ndarray]:
         """
         Getter the last linear velocity lateral control computed by the controller
 
@@ -545,7 +545,7 @@ def linear_y_control(self) -> np.ndarray:
         return [0.0]
 
     @property
-    def angular_control(self) -> np.ndarray:
+    def angular_control(self) -> Union[List[float], np.ndarray]:
         """
         Getter of the last angular velocity control computed by the controller
 

src/kompass_core/performance.py

@@ -1,4 +1,4 @@
-from typing import Any, List
+from typing import Any, List, Optional
 
 import numpy as np
 from .datatypes.obstacles import ObstaclesData
@@ -18,7 +18,7 @@ class MotionResult:
     def __init__(self) -> None:
         self.time = []
         self.steer_cmds: List[float] = []
-        self.robot_path: PathSample = None
+        self.robot_path: Optional[PathSample] = None
         self.speed_cmd: List[float] = []
         self.ori_error: List[float] = []
         self.lat_error: List[float] = []
@@ -93,7 +93,7 @@ def vis_result(
         )
 
         # Plot test path
-        visualization.plt_path_points_List(test, color="red", ax=ax3)
+        visualization.plt_path_points_list(test, color="red", ax=ax3)
 
         # Plot robot initial state
         robot_footprint.plt_robot(

src/kompass_core/utils/emergency_stop.py

@@ -1,4 +1,4 @@
-from typing import Optional, List
+from typing import Optional
 from logging import Logger
 from ..models import (
     Robot,

src/kompass_cpp/bindings/bindings_planning.cpp

@@ -7,7 +7,6 @@
 namespace py = nanobind;
 using namespace Kompass;
 
-
 // Utils submodule
 void bindings_planning(py::module_ &m) {
   auto m_planning = m.def_submodule("planning", "KOMPASS CPP Planning Module");
@@ -24,7 +23,7 @@ void bindings_planning(py::module_ &m) {
            py::arg("goal_x"), py::arg("goal_y"), py::arg("goal_yaw"),
            py::arg("map_3d"))
       .def("solve", &Planning::OMPL2DGeometricPlanner::solve,
-               py::arg("planning_timeout") = 1.0)
+           py::arg("planning_timeout") = 1.0)
       .def("get_solution", &Planning::OMPL2DGeometricPlanner::getPath)
       .def("set_space_bounds_from_map",
            &Planning::OMPL2DGeometricPlanner::setSpaceBoundsFromMap,

src/kompass_cpp/kompass_cpp/include/controllers/dwa.h

@@ -72,7 +72,8 @@ class DWA : public Follower {
                  const int maxNumThreads = 1);
 
   /**
-   * @brief Reset the resolution of the robot's local map, this is equivalent to the box size representing each obstacle after conversion
+   * @brief Reset the resolution of the robot's local map, this is equivalent to
+   * the box size representing each obstacle after conversion
    *
    * @param octreeRes
    */
@@ -111,7 +112,7 @@ class DWA : public Follower {
 
   template <typename T>
   Controller::Result computeVelocityCommand(const Velocity2D &global_vel,
-                                            const T &scan_points){
+                                            const T &scan_points) {
     TrajSearchResult searchRes = findBestPath(global_vel, scan_points);
     Controller::Result finalResult;
     if (searchRes.isTrajFound) {
@@ -123,18 +124,18 @@ class DWA : public Follower {
       finalResult.status = Controller::Result::Status::NO_COMMAND_POSSIBLE;
     }
     return finalResult;
-                                            };
+  };
 
   template <typename T>
   TrajSearchResult computeVelocityCommandsSet(const Velocity2D &global_vel,
-                                              const T &scan_points){
+                                              const T &scan_points) {
     TrajSearchResult searchRes = findBestPath(global_vel, scan_points);
     // Update latest velocity command
     if (searchRes.isTrajFound) {
       latest_velocity_command_ = searchRes.trajectory.velocities.getFront();
     }
     return searchRes;
-                                              };
+  };
 
   std::tuple<MatrixXfR, MatrixXfR> getDebuggingSamples() const;
 
@@ -175,7 +176,7 @@ class DWA : public Follower {
    */
   template <typename T>
   TrajSearchResult findBestPath(const Velocity2D &global_vel,
-                                const T &scan_points){
+                                const T &scan_points) {
     // Throw an error if the global path is not set
     if (!currentPath) {
       throw std::invalid_argument("Pointer to global path is NULL. Cannot use "
@@ -185,11 +186,15 @@ class DWA : public Follower {
     // find closest segment to use in cost computation
     determineTarget();
 
-    if(rotate_in_place and currentTrackedTarget_->heading_error > goal_orientation_tolerance * 10.0){
+    if (rotate_in_place and currentTrackedTarget_->heading_error >
+                                goal_orientation_tolerance * 10.0) {
       // If the robot is rotating in place and the heading error is large, we
       // do not need to sample trajectories
       LOG_DEBUG("Rotating In Place ...");
-      auto trajectory = trajSampler->generateSingleSampleFromVel(Velocity2D(0.0, 0.0, - currentTrackedTarget_->heading_error * ctrlimitsParams.omegaParams.maxOmega / M_PI));
+      auto trajectory = trajSampler->generateSingleSampleFromVel(
+          Velocity2D(0.0, 0.0,
+                     -currentTrackedTarget_->heading_error *
+                         ctrlimitsParams.omegaParams.maxOmega / M_PI));
       return TrajSearchResult{trajectory, true, 0.0};
     }
 
@@ -208,7 +213,7 @@ class DWA : public Follower {
     // Evaluate the samples and get the sample with the minimum cost
     return trajCostEvaluator->getMinTrajectoryCost(samples_, currentPath.get(),
                                                    trackedRefPathSegment);
-                                };
+  };
 
 private:
   double max_forward_distance_ = 0.0;

src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h

@@ -26,8 +26,8 @@ class VisionDWA : public DWA, public RGBFollower {
   public:
     VisionDWAConfig() : RGBFollower::RGBFollowerConfig() {
       addParameter(
-        "control_horizon",
-        Parameter(2, 1, 1000, "Number of steps for applying the control"));
+          "control_horizon",
+          Parameter(2, 1, 1000, "Number of steps for applying the control"));
       addParameter(
           "prediction_horizon",
           Parameter(10, 1, 1000, "Number of steps for future prediction"));
@@ -219,8 +219,8 @@ class VisionDWA : public DWA, public RGBFollower {
                      const Bbox2D &target_box_2d, const float yaw = 0.0);
 
   Eigen::Vector2f getErrors() const {
-      return Eigen::Vector2f(dist_error_, orientation_error_);
-    }
+    return Eigen::Vector2f(dist_error_, orientation_error_);
+  }
 
 private:
   VisionDWAConfig config_;
@@ -247,7 +247,8 @@ class VisionDWA : public DWA, public RGBFollower {
    * @param tracking_pose
    * @return Velocity2D
    */
-  Velocity2D getPureTrackingCtrl(const TrackedPose2D &tracking_pose, const bool update_global_error = false);
+  Velocity2D getPureTrackingCtrl(const TrackedPose2D &tracking_pose,
+                                 const bool update_global_error = false);
 
   /**
    * @brief Get the Tracking Control Result based on object tracking and DWA