Merge branch 'main' into fix/remove_2d_trajectory

# Conflicts:
#	src/EnvironmentXYZTheta.cpp

Author: haider8645

paper/paper.pdf

@@ -1,19 +1,15 @@
 #include "Dijkstra.hpp"
-#include <traversability_generator3d/TraversabilityConfig.hpp>
 #include <maps/grid/TraversabilityMap3d.hpp>
+#include <traversability_generator3d/TraversabilityConfig.hpp>
 
 using namespace maps::grid;
 
 namespace ugv_nav4d
 {
-
-    
 void Dijkstra::computeCost(const TraversabilityNodeBase* source,
                            std::unordered_map<const TraversabilityNodeBase*, double>& outDistances,
                            const traversability_generator3d::TraversabilityConfig& config)
-{
-
-    
+{   
     outDistances.clear();
     outDistances[source] = 0.0;
 
@@ -42,7 +38,7 @@ void Dijkstra::computeCost(const TraversabilityNodeBase* source,
                                        v->getIndex().y() * config.gridResolution,
                                        v->getHeight());
 
-            const double distance = getHeuristicDistance(vPos, uPos, config);
+            const double distance = (vPos - uPos).norm();
             double distance_through_u = dist + distance;
 
             if (outDistances.find(v) == outDistances.end() || 
@@ -55,12 +51,4 @@ void Dijkstra::computeCost(const TraversabilityNodeBase* source,
         }
     }
 }
-
-double Dijkstra::getHeuristicDistance(const Eigen::Vector3d& a, const Eigen::Vector3d& b,
-                            const traversability_generator3d::TraversabilityConfig& config)
-{
-    return (a - b).norm();
-}
-
-    
 }

src/Dijkstra.cpp

@@ -1,18 +1,18 @@
 #pragma once
 #include <unordered_map>
 #include <base/Eigen.hpp>
-#include <traversability_generator3d/TraversabilityConfig.hpp>
 
 namespace maps { namespace grid 
 {
 class TraversabilityNodeBase;
 }}
 
+namespace traversability_generator3d{
+    class TraversabilityConfig;
+}
+
 namespace ugv_nav4d
 {
-
-class traversability_generator3d::TraversabilityConfig;
-    
 class Dijkstra
 {
 public:
@@ -23,10 +23,6 @@ class Dijkstra
     static void computeCost(const maps::grid::TraversabilityNodeBase* source,
                             std::unordered_map<const maps::grid::TraversabilityNodeBase*, double> &outDistances,
                             const traversability_generator3d::TraversabilityConfig& config);
-
-private:
-    static double getHeuristicDistance(const Eigen::Vector3d& a, const Eigen::Vector3d& b,
-                                       const traversability_generator3d::TraversabilityConfig& config);
 };
 
 } 

src/Dijkstra.hpp

@@ -966,7 +966,7 @@ void EnvironmentXYZTheta::getTrajectory(const vector<int>& stateIDPath,
         V3DD::CLEAR_DRAWING("ugv_nav4d_trajectory");
 #endif
 
-    int indexOfMotionToUpdate{stateIDPath.size()-2};
+    size_t indexOfMotionToUpdate{stateIDPath.size()-2};
     const Motion& finalMotion = getMotion(stateIDPath[stateIDPath.size()-2], stateIDPath[stateIDPath.size()-1]);
     if (finalMotion.type == Motion::Type::MOV_POINTTURN && stateIDPath.size() > 2){ //assuming that there are no consecutive point turns motion at the end of a planned trajectory
         indexOfMotionToUpdate = stateIDPath.size()-3;
@@ -1097,26 +1097,10 @@ void EnvironmentXYZTheta::getTrajectory(const vector<int>& stateIDPath,
         }
         else
         {
-            if (curMotion.type == Motion::Type::MOV_BACKWARD)
-            {
-                curPart.speed = -mobilityConfig.translationSpeed;
-            }
-            else
-            {
-                curPart.speed = mobilityConfig.translationSpeed;
-            }
             SubTrajectory curPartSub(curPart);
-            switch (curMotion.type) {
-                case Motion::Type::MOV_FORWARD:
-                    curPartSub.driveMode = DriveMode::ModeAckermann;
-                    break;
-                case Motion::Type::MOV_BACKWARD:
-                    curPartSub.driveMode = DriveMode::ModeAckermann;
-                    break;
-                case Motion::Type::MOV_LATERAL:
-                    curPartSub.driveMode = DriveMode::ModeSideways;
-                    break;
-            }
+            curPartSub.speed = (curMotion.type == Motion::Type::MOV_BACKWARD) ? -mobilityConfig.translationSpeed : mobilityConfig.translationSpeed;
+            curPartSub.driveMode = (curMotion.type == Motion::Type::MOV_LATERAL) ? DriveMode::ModeSideways : DriveMode::ModeAckermann;
+
             result.push_back(curPartSub);
 
             if (updateGoalPose){