Merge pull request #509 from SteveMacenski/dashing-devel

Dashing cleanup

Author: ayrton04

CMakeLists.txt

@@ -57,8 +57,13 @@ rosidl_target_interfaces(${library_name}
   ${PROJECT_NAME} "rosidl_typesupport_cpp")
 
 add_executable(
-  se_node
-  src/se_node.cpp
+  ekf_node
+  src/ekf_node.cpp
+)
+
+add_executable(
+  ukf_node
+  src/ukf_node.cpp
 )
 
 add_executable(
@@ -86,31 +91,33 @@ ament_target_dependencies(
 )
 
 target_link_libraries(
-  se_node
+  ekf_node
   ${library_name}
 )
 
 ament_target_dependencies(
-  se_node
+  ekf_node
   rclcpp
 )
 
 target_link_libraries(
-  navsat_transform_node
+  ukf_node
   ${library_name}
 )
 
 ament_target_dependencies(
-  navsat_transform_node
+  ukf_node
   rclcpp
 )
 
-install(
-  TARGETS
+target_link_libraries(
   navsat_transform_node
-  se_node
   ${library_name}
-  DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_target_dependencies(
+  navsat_transform_node
+  rclcpp
 )
 
 if(BUILD_TESTING)
@@ -125,48 +132,68 @@ if(BUILD_TESTING)
   ament_add_gtest(filter_base-test test/test_filter_base.cpp)
   target_link_libraries(filter_base-test ${library_name})
 
-  # This test uses se_node node for convenience.
-  ament_add_gtest(test_filter_base_diagnostics_timestamps
+  #### DIAGNOSTICS TESTS ####
+  ament_add_gtest_executable(test_filter_base_diagnostics_timestamps
                     test/test_filter_base_diagnostics_timestamps.cpp)
   target_link_libraries(test_filter_base_diagnostics_timestamps ${library_name})
-  add_dependencies(test_filter_base_diagnostics_timestamps se_node)
+  add_dependencies(test_filter_base_diagnostics_timestamps ekf_node)
+  ament_add_test(test_filter_base_diagnostics_timestamps
+    GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+    TIMEOUT 300
+    COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test/test_filter_base_diagnostics_timestamps.launch.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  )
 
   #### EKF TESTS ######
   ament_add_gtest(test_ekf test/test_ekf.cpp)
   target_link_libraries(test_ekf ${library_name})
 
-  ament_add_gtest(test_ekf_localization_node_interfaces
-  test/test_ekf_localization_node_interfaces.cpp)
+  ament_add_gtest_executable(test_ekf_localization_node_interfaces
+                    test/test_ekf_localization_node_interfaces.cpp)
   target_link_libraries(test_ekf_localization_node_interfaces ${library_name})
-
-  ament_add_gtest(test_ekf_localization_node_bag1 test/test_localization_node_bag_pose_tester.cpp)
-  target_link_libraries(test_ekf_localization_node_bag1 ${library_name})
-
-  ament_add_gtest(test_ekf_localization_node_bag2 test/test_localization_node_bag_pose_tester.cpp)
-  target_link_libraries(test_ekf_localization_node_bag2 ${library_name})
-
-  ament_add_gtest(test_ekf_localization_node_bag3 test/test_localization_node_bag_pose_tester.cpp)
-  target_link_libraries(test_ekf_localization_node_bag3 ${library_name})
+  add_dependencies(test_ekf_localization_node_interfaces ekf_node)
+  ament_add_test(test_ekf_localization_node_interfaces
+    GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+    TIMEOUT 300
+    COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test/test_ekf_localization_node_interfaces.launch.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  )
 
   #### UKF TESTS #####
   ament_add_gtest(test_ukf test/test_ukf.cpp)
   target_link_libraries(test_ukf ${library_name})
 
-  ament_add_gtest(test_ukf_localization_node_interfaces test/test_ukf_localization_node_interfaces.cpp)
+
+  ament_add_gtest_executable(test_ukf_localization_node_interfaces
+                    test/test_ukf_localization_node_interfaces.cpp)
   target_link_libraries(test_ukf_localization_node_interfaces ${library_name})
+  add_dependencies(test_ukf_localization_node_interfaces ekf_node)
+  ament_add_test(test_ukf_localization_node_interfaces
+    GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+    TIMEOUT 300
+    COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test/test_ukf_localization_node_interfaces.launch.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  )
 
-  ament_add_gtest(test_ukf_localization_node_bag1 test/test_localization_node_bag_pose_tester.cpp)
-  target_link_libraries(test_ukf_localization_node_bag1 ${library_name})
+  #### DATA TESTS #####
+#  ament_add_gtest(test_ekf_localization_node_bag1 test/test_localization_node_bag_pose_tester.cpp)
+#  target_link_libraries(test_ekf_localization_node_bag1 ${library_name})
 
-  ament_add_gtest(test_ukf_localization_node_bag2 test/test_localization_node_bag_pose_tester.cpp)
-  target_link_libraries(test_ukf_localization_node_bag2 ${library_name})
+#  ament_add_gtest(test_ekf_localization_node_bag2 test/test_localization_node_bag_pose_tester.cpp)
+#  target_link_libraries(test_ekf_localization_node_bag2 ${library_name})
 
-  ament_add_gtest(test_ukf_localization_node_bag3 test/test_localization_node_bag_pose_tester.cpp)
-  target_link_libraries(test_ukf_localization_node_bag3 ${library_name})
+#  ament_add_gtest(test_ekf_localization_node_bag3 test/test_localization_node_bag_pose_tester.cpp)
+#  target_link_libraries(test_ekf_localization_node_bag3 ${library_name})
+
+#  ament_add_gtest(test_ukf_localization_node_bag1 test/test_localization_node_bag_pose_tester.cpp)
+#  target_link_libraries(test_ukf_localization_node_bag1 ${library_name})
+
+#  ament_add_gtest(test_ukf_localization_node_bag2 test/test_localization_node_bag_pose_tester.cpp)
+#  target_link_libraries(test_ukf_localization_node_bag2 ${library_name})
+
+#  ament_add_gtest(test_ukf_localization_node_bag3 test/test_localization_node_bag_pose_tester.cpp)
+#  target_link_libraries(test_ukf_localization_node_bag3 ${library_name})
 
-  # This forces cppcheck to consider all files in this project to be C++,
-  # including the headers which end with .h, which cppcheck would normally
-  # consider to be C instead.
   ament_cppcheck(LANGUAGE "c++")
   ament_cpplint()
   ament_lint_cmake()
@@ -178,23 +205,40 @@ if(BUILD_TESTING)
     test_filter_base_diagnostics_timestamps
     test_ekf
     test_ekf_localization_node_interfaces
-    test_ekf_localization_node_bag1
-    test_ekf_localization_node_bag2
-    test_ekf_localization_node_bag3
     test_ukf
     test_ukf_localization_node_interfaces
-    test_ukf_localization_node_bag1
-    test_ukf_localization_node_bag2
-    test_ukf_localization_node_bag3
+    #test_ekf_localization_node_bag1
+    #test_ekf_localization_node_bag2
+    #test_ekf_localization_node_bag3
+    #test_ukf_localization_node_bag1
+    #test_ukf_localization_node_bag2
+    #test_ukf_localization_node_bag3
     DESTINATION lib/${PROJECT_NAME}
   )
+
+  # Install test launch config files.
+  install(DIRECTORY
+    test
+    DESTINATION share/${PROJECT_NAME}
+    USE_SOURCE_PERMISSIONS
+  )
 endif()
 
+install(
+  TARGETS
+  navsat_transform_node
+  ekf_node
+  ukf_node
+  ${library_name}
+  DESTINATION lib/${PROJECT_NAME}
+)
+
 # Install params config files.
 install(DIRECTORY
   params
   launch
   DESTINATION share/${PROJECT_NAME}
+  USE_SOURCE_PERMISSIONS
 )
 
 ament_export_include_directories(include)

include/robot_localization/filter_base.hpp

@@ -36,11 +36,8 @@
 
 #include <robot_localization/filter_common.hpp>
 #include <robot_localization/filter_utilities.hpp>
-
-#include <Eigen/Dense>
-#include <rclcpp/duration.hpp>
-#include <rclcpp/macros.hpp>
-#include <rclcpp/time.hpp>
+#include <robot_localization/measurement.hpp>
+#include <robot_localization/filter_state.hpp>
 
 #include <algorithm>
 #include <memory>
@@ -52,96 +49,6 @@
 namespace robot_localization
 {
 
-/**
- * @brief Structure used for storing and comparing measurements
- * (for priority queues)
- *
- * Measurement units are assumed to be in meters and radians. Times are
- * real-valued and measured in seconds.
- */
-struct Measurement
-{
-  // The real-valued time, in seconds, since some epoch (presumably the start of
-  // execution, but any will do)
-  rclcpp::Time time_;
-
-  // The Mahalanobis distance threshold in number of sigmas
-  double mahalanobis_thresh_;
-
-  // The time stamp of the most recent control term (needed for lagged data)
-  rclcpp::Time latest_control_time_;
-
-  // The topic name for this measurement. Needed for capturing previous state
-  // values for new measurements.
-  std::string topic_name_;
-
-  // This defines which variables within this measurement actually get passed
-  // into the filter. std::vector<bool> is generally frowned upon, so we use
-  // ints.
-  std::vector<bool> update_vector_;
-
-  // The most recent control vector (needed for lagged data)
-  Eigen::VectorXd latest_control_;
-
-  // The measurement and its associated covariance
-  Eigen::VectorXd measurement_;
-  Eigen::MatrixXd covariance_;
-
-  // We want earlier times to have greater priority
-  bool operator()(
-    const std::shared_ptr<Measurement> & a,
-    const std::shared_ptr<Measurement> & b)
-  {
-    return (*this)(*(a.get()), *(b.get()));
-  }
-
-  bool operator()(const Measurement & a, const Measurement & b)
-  {
-    return a.time_ > b.time_;
-  }
-
-  Measurement()
-  : time_(0), mahalanobis_thresh_(std::numeric_limits<double>::max()),
-    latest_control_time_(0), topic_name_(""), latest_control_() {}
-};
-using MeasurementPtr = std::shared_ptr<Measurement>;
-
-/**
- * @brief Structure used for storing and comparing filter states
- *
- * This structure is useful when higher-level classes need to remember filter
- * history. Measurement units are assumed to be in meters and radians. Times are
- * real-valued and measured in seconds.
- */
-struct FilterState
-{
-  // The filter state vector
-  Eigen::VectorXd state_;
-
-  // The filter error covariance matrix
-  Eigen::MatrixXd estimate_error_covariance_;
-
-  // The most recent control vector
-  Eigen::VectorXd latest_control_;
-
-  // The time stamp of the most recent measurement for the filter
-  rclcpp::Time last_measurement_time_;
-
-  // The time stamp of the most recent control term
-  rclcpp::Time latest_control_time_;
-
-  // We want the queue to be sorted from latest to earliest timestamps.
-  bool operator()(const FilterState & a, const FilterState & b)
-  {
-    return a.last_measurement_time_ < b.last_measurement_time_;
-  }
-
-  FilterState()
-  : state_(), estimate_error_covariance_(), latest_control_(),
-    last_measurement_time_(0.0), latest_control_time_(0) {}
-};
-using FilterStatePtr = std::shared_ptr<FilterState>;
-
 class FilterBase
 {
 public:
@@ -384,43 +291,13 @@ class FilterBase
    * @param[in] deceleration_gain - Gain applied to deceleration control error
    * @return a usable acceleration estimate for the control vector
    */
-  inline double computeControlAcceleration(
+  double computeControlAcceleration(
     const double state,
     const double control,
     const double acceleration_limit,
     const double acceleration_gain,
     const double deceleration_limit,
-    const double deceleration_gain)
-  {
-    FB_DEBUG("---------- FilterBase::computeControlAcceleration ----------\n");
-
-    const double error = control - state;
-    const bool same_sign = (::fabs(error) <= ::fabs(control) + 0.01);
-    const double set_point = (same_sign ? control : 0.0);
-    const bool decelerating = ::fabs(set_point) < ::fabs(state);
-    double limit = acceleration_limit;
-    double gain = acceleration_gain;
-
-    if (decelerating) {
-      limit = deceleration_limit;
-      gain = deceleration_gain;
-    }
-
-    const double final_accel = std::min(std::max(gain * error, -limit), limit);
-
-    FB_DEBUG("Control value: " <<
-      control << "\n" <<
-      "State value: " << state << "\n" <<
-      "Error: " << error << "\n" <<
-      "Same sign: " << (same_sign ? "true" : "false") << "\n" <<
-      "Set point: " << set_point << "\n" <<
-      "Decelerating: " << (decelerating ? "true" : "false") << "\n" <<
-      "Limit: " << limit << "\n" <<
-      "Gain: " << gain << "\n" <<
-      "Final is " << final_accel << "\n");
-
-    return final_accel;
-  }
+    const double deceleration_gain);
 
   /**
    * @brief Keeps the state Euler angles in the range [-pi, pi]