Port to Dashing: melodic-devel 2.5.1 to 2.6.2 (cra-ros-pkg#526)

* Fixing issue with potential seg fault (cherry picked from commit a426d7d) * adding log statements for nans in the invertable matrix (cherry picked from commit 627ff2a) * redefining error in ros filter for ros logging (cherry picked from commit 327e3fd) * swap validateFilterOutput logic and 1 line (cherry picked from commit b0964de) * Add published accel topic to documentation (cherry picked from commit 8c992fa) * template for toggle service (cherry picked from commit a1ce8fc) * functionize repetative clearing (cherry picked from commit a86fa92) * add service message (cherry picked from commit 3656917) * fix compiler errors * adding service caller information in rosout (cherry picked from commit 8b743cc) * snake -> camel (cherry picked from commit 62c3336) * adding time reset to the toggle off mode to prevent covariance explosion (cherry picked from commit 91ad1c2) * check that filter_ is valid before setting a time (cherry picked from commit abd26ad) * Adding more output for measurement history failures (cherry picked from commit f4e60a9) * Updating comment and output (cherry picked from commit 33f9c89) * Fixing tests (cherry picked from commit c492276) * Fixing CMakeLists (cherry picked from commit 6dbbf3e) * correct CMakeLists merge error; fix warnings; linters * fix RLE/RLL tests Co-authored-by: Tom Moore <[email protected]> Co-authored-by: Steven Macenski <[email protected]> Co-authored-by: Oleg Kalachev <[email protected]>
arece-fs · Jan 28, 2020 · 3c93104 · 3c93104
1 parent 8edc978
commit 3c93104
Show file tree

Hide file tree

Showing 10 changed files with 209 additions and 58 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -25,14 +25,17 @@ find_package(tf2_eigen REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(Eigen3 REQUIRED)
-find_package(yaml-cpp REQUIRED)
+
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(YAML_CPP yaml-cpp)
 
 set(library_name rl_lib)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/GetState.srv"
   "srv/SetDatum.srv"
   "srv/SetPose.srv"
+  "srv/ToggleFilterProcessing.srv"
   DEPENDENCIES
     builtin_interfaces
     geometry_msgs
@@ -85,7 +88,7 @@ add_executable(
 target_link_libraries(
   ${library_name}
   ${EIGEN3_LIBRARIES}
-  yaml-cpp
+  ${YAML_CPP_LIBRARIES}
 )
 
 ament_target_dependencies(
@@ -221,17 +224,28 @@ if(BUILD_TESTING)
 #  target_link_libraries(test_ukf_localization_node_bag3 ${library_name})
 
   #### RLE/RLL TESTS #####
-  ament_add_gtest(test_robot_localization_estimator
+  ament_add_gtest_executable(test_robot_localization_estimator
                   test/test_robot_localization_estimator.cpp)
   target_link_libraries(test_robot_localization_estimator ${library_name})
+  ament_add_test(test_robot_localization_estimator
+    GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+    TIMEOUT 100
+    COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test/test_robot_localization_estimator.launch.py"
+    WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  )
 
-  ament_add_gtest(test_ros_robot_localization_listener
+  ament_add_gtest_executable(test_ros_robot_localization_listener
                   test/test_ros_robot_localization_listener.cpp)
   target_link_libraries(test_ros_robot_localization_listener ${library_name})
-
-  ament_add_gtest(test_ros_robot_localization_listener_publisher
+  ament_add_gtest_executable(test_ros_robot_localization_listener_publisher
                   test/test_ros_robot_localization_listener_publisher.cpp)
   target_link_libraries(test_ros_robot_localization_listener_publisher ${library_name})
+  ament_add_test(test_ros_robot_localization_listener
+    GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+    TIMEOUT 100
+    COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test/test_ros_robot_localization_listener.launch.py"
+    WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  )
 
   ament_cppcheck(LANGUAGE "c++")
   ament_cpplint()

diff --git a/doc/state_estimation_nodes.rst b/doc/state_estimation_nodes.rst
@@ -305,6 +305,7 @@ Published Topics
 ================
 
 * ``odometry/filtered`` (`nav_msgs/Odometry <http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html>`_)
+* ``accel/filtered`` (`geometry_msgs/AccelWithCovarianceStamped <http://docs.ros.org/api/geometry_msgs/html/msg/AccelWithCovarianceStamped.html>`_) (if enabled)
 
 Published Transforms
 ====================

diff --git a/include/robot_localization/robot_localization_estimator.hpp b/include/robot_localization/robot_localization_estimator.hpp
@@ -211,7 +211,7 @@ class RobotLocalizationEstimator
   //!
   void interpolate(
     const EstimatorState & given_state_1,
-    const EstimatorState & given_state_2,
+    const EstimatorState & /*given_state_2*/,
     const double requested_time, EstimatorState & state_at_req_time) const;
 
   //!

diff --git a/include/robot_localization/ros_filter.hpp b/include/robot_localization/ros_filter.hpp
@@ -34,6 +34,7 @@
 #define ROBOT_LOCALIZATION__ROS_FILTER_HPP_
 
 #include <robot_localization/srv/set_pose.hpp>
+#include <robot_localization/srv/toggle_filter_processing.hpp>
 
 #include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
@@ -116,6 +117,19 @@ class RosFilter : public rclcpp::Node
   //!
   void reset();
 
+  //! @brief Service callback to toggle processing measurements for a standby
+  //! mode but continuing to publish
+  //! @param[in] request - The state requested, on (True) or off (False)
+  //! @param[out] response - status if upon success
+  //! @return boolean true if successful, false if not
+  //!
+  void toggleFilterProcessingCallback(
+    const std::shared_ptr<rmw_request_id_t>/*request_header*/,
+    const std::shared_ptr<
+      robot_localization::srv::ToggleFilterProcessing::Request> req,
+    const std::shared_ptr<
+      robot_localization::srv::ToggleFilterProcessing::Response> resp);
+
   //! @brief Callback method for receiving all acceleration (IMU) messages
   //! @param[in] msg - The ROS IMU message to take in.
   //! @param[in] callback_data - Relevant static callback data
@@ -301,6 +315,12 @@ class RosFilter : public rclcpp::Node
     const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg,
     const CallbackData & callback_data, const std::string & target_frame);
 
+  //! @brief Validates filter outputs for NaNs and Infinite values
+  //! @param[out] message - The standard ROS odometry message to be validated
+  //! @return true if the filter output is valid, false otherwise
+  //!
+  bool validateFilterOutput(const nav_msgs::msg::Odometry & message);
+
 protected:
   //! @brief Finds the latest filter state before the given timestamp and makes
   //! it the current state again.
@@ -329,6 +349,10 @@ class RosFilter : public rclcpp::Node
   //!
   void clearExpiredHistory(const rclcpp::Time cutoff_time);
 
+  //! @brief Clears measurement queue
+  //!
+  void clearMeasurementQueue();
+
   //! @brief Adds a diagnostic message to the accumulating map and updates the
   //! error level
   //! @param[in] error_level - The error level of the diagnostic
@@ -475,10 +499,9 @@ class RosFilter : public rclcpp::Node
   //!
   bool smooth_lagged_data_;
 
-  //! @brief Service that allows another node to enable the filter. Uses a
-  //! standard Empty service.
+  //! @brief Whether the filter should process new measurements or not.
   //!
-  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr enable_filter_srv_;
+  bool toggled_on_;
 
   //! @brief Whether or not we're in 2D mode
   //!
@@ -679,6 +702,13 @@ class RosFilter : public rclcpp::Node
   //!
   rclcpp::Duration tf_time_offset_;
 
+  //! @brief Service that allows another node to toggle on/off filter
+  //! processing while still publishing.
+  //! Uses a robot_localization ToggleFilterProcessing service.
+  //!
+  rclcpp::Service<robot_localization::srv::ToggleFilterProcessing>::SharedPtr
+    toggle_filter_processing_srv_;
+
   //! @brief Subscribes to the control input topic
   //!
   rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr control_sub_;
@@ -695,6 +725,11 @@ class RosFilter : public rclcpp::Node
   rclcpp::Service<robot_localization::srv::SetPose>::SharedPtr
     set_pose_service_;
 
+  //! @brief Service that allows another node to enable the filter. Uses a
+  //! standard Empty service.
+  //!
+  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr enable_filter_srv_;
+
   //! @brief Transform buffer for managing coordinate transforms
   //!
   std::unique_ptr<tf2_ros::Buffer> tf_buffer_;

diff --git a/src/robot_localization_estimator.cpp b/src/robot_localization_estimator.cpp
@@ -190,7 +190,7 @@ void RobotLocalizationEstimator::extrapolate(
 
 void RobotLocalizationEstimator::interpolate(
   const EstimatorState & given_state_1,
-  const EstimatorState & given_state_2,
+  const EstimatorState & /*given_state_2*/,
   const double requested_time,
   EstimatorState & state_at_req_time) const
 {