Test for reverse traj

common/CMakeLists.txt

@@ -142,6 +142,12 @@ add_test(relative_trajectory_point_test
 target_link_libraries(test_relativetrajectory
 	${ATOS_COMMON_TARGET}
 )
+add_executable(test_reversetrajectory tests/test_reversetrajectory.cpp)
+add_test(reverse_trajectory_test
+	${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/test_reversetrajectory)
+target_link_libraries(test_reversetrajectory
+	${MAESTRO_COMMON_TARGET}
+)
 
 # Installation rules
 install(CODE "MESSAGE(STATUS \"Installing target ${ATOS_UTIL_TARGET}\")")

common/tests/test_reversetrajectory.cpp

@@ -0,0 +1,41 @@
+#include "../trajectory.hpp"
+#include <cstdlib>
+
+static void test_double_reverse();
+
+int main(int argc, char** argv) {
+	try {
+		test_double_reverse();
+		exit(EXIT_SUCCESS);
+	} catch (std::runtime_error& e) {
+		std::cerr << "Test " << __FILE__ << " failed: " << std::endl
+				  << e.what() << std::endl;
+		exit(EXIT_FAILURE);
+	}
+}
+
+void test_double_reverse() {
+
+	Trajectory t;
+	srand(1234);
+	for (int i = 0; i < 250; ++i) {
+		Trajectory::TrajectoryPoint p;
+		p.setTime(i*0.1);
+		p.setXCoord(rand() % 100 - 50);
+		p.setYCoord(rand() % 100 - 50);
+		p.setZCoord(rand() % 100 - 50);
+		p.setHeading(rand() % 100 - 5*M_PI);
+		p.setCurvature(rand() % 100 - 50);
+		p.setLongitudinalVelocity(rand() % 100 - 50);
+		p.setLongitudinalAcceleration(rand() % 100 - 50);
+		p.setLateralVelocity(rand() % 100 - 50);
+		p.setLateralAcceleration(rand() % 100 - 50);
+
+		t.points.push_back(p);
+	}
+
+	if (t == t.reversed().reversed()) {
+		return;
+	}
+	throw std::runtime_error("Reversing twice does not produce the original trajectory");
+}

common/trajectory.cpp

@@ -347,6 +347,23 @@ Trajectory::TrajectoryPoint Trajectory::TrajectoryPoint::relativeTo(
 	return relative;
 }
 
+
+bool Trajectory::TrajectoryPoint::operator==(
+		const TrajectoryPoint &other) const {
+	return getMode() == other.getMode()
+			&& abs(getTime() - other.getTime()).count() < 1e3
+			&& (getPosition() - other.getPosition()).norm() < 1e-6
+			&& (getVelocity() - other.getVelocity()).norm() < 1e-4
+			&& (getAcceleration() - other.getAcceleration()).norm() < 1e-2
+			&& abs(getHeading() - other.getHeading()) < 1e-4
+			&& abs(getCurvature() - other.getCurvature()) < 1e-4;
+}
+
+bool Trajectory::operator==(
+		const Trajectory &other) const {
+	return points == other.points && id == other.id;
+}
+
 Trajectory Trajectory::relativeTo(
 		const Trajectory &other) const {
 	using namespace Eigen;

common/trajectory.hpp

@@ -109,8 +109,15 @@ class Trajectory : public Loggable {
 
 		std::string toString() const;
 		std::string getFormatString() const;
+<<<<<<< HEAD
+
+		bool operator==(const TrajectoryPoint& other) const;
+    private:
+        double time = 0;
+=======
 	private:
 		std::chrono::milliseconds time = std::chrono::milliseconds(0);
+>>>>>>> dev
 		Eigen::Vector3d position; //! x, y, z [m]
 		double heading = 0;		  //! Heading ccw from x axis [rad]
 		Eigen::Vector2d velocity; //! Vehicle frame, x forward [m/s]
@@ -171,6 +178,7 @@ class Trajectory : public Loggable {
 		return newTrajectory;
 	}
 
+	bool operator==(const Trajectory& other) const;
 	bool isValid() const;
 private:
 	static const std::regex fileHeaderPattern;