Test for reverse traj

common/CMakeLists.txt

@@ -105,6 +105,12 @@ add_test(relative_trajectory_point_test
 target_link_libraries(test_relativetrajectory
 	${MAESTRO_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 ${MAESTRO_UTIL_TARGET}\")")
 install(TARGETS ${MAESTRO_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

@@ -213,6 +213,23 @@ Trajectory::TrajectoryPoint Trajectory::TrajectoryPoint::relativeTo(
 	return relative;
 }
 
+
+bool Trajectory::TrajectoryPoint::operator==(
+		const TrajectoryPoint &other) const {
+	return getMode() == other.getMode()
+			&& abs(getTime() - other.getTime()) < 1e-6
+			&& (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

@@ -94,6 +94,8 @@ class Trajectory {
 
 		std::string toString() const;
 		std::string getFormatString() const;
+
+		bool operator==(const TrajectoryPoint& other) const;
     private:
         double time = 0;
 		Eigen::Vector3d position; //! x, y, z [m]
@@ -134,6 +136,8 @@ class Trajectory {
 	Trajectory reversed() const;
 	Trajectory rescaledToVelocity(const double vel_m_s) const;
 
+	bool operator==(const Trajectory& other) const;
+
 private:
 	static const std::regex fileHeaderPattern;
 	static const std::regex fileLinePattern;