RRT

CMakeLists.txt

@@ -66,14 +66,26 @@ install(
 )
 
 if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # uncomment the line when a copyright and license is not present in all source files
-  #set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # uncomment the line when this package is not in a git repo
-  #set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_add_gtest(${PROJECT_NAME}_my_test test/test.cpp)
+
+  # Link isc_nav files
+  target_include_directories(${PROJECT_NAME}_my_test PUBLIC src/)
+
+  target_include_directories(${PROJECT_NAME}_my_test PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+
+  # We need to ament link because we use isc_nav files which need ros
+  ament_target_dependencies(
+  ${PROJECT_NAME}_my_test
+  geometry_msgs
+  rclcpp
+  nav_msgs
+  tf2
+  tf2_geometry_msgs
+  tf2_ros
+)
 endif()
 
 ament_package()

include/isc_nav/pure_pursuit.hpp

@@ -27,12 +27,14 @@
 #include <vector>
 #include "utility/point.hpp"
 
+
+
+namespace isc_nav
+{
 typedef std::vector<Point3D> Path;
 typedef std::pair<Point3D, Point3D> Segment3D;
 typedef std::pair<Point2D, Point2D> Segment2D;
 
-namespace PurePursuit
-{
 class PurePursuit 
 {
 public:

include/isc_nav/pure_pursuit_node.hpp

@@ -26,7 +26,7 @@ namespace isc_nav
 class PurePursuitNode : public rclcpp::Node
 {
 public:
-  PurePursuit::PurePursuit m_tracker;
+  PurePursuit m_tracker;
   explicit PurePursuitNode(rclcpp::NodeOptions options);
 
 private:

include/isc_nav/utility/bfs.hpp

@@ -34,7 +34,7 @@ class BreadthFirstSearch
             // TODO: Param
             if ((current - goal_).x < 0.1 && (current - goal_).y < 0.1)
             {
-                std::cout << "Goal found, getting path." << std::endl;
+                //std::cout << "Goal found, getting path." << std::endl;
                 trace_back_path(current, path, came_from);
             }
 

include/isc_nav/utility/distance_helper.hpp

@@ -3,6 +3,9 @@
 #include "point.hpp"
 #include <math.h>
 
+namespace isc_nav
+{
+
 constexpr double EPSILON = 1e-3;
 
 // distance formula function that finds distance between two points
@@ -46,3 +49,4 @@ inline bool approximately_equals( const Point2D& expected, const Point2D& actual
 	return ( std::fabs( actual.x - expected.x ) < EPSILON )
 		&& ( std::fabs( actual.y - expected.y ) < EPSILON );
 }
+}

include/isc_nav/utility/map.hpp

@@ -1,8 +1,10 @@
 #pragma once
 
+#include <cstdint>
 #include <inttypes.h>
-#include <isc_nav/utility/point.hpp>
+#include "../utility/point.hpp"
 #include <nav_msgs/msg/occupancy_grid.hpp>
+#include "utility"
 
 namespace isc_nav
 {
@@ -71,7 +73,7 @@ class CostMap
 
     bool valid_cell(const int32_t& x, const int32_t& y) const noexcept
     {
-        if (x > size_x_ || y > size_y_ ||
+        if (x > (int64_t)size_x_ || y > (int64_t)size_y_ ||
             x < 0       || y < 0)
         {
             return false;
@@ -132,13 +134,9 @@ class CostMap
         return neighbors;
     }
 
-private:
-    uint32_t size_x_;
-    uint32_t size_y_;
-    float resolution_;
-    Point2D origin_;
-
-    std::vector<uint8_t> costmap_;
+    double get_size_x() { return size_x_; }
+    double get_size_y() { return size_y_; }
+    float get_resolution() { return resolution_; }
 
     // Values cooresponding to definition of Occupancy Grid in ROS
     static constexpr int8_t OCC_GRID_UNKNOWN = -1;
@@ -149,5 +147,13 @@ class CostMap
     static constexpr uint8_t NO_INFORMATION = 255;
     static constexpr uint8_t LETHAL_OBSTACLE = 254;
     static constexpr uint8_t FREE_SPACE = 0;
+
+private:
+    uint32_t size_x_;
+    uint32_t size_y_;
+    float resolution_;
+    Point2D origin_;
+
+    std::vector<uint8_t> costmap_;
 };
 } // namespace isc_nav

include/isc_nav/utility/point.hpp

@@ -3,6 +3,8 @@
 #include <tuple>
 #include <vector>
 
+namespace isc_nav
+{
 /**
  * @brief data structure to represent x and y coordinates for the robot
  */
@@ -55,12 +57,13 @@ inline bool operator!=(const Point3D& lhs, const Point3D& rhs)
 {
     return ( lhs.x != rhs.x && lhs.y != rhs.y && lhs.z != rhs.z );
 }
+}
 
 namespace std
 {
-template<> struct hash<Point2D>
+template<> struct hash<isc_nav::Point2D>
 {
-    [[nodiscard]] std::size_t operator()(const Point2D& point) const noexcept
+    [[nodiscard]] std::size_t operator()(const isc_nav::Point2D& point) const noexcept
     {
         return std::hash<size_t>()(static_cast<size_t>(point.x) ^ (static_cast<size_t>(point.y) << 16));
     }