Remove 2-Suffix and rebase on main

CMakeLists.txt

@@ -24,25 +24,33 @@ include_directories(
 
 add_library(${PROJECT_NAME} SHARED
   src/trajectory_generator.cpp
+  src/logger.cpp
   src/constant_acceleration_solver.cpp
   src/path_manager.cpp
-  src/point.cpp
   src/path.cpp
-  src/section_constraint.cpp
   src/section.cpp
-  src/linear_segment.cpp
-  src/blend_segment.cpp
-  src/segment_constraint.cpp
   src/utility_functions.cpp
+  src/value_group.cpp
+  src/symbol_group.cpp
 )
 
 target_link_libraries (${PROJECT_NAME} Eigen3::Eigen)
 
+add_executable(test tests/test.cpp)
+target_link_libraries (test ${PROJECT_NAME})
+
 install(TARGETS sotg DESTINATION lib)
 
+
+#add_definitions(-DVERBOSE) # Enable printing info and warn messages to cout using default logger
+
   ###########
   ## Debug ##
   ###########
 
 #add_definitions(-DDEBUG) #uncomment for additional debug output
-#add_definitions(-DVERBOSE_TESTS) # print all calculated values
+#add_definitions(-DVERBOSE_TESTS) # print all calculated values
+
+  #############
+  ## Testing ##
+  #############

include/sotg/constant_acceleration_solver.hpp

@@ -6,13 +6,12 @@
 
 #include <bits/stdc++.h>
 
-#include "sotg/blend_segment.hpp"
 #include "sotg/kinematic_solver.hpp"
-#include "sotg/linear_segment.hpp"
 #include "sotg/section.hpp"
-#include "sotg/segment_constraint.hpp"
 #include "sotg/utility_functions.hpp"
 
+#include "sotg/result.hpp"
+
 namespace SOTG {
 namespace detail {
 
@@ -21,65 +20,30 @@ namespace detail {
     // bang coast bang profile
     class ConstantAccelerationSolver : public KinematicSolver {
     private:
-        void calcSegmentPreparations(const Section& pre_section, const Section& post_section,
-                                     std::vector<double>& a_max_post, std::vector<double>& a_max_pre,
-                                     double& L_acc_magnitude_post, double& T_acc_post, double& T_acc_pre);
-
-        void calcAccAndVelPerDoF(const Section& section, std::vector<double>& a_max_vec,
-                                 std::vector<double>& v_max_vec);
+        void calcPhaseDurationAndDistance(double a_max, double v_max, double L_total, PhaseComponent& acc_phase_single_dof,
+                                      PhaseComponent& coast_phase_single_dof, PhaseComponent& dec_phase_single_dof);
 
-        void calcPhaseTimeAndDistance(double& a_max, double& v_max, double L_total, PhaseDoF& acc_phase_single_dof,
-                                      PhaseDoF& coast_phase_single_dof, PhaseDoF& dec_phase_single_dof);
+        void calcDurationAndDistancePerGroup(double& a_max, double& v_max, double distance,
+                                               double& duration);
 
-        void calcTotalTimeAndDistanceSingleDoF(double& a_max, double& v_max, double total_length,
-                                               double& total_time);
-
-        void calcTimesAndLengthsMultiDoF(Phase& acc_phase, Phase& coast_phase, Phase& dec_phase,
+        void calcDurationAndDistance(Phase& acc_phase, Phase& coast_phase, Phase& dec_phase,Point& start_point, Point& end_point,
                                          std::vector<double>& total_time_per_dof,
-                                         std::vector<double>& total_length_per_dof, Point diff,
-                                         std::vector<double>& a_max_vec, std::vector<double>& v_max_vec);
-
-        void calcSecondBlendingDist(double T_blend, double T_acc_post, double a_max_magnitude_post,
-                                    double vel_pre_blend_magnitude, double blending_dist_pre,
-                                    double& blending_dist_post, size_t segment_id);
+                                         std::vector<double>& total_length_per_dof, std::map<std::string, double>& group_acc, std::map<std::string, double>& group_vel);
 
-        void calcPosAndVelSingleDoFLinear(double section_length, const Phase& phase,
+        void calcPosAndVelSingleGroup(double section_length, const Phase& phase,
                                           double phase_distance_to_p_start, double t_phase, double a_max_reduced,
                                           double v_max_reduced, double& pos_magnitude,
                                           double& vel_magnitude) const;
-        void calcVelAndTimeByDistance(const Section& section, double distance, Point& velocity_per_dof,
-                                      double& time_when_distance_is_reached);
-
-        void calcPreBlendParams(double blending_dist_pre, const Section& pre_section, Point& A_blend,
-                                double& T_blend, double& vel_pre_blend_magnitude,
-                                double& absolute_blend_start_time);
-
-        void calcPostBlendParams(double blending_dist_pre, const Section& pre_section, Point& C_blend,
-                                 double& T_blend, double& vel_pre_blend_magnitude, double& t_abs_start_blend);
-
-        bool isBlendAccelerationTooHigh(const std::vector<double>& a_max, const double& T_blend,
-                                        const double& vel_pre_blend_magnitude,
-                                        const double& vel_post_blend_magnitude, const Section& pre_section,
-                                        const Section& post_section, size_t segment_id);
-        void setNoBlendingParams(const Section& pre_section, const Section& post_section, double& T_blend,
-                                 double& absolute_blend_start_time_with_shift,
-                                 double& absolute_blend_end_time_without_shift, Point& A_blend, Point& C_blend,
-                                 double& vel_pre_blend_magnitude, double& vel_post_blend_magnitude);
 
     public:
-        Section calcSection(Point& p_start_ref, Point& p_end_ref, SectionConstraint constraint_copy,
-                            size_t section_id) override;
-        std::shared_ptr<BlendSegment> calcBlendSegment(Section& pre_section, Section& post_section,
-                                                       const SegmentConstraint& constraint, size_t segment_id,
-                                                       std::map<std::string, double>& debug_output) override;
-
-        void calcPosAndVelSection(double t_section, const Section& section, Point& pos, Point& vel) const override;
+        ConstantAccelerationSolver(SymbolGroupMap& symbol_map, const Logger& logger)
+            : KinematicSolver(symbol_map, logger)
+        {
+        }
 
-        void calcPosAndVelLinearSegment(double t_section, const LinearSegment& segment, Point& pos,
-                                        Point& vel) const override;
+        Section calcSection(Point& p_start_ref, Point& p_end_ref, size_t section_id) override;
 
-        void calcPosAndVelBlendSegment(double t_segment, const BlendSegment& segment, Point& pos,
-                                       Point& vel) const override;
+        void calcPosAndVelSection(double t_section, Section& section, Result& result) const override;
     };
 }  // namespace detail
 }  // namespace SOTG

include/sotg/kinematic_solver.hpp

@@ -2,35 +2,31 @@
 
 #include <map>
 #include <memory>
+#include <string>
 
-#include "sotg/blend_segment.hpp"
-#include "sotg/linear_segment.hpp"
+#include "sotg/logger.hpp"
 #include "sotg/section.hpp"
-#include "sotg/segment.hpp"
+#include "sotg/result.hpp"
 
 namespace SOTG {
 namespace detail {
     // Derived classes of this base class implement the logic for section and
     // segment generation aswell as the calculation of position and velocity for a
     // specific point in time
     class KinematicSolver {
-    public:
-        virtual Section calcSection(Point& p_start_ref, Point& p_end_ref, SectionConstraint constraint_copy,
-                                    size_t section_id)
-            = 0;
-        virtual std::shared_ptr<BlendSegment>
-        calcBlendSegment(Section& pre_section, Section& post_section, const SegmentConstraint& constraint,
-                         size_t segment_id, std::map<std::string, double>& debug_output)
-            = 0;
+    protected:
+        SymbolGroupMap& symbol_map_;
+        const Logger& logger_;
 
-        virtual void calcPosAndVelSection(double t_section, const Section& section, Point& pos,
-                                          Point& vel) const = 0;
+    public:
+        KinematicSolver(SymbolGroupMap& symbol_map, const Logger& logger)
+            : symbol_map_(symbol_map), logger_(logger)
+        {
+        }
 
-        virtual void calcPosAndVelLinearSegment(double t_section, const LinearSegment& segment, Point& pos,
-                                                Point& vel) const = 0;
+        virtual Section calcSection(Point& p_start_ref, Point& p_end_ref, size_t section_id) = 0;
 
-        virtual void calcPosAndVelBlendSegment(double t_segment, const BlendSegment& segment, Point& pos,
-                                               Point& vel) const = 0;
+        virtual void calcPosAndVelSection(double t_section, Section& section, Result& result) const = 0;
 
         virtual ~KinematicSolver() = default;
     };

include/sotg/logger.hpp

@@ -0,0 +1,18 @@
+#pragma once
+
+#include <iostream>
+
+namespace SOTG {
+
+/*The Logger class can be used in order to pass SOTG internal warnings and debugging info to the user instead
+ * of printing them to directly to cout*/
+class Logger {
+public:
+    enum MsgType { INFO, WARNING, DEBUG };
+
+    virtual void log(const std::string& message, MsgType type = INFO) const;
+
+    virtual ~Logger() = default;
+};
+
+}  // namespace SOTG

include/sotg/path.hpp

@@ -1,4 +1,3 @@
-
 #pragma once
 
 #include <string>
@@ -18,10 +17,10 @@ class Path {
 
 public:
     void addPoint(Point point);
-    void addPoint(const std::vector<Eigen::VectorXd>& old_point);
-    void addPoint(const std::vector<std::vector<double>>& old_point);
+
     size_t getNumWaypoints() { return waypoints_.size(); };
-    Point& getPoint(size_t index);
+    Point& getPoint(std::string name);
+    Point& getPoint(size_t index) { return waypoints_[index]; }
 
     std::vector<Point>::iterator begin() { return waypoints_.begin(); }
     std::vector<Point>::iterator end() { return waypoints_.end(); }

include/sotg/path_manager.hpp

@@ -5,13 +5,9 @@
 #include <memory>
 #include <vector>
 
-#include "sotg/blend_segment.hpp"
 #include "sotg/kinematic_solver.hpp"
-#include "sotg/linear_segment.hpp"
 #include "sotg/path.hpp"
 #include "sotg/section.hpp"
-#include "sotg/section_constraint.hpp"
-#include "sotg/segment_constraint.hpp"
 
 namespace SOTG {
 namespace detail {
@@ -22,34 +18,26 @@ namespace detail {
     private:
         Path path_;
         std::list<Section> sections_;
-        std::list<std::shared_ptr<Segment>> segments_;
-
-        std::vector<SectionConstraint> section_constraints_;
-        std::vector<SegmentConstraint> segment_constraints_;
 
         std::shared_ptr<KinematicSolver> kinematic_solver_;
 
         std::vector<std::map<std::string, double>>& debug_info_vec_;
 
         void resetSections();
-        void resetSegments();
-        void generateBlendSegments();
-        void generateLinearSegments();
 
     public:
         PathManager(std::shared_ptr<KinematicSolver> solver,
-                    std::vector<std::map<std::string, double>>& debug_info_vec);
+                     std::vector<std::map<std::string, double>>& debug_info_vec);
 
-        void resetPath(Path path, std::vector<SectionConstraint> section_constraints,
-                       std::vector<SegmentConstraint> segment_constraints);
+        void resetPath(Path path);
 
         int getNumSections() { return sections_.size(); }
-        int getNumSegments() { return segments_.size(); }
-        const std::list<std::shared_ptr<Segment>>& getSegments() const { return segments_; }
+
         const std::list<Section>& getSections() const { return sections_; }
 
-        const Section& getSectionAtTime(double time);
-        const Segment& getSegmentAtTime(double time);
+        Section& getSectionAtTime(double time);
+
+        const Path& getPath() { return path_; }
 
         std::ostream& operator<<(std::ostream& out);
     };