adding boost dep (#579)

* adding boost dep

* bump 3.1.1

* adding more destinations

* fix deprecated interfaces

* fixes

* sep exe and lib

* dest change lib

* uncrustify fixes

* adding rclcpp macro definition

Author: SteveMacenski

CMakeLists.txt

@@ -29,6 +29,7 @@ find_package(tf2_eigen REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(Eigen3 REQUIRED)
+find_package(Boost REQUIRED)
 
 find_package(PkgConfig REQUIRED)
 pkg_check_modules(YAML_CPP yaml-cpp)
@@ -211,7 +212,7 @@ if(BUILD_TESTING)
   target_link_libraries(test_ukf_localization_node_interfaces ${library_name})
   rosidl_target_interfaces(test_ukf_localization_node_interfaces
     ${PROJECT_NAME} "rosidl_typesupport_cpp")
-  add_dependencies(test_ukf_localization_node_interfaces ekf_node)
+  add_dependencies(test_ukf_localization_node_interfaces ukf_node)
   ament_add_test(test_ukf_localization_node_interfaces
     GENERATE_RESULT_FOR_RETURN_CODE_ZERO
     TIMEOUT 300
@@ -307,8 +308,13 @@ install(
   ekf_node
   ukf_node
   robot_localization_listener_node
-  ${library_name}
-  DESTINATION lib/${PROJECT_NAME}
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+install(TARGETS ${library_name}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
 )
 
 # Install params config files.

include/robot_localization/filter_base.hpp

@@ -46,6 +46,8 @@
 #include <vector>
 #include <limits>
 
+#include "rclcpp/macros.hpp"
+
 namespace robot_localization
 {
 

include/robot_localization/navsat_conversions.hpp

@@ -259,7 +259,8 @@ static inline void LLtoUTM(
   char zone_buf[] = {0, 0, 0, 0};
   // We &0x3fU to let GCC know the size of ZoneNumber. In this case, it's under
   // 63 (6bits)
-  snprintf(zone_buf, sizeof(zone_buf), "%d%c", ZoneNumber & 0x3fU,
+  snprintf(
+    zone_buf, sizeof(zone_buf), "%d%c", ZoneNumber & 0x3fU,
     UTMLetterDesignator(Lat));
   UTMZone = std::string(zone_buf);
 

launch/ekf.launch.py

@@ -27,8 +27,8 @@ def generate_launch_description():
     return LaunchDescription([
         launch_ros.actions.Node(
             package='robot_localization',
-            node_executable='ekf_node',
-            node_name='ekf_filter_node',
+            executable='ekf_node',
+            name='ekf_filter_node',
             output='screen',
             parameters=[os.path.join(get_package_share_directory("robot_localization"), 'params', 'ekf.yaml')],
            ),

launch/navsat_transform.launch.py

@@ -26,8 +26,8 @@ def generate_launch_description():
     return LaunchDescription([
         launch_ros.actions.Node(
             package='robot_localization',
-            node_executable='navsat_transform_node',
-            node_name='navsat_transform_node',
+            executable='navsat_transform_node',
+            name='navsat_transform_node',
             output='screen',
             parameters=[os.path.join(get_package_share_directory("robot_localization"), 'params', 'navsat_transform.yaml')],
            ),

launch/ukf.launch.py

@@ -27,8 +27,8 @@ def generate_launch_description():
     return LaunchDescription([
         launch_ros.actions.Node(
             package='robot_localization',
-            node_executable='ukf_node',
-            node_name='ukf_filter_node',
+            executable='ukf_node',
+            name='ukf_filter_node',
             output='screen',
             parameters=[os.path.join(get_package_share_directory("robot_localization"), 'params', 'ukf.yaml')],
            ),

package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>robot_localization</name>
-  <version>3.1.0</version>
+  <version>3.1.1</version>
   <description>Provides nonlinear state estimation through sensor fusion of an abritrary number of sensors.</description>
 
   <author email="[email protected]">Tom Moore</author> 
@@ -33,6 +33,7 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>yaml-cpp</depend>
+  <depend>boost</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>builtin_interfaces</test_depend>

src/ekf.cpp

@@ -47,21 +47,22 @@ Ekf::~Ekf() {}
 
 void Ekf::correct(const Measurement & measurement)
 {
-  FB_DEBUG("---------------------- Ekf::correct ----------------------\n" <<
-    "State is:\n" <<
-    state_ <<
-    "\n"
-    "Topic is:\n" <<
-    measurement.topic_name_ <<
-    "\n"
-    "Measurement is:\n" <<
-    measurement.measurement_ <<
-    "\n"
-    "Measurement topic name is:\n" <<
-    measurement.topic_name_ <<
-    "\n\n"
-    "Measurement covariance is:\n" <<
-    measurement.covariance_ << "\n");
+  FB_DEBUG(
+    "---------------------- Ekf::correct ----------------------\n" <<
+      "State is:\n" <<
+      state_ <<
+      "\n"
+      "Topic is:\n" <<
+      measurement.topic_name_ <<
+      "\n"
+      "Measurement is:\n" <<
+      measurement.measurement_ <<
+      "\n"
+      "Measurement topic name is:\n" <<
+      measurement.topic_name_ <<
+      "\n\n"
+      "Measurement covariance is:\n" <<
+      measurement.covariance_ << "\n");
 
   // We don't want to update everything, so we need to build matrices that only
   // update the measured parts of our state vector. Throughout prediction and
@@ -73,11 +74,13 @@ void Ekf::correct(const Measurement & measurement)
     if (measurement.update_vector_[i]) {
       // Handle nan and inf values in measurements
       if (std::isnan(measurement.measurement_(i))) {
-        FB_DEBUG("Value at index " << i <<
-          " was nan. Excluding from update.\n");
+        FB_DEBUG(
+          "Value at index " << i <<
+            " was nan. Excluding from update.\n");
       } else if (std::isinf(measurement.measurement_(i))) {
-        FB_DEBUG("Value at index " << i <<
-          " was inf. Excluding from update.\n");
+        FB_DEBUG(
+          "Value at index " << i <<
+            " was inf. Excluding from update.\n");
       } else {
         update_indices.push_back(i);
       }
@@ -117,10 +120,11 @@ void Ekf::correct(const Measurement & measurement)
     // than exclude the measurement or make up a covariance, just take
     // the absolute value.
     if (measurement_covariance_subset(i, i) < 0) {
-      FB_DEBUG("WARNING: Negative covariance for index " <<
-        i << " of measurement (value is" <<
-        measurement_covariance_subset(i, i) <<
-        "). Using absolute value...\n");
+      FB_DEBUG(
+        "WARNING: Negative covariance for index " <<
+          i << " of measurement (value is" <<
+          measurement_covariance_subset(i, i) <<
+          "). Using absolute value...\n");
 
       measurement_covariance_subset(i, i) =
         ::fabs(measurement_covariance_subset(i, i));
@@ -133,9 +137,10 @@ void Ekf::correct(const Measurement & measurement)
     // measurement can be completely without error, so add a small
     // amount in that case.
     if (measurement_covariance_subset(i, i) < 1e-9) {
-      FB_DEBUG("WARNING: measurement had very small error covariance for index " <<
-        update_indices[i] <<
-        ". Adding some noise to maintain filter stability.\n");
+      FB_DEBUG(
+        "WARNING: measurement had very small error covariance for index " <<
+          update_indices[i] <<
+          ". Adding some noise to maintain filter stability.\n");
 
       measurement_covariance_subset(i, i) = 1e-9;
     }
@@ -148,12 +153,13 @@ void Ekf::correct(const Measurement & measurement)
     state_to_measurement_subset(i, update_indices[i]) = 1;
   }
 
-  FB_DEBUG("Current state subset is:\n" <<
-    state_subset << "\nMeasurement subset is:\n" <<
-    measurement_subset << "\nMeasurement covariance subset is:\n" <<
-    measurement_covariance_subset <<
-    "\nState-to-measurement subset is:\n" <<
-    state_to_measurement_subset << "\n");
+  FB_DEBUG(
+    "Current state subset is:\n" <<
+      state_subset << "\nMeasurement subset is:\n" <<
+      measurement_subset << "\nMeasurement covariance subset is:\n" <<
+      measurement_covariance_subset <<
+      "\nState-to-measurement subset is:\n" <<
+      state_to_measurement_subset << "\n");
 
   // (1) Compute the Kalman gain: K = (PH') / (HPH' + R)
   Eigen::MatrixXd pht =
@@ -182,8 +188,9 @@ void Ekf::correct(const Measurement & measurement)
   }
 
   // (2) Check Mahalanobis distance between mapped measurement and state.
-  if (checkMahalanobisThreshold(innovation_subset, hphr_inverse,
-    measurement.mahalanobis_thresh_))
+  if (checkMahalanobisThreshold(
+      innovation_subset, hphr_inverse,
+      measurement.mahalanobis_thresh_))
   {
     // (3) Apply the gain to the difference between the state and measurement: x
     // = x + K(z - Hx)
@@ -218,9 +225,10 @@ void Ekf::predict(
 {
   const double delta_sec = filter_utilities::toSec(delta);
 
-  FB_DEBUG("---------------------- Ekf::predict ----------------------\n" <<
-    "delta is " << filter_utilities::toSec(delta) << "\n" <<
-    "state is " << state_ << "\n");
+  FB_DEBUG(
+    "---------------------- Ekf::predict ----------------------\n" <<
+      "delta is " << filter_utilities::toSec(delta) << "\n" <<
+      "state is " << state_ << "\n");
 
   double roll = state_(StateMemberRoll);
   double pitch = state_(StateMemberPitch);
@@ -377,11 +385,12 @@ void Ekf::predict(
   transfer_function_jacobian_(StateMemberYaw, StateMemberRoll) = dFY_dR;
   transfer_function_jacobian_(StateMemberYaw, StateMemberPitch) = dFY_dP;
 
-  FB_DEBUG("Transfer function is:\n" <<
-    transfer_function_ << "\nTransfer function Jacobian is:\n" <<
-    transfer_function_jacobian_ << "\nProcess noise covariance is:\n" <<
-    process_noise_covariance_ << "\nCurrent state is:\n" <<
-    state_ << "\n");
+  FB_DEBUG(
+    "Transfer function is:\n" <<
+      transfer_function_ << "\nTransfer function Jacobian is:\n" <<
+      transfer_function_jacobian_ << "\nProcess noise covariance is:\n" <<
+      process_noise_covariance_ << "\nCurrent state is:\n" <<
+      state_ << "\n");
 
   Eigen::MatrixXd * process_noise_covariance = &process_noise_covariance_;
 
@@ -414,9 +423,10 @@ void Ekf::predict(
   // Handle wrapping
   wrapStateAngles();
 
-  FB_DEBUG("Predicted state is:\n" <<
-    state_ << "\nCurrent estimate error covariance is:\n" <<
-    estimate_error_covariance_ << "\n");
+  FB_DEBUG(
+    "Predicted state is:\n" <<
+      state_ << "\nCurrent estimate error covariance is:\n" <<
+      estimate_error_covariance_ << "\n");
 
   // (3) Project the error forward: P = J * P * J' + Q
   estimate_error_covariance_ =

src/filter_base.cpp

@@ -147,8 +147,9 @@ void FilterBase::computeDynamicProcessNoiseCovariance(
   dynamic_process_noise_covariance_.block<TWIST_SIZE, TWIST_SIZE>(
     POSITION_OFFSET, POSITION_OFFSET) =
     velocity_matrix *
-    process_noise_covariance_.block<TWIST_SIZE, TWIST_SIZE>(POSITION_OFFSET,
-      POSITION_OFFSET) *
+    process_noise_covariance_.block<TWIST_SIZE, TWIST_SIZE>(
+    POSITION_OFFSET,
+    POSITION_OFFSET) *
     velocity_matrix.transpose();
 }
 
@@ -192,8 +193,9 @@ const Eigen::VectorXd & FilterBase::getState() {return state_;}
 
 void FilterBase::processMeasurement(const Measurement & measurement)
 {
-  FB_DEBUG("------ FilterBase::processMeasurement (" << measurement.topic_name_ <<
-    ") ------\n");
+  FB_DEBUG(
+    "------ FilterBase::processMeasurement (" << measurement.topic_name_ <<
+      ") ------\n");
 
   rclcpp::Duration delta(0);
 
@@ -256,8 +258,9 @@ void FilterBase::processMeasurement(const Measurement & measurement)
     last_measurement_time_ = measurement.time_;
   }
 
-  FB_DEBUG("------ /FilterBase::processMeasurement (" << measurement.topic_name_ <<
-    ") ------\n");
+  FB_DEBUG(
+    "------ /FilterBase::processMeasurement (" << measurement.topic_name_ <<
+      ") ------\n");
 }
 
 void FilterBase::setControl(
@@ -357,10 +360,11 @@ void FilterBase::prepareControl(
       (reference_time - latest_control_time_ >= control_timeout_);
 
     if (timed_out) {
-      FB_DEBUG("Control timed out. Reference time was " <<
-        reference_time.nanoseconds() << ", latest control time was " <<
-        latest_control_time_.nanoseconds() << ", control timeout was " <<
-        control_timeout_.nanoseconds() << "\n");
+      FB_DEBUG(
+        "Control timed out. Reference time was " <<
+          reference_time.nanoseconds() << ", latest control time was " <<
+          latest_control_time_.nanoseconds() << ", control timeout was " <<
+          control_timeout_.nanoseconds() << "\n");
     }
 
     for (size_t controlInd = 0; controlInd < TWIST_SIZE; ++controlInd) {
@@ -399,16 +403,17 @@ inline double FilterBase::computeControlAcceleration(
 
   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");
+  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;
 }