updated tests to the new format

CMakeLists.txt

@@ -27,6 +27,7 @@ set(CATKIN_DEPENDENCIES
   mrs_msgs
   mrs_lib
   nlopt
+  mrs_uav_testing
   )
 
 find_package(catkin REQUIRED COMPONENTS
@@ -47,7 +48,7 @@ set(LIBRARIES
   )
 
 catkin_package(
-  INCLUDE_DIRS include
+  INCLUDE_DIRS include test/include
   LIBRARIES ${LIBRARIES}
   CATKIN_DEPENDS ${CATKIN_DEPENDENCIES}
   DEPENDS Eigen
@@ -70,6 +71,7 @@ add_library(MrsUavTrajectoryGeneration_MrsTrajectoryGeneration
 
 include_directories(
   include
+  test/include
   ${catkin_INCLUDE_DIRS}
   ${Eigen_INCLUDE_DIRS}
   ${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake

package.xml

@@ -20,6 +20,7 @@
   <depend>std_msgs</depend>
   <depend>mrs_msgs</depend>
   <depend>mrs_lib</depend>
+  <depend>mrs_uav_testing</depend>
   <depend version_gte="2.4.2">nlopt</depend>
 
   <export>

src/mrs_trajectory_generation.cpp

@@ -616,7 +616,7 @@ std::tuple<bool, std::string, mrs_msgs::TrajectoryReference> MrsTrajectoryGenera
   bool              safe = false;
   int               traj_idx;
   std::vector<bool> segment_safeness;
-  double            max_deviation;
+  double            max_deviation = 0;
 
   eth_mav_msgs::EigenTrajectoryPoint::Vector trajectory;
 
@@ -903,6 +903,15 @@ std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector> MrsTrajectoryGeneratio
   double vertical_speed_lim        = std::min(constraints.vertical_ascending_speed, constraints.vertical_descending_speed);
   double vertical_acceleration_lim = std::min(constraints.vertical_ascending_acceleration, constraints.vertical_descending_acceleration);
 
+  v_max_horizontal = constraints.horizontal_speed;
+  a_max_horizontal = constraints.horizontal_acceleration;
+
+  v_max_vertical = vertical_speed_lim;
+  a_max_vertical = vertical_acceleration_lim;
+
+  j_max_horizontal = constraints.horizontal_jerk;
+  j_max_vertical   = std::min(constraints.vertical_ascending_jerk, constraints.vertical_descending_jerk);
+
   if (override_constraints_) {
 
     bool can_change = (hypot(initial_state.velocity.x, initial_state.velocity.y) < override_max_velocity_horizontal_) &&
@@ -927,20 +936,8 @@ std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector> MrsTrajectoryGeneratio
 
       ROS_WARN("[MrsTrajectoryGeneration]: overrifing constraints refused due to possible infeasibility");
     }
-
-  } else {
-
-    v_max_horizontal = constraints.horizontal_speed;
-    a_max_horizontal = constraints.horizontal_acceleration;
-
-    v_max_vertical = vertical_speed_lim;
-    a_max_vertical = vertical_acceleration_lim;
-
-    j_max_horizontal = constraints.horizontal_jerk;
-    j_max_vertical   = std::min(constraints.vertical_ascending_jerk, constraints.vertical_descending_jerk);
   }
 
-
   double v_max_heading, a_max_heading, j_max_heading;
 
   if (relax_heading) {
@@ -1424,19 +1421,14 @@ std::vector<int> MrsTrajectoryGeneration::getTrajectorySegmentCenterIdxs(const e
     // next sample
     const vec3_t next_sample = vec3_t(trajectory[i + 1].position_W[0], trajectory[i + 1].position_W[1], trajectory[i + 1].position_W[2]);
 
-    // segment start
-    const vec3_t segment_start =
-        vec3_t(waypoints.at(last_segment_start).coords[0], waypoints.at(last_segment_start).coords[1], waypoints.at(last_segment_start).coords[2]);
-
     // segment end
     const vec3_t segment_end =
         vec3_t(waypoints.at(last_segment_start + 1).coords[0], waypoints.at(last_segment_start + 1).coords[1], waypoints.at(last_segment_start + 1).coords[2]);
 
-    const double distance_from_segment = distFromSegment(sample, segment_start, segment_end);
-
     const double segment_end_dist = distFromSegment(segment_end, sample, next_sample);
 
     if (segment_end_dist < 0.05 && last_segment_start < (int(waypoints.size()) - 2)) {
+
       last_segment_start++;
 
       segment_centers.push_back(int(floor(double(i - last_segment_start_sample) / 2.0)));

test/CMakeLists.txt

@@ -1,25 +1,5 @@
 find_package(rostest REQUIRED)
 
-### WARNING
-### NEVER name your test build unit the same as the folder that it is placed in!
-### e.g., this will cause problems:
-###   add_rostest_gtest(my_test
-###     my_test/basic.test
-###     my_test/test.cpp
-###     )
+add_subdirectory(service_fly_now)
 
-# basic test
-
-add_rostest_gtest(test_basic
-  basic/basic.test
-  basic/test.cpp
-  )
-
-target_link_libraries(test_basic
-  ${catkin_LIBRARIES}
-  )
-
-add_dependencies(test_basic
-  ${${PROJECT_NAME}_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
-  )
+add_subdirectory(topic_fly_now)

test/all_tests.sh

@@ -5,31 +5,8 @@ set -e
 trap 'last_command=$current_command; current_command=$BASH_COMMAND' DEBUG
 trap 'echo "$0: \"${last_command}\" command failed with exit code $?"' ERR
 
-PACKAGE="mrs_uav_trajectory_generation"
-VERBOSE=1
-
-[ "$VERBOSE" = "0" ] && TEXT_OUTPUT=""
-[ "$VERBOSE" = "1" ] && TEXT_OUTPUT="-t"
-
 # build the package
-catkin build $PACKAGE # it has to be fully built normally before building with --catkin-make-args tests
-catkin build $PACKAGE --no-deps --catkin-make-args tests
-
-TEST_FILES=$(find . -name "*.test" -type f -printf "%f\n")
-
-for TEST_FILE in `echo $TEST_FILES`; do
-
-  echo "$0: running test '$TEST_FILE'"
-
-  # folder for test results
-  TEST_RESULT_PATH=$(realpath /tmp/$RANDOM)
-  mkdir -p $TEST_RESULT_PATH
-
-  # run the test
-  rostest $PACKAGE $TEST_FILE $TEXT_OUTPUT --results-filename=$PACKAGE.test --results-base-dir="$TEST_RESULT_PATH"
-
-  # evaluate the test results
-  echo test result path is $TEST_RESULT_PATH
-  catkin_test_results "$TEST_RESULT_PATH"
+catkin build --this # it has to be fully built normally before building with --catkin-make-args tests
+catkin build --this --no-deps --catkin-make-args tests
 
-done
+catkin test --this -i -p 1 -s