Linting and header cleanup, part 1 (#752)

* Header cleanup and logging fixes

Author: ayrton04

include/robot_localization/ekf.hpp

@@ -30,12 +30,12 @@
  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
-
 #ifndef ROBOT_LOCALIZATION__EKF_HPP_
 #define ROBOT_LOCALIZATION__EKF_HPP_
 
-#include <robot_localization/filter_base.hpp>
-#include <vector>
+#include "rclcpp/time.hpp"
+#include "robot_localization/filter_base.hpp"
+#include "robot_localization/measurement.hpp"
 
 namespace robot_localization
 {

include/robot_localization/filter_base.hpp

@@ -30,21 +30,15 @@
  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
-
 #ifndef ROBOT_LOCALIZATION__FILTER_BASE_HPP_
 #define ROBOT_LOCALIZATION__FILTER_BASE_HPP_
 
-#include <robot_localization/filter_common.hpp>
-#include <robot_localization/filter_utilities.hpp>
-#include <robot_localization/measurement.hpp>
-#include <robot_localization/filter_state.hpp>
-
-#include <algorithm>
-#include <memory>
 #include <ostream>
-#include <string>
 #include <vector>
-#include <limits>
+
+#include "Eigen/Dense"
+#include "rclcpp/time.hpp"
+#include "robot_localization/measurement.hpp"
 
 namespace robot_localization
 {

include/robot_localization/filter_common.hpp

@@ -29,7 +29,6 @@
  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
-
 #ifndef ROBOT_LOCALIZATION__FILTER_COMMON_HPP_
 #define ROBOT_LOCALIZATION__FILTER_COMMON_HPP_
 

include/robot_localization/filter_state.hpp

@@ -31,13 +31,12 @@
  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
-
 #ifndef ROBOT_LOCALIZATION__FILTER_STATE_HPP_
 #define ROBOT_LOCALIZATION__FILTER_STATE_HPP_
 
 #include <memory>
+
 #include "Eigen/Dense"
-#include "rclcpp/duration.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/time.hpp"
 

include/robot_localization/filter_utilities.hpp

@@ -29,20 +29,18 @@
  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
-
 #ifndef ROBOT_LOCALIZATION__FILTER_UTILITIES_HPP_
 #define ROBOT_LOCALIZATION__FILTER_UTILITIES_HPP_
 
-#include <Eigen/Dense>
-#include <rclcpp/duration.hpp>
-#include <rclcpp/time.hpp>
-#include <std_msgs/msg/header.hpp>
-
 #include <iostream>
 #include <ostream>
 #include <string>
 #include <vector>
 
+#include "Eigen/Dense"
+#include "rclcpp/time.hpp"
+#include "std_msgs/msg/header.hpp"
+
 #define FB_DEBUG(msg) \
   if (getDebug()) { \
     *debug_stream_ << msg; \
@@ -64,20 +62,23 @@ namespace filter_utilities
  * @param[in] tf_prefix - the tf2 prefix to append
  * @param[in, out] frame_id - the resulting frame_id value
  */
-inline void appendPrefix(std::string tf_prefix, std::string & frame_id)
+inline void appendPrefix(const std::string & tf_prefix, std::string & frame_id)
 {
+  size_t frame_id_prefix_index = 0u;
+  size_t tf_prefix_index = 0u;
+
   // Strip all leading slashes for tf2 compliance
-  if (!frame_id.empty() && frame_id.at(0) == '/') {
-    frame_id = frame_id.substr(1);
+  if (!frame_id.empty() && frame_id.at(0u) == '/') {
+    frame_id_prefix_index = 1u;
   }
 
-  if (!tf_prefix.empty() && tf_prefix.at(0) == '/') {
-    tf_prefix = tf_prefix.substr(1);
+  if (!tf_prefix.empty() && tf_prefix.at(0u) == '/') {
+    tf_prefix_index = 1u;
   }
 
   // If we do have a tf prefix, then put a slash in between
   if (!tf_prefix.empty()) {
-    frame_id = tf_prefix + "/" + frame_id;
+    frame_id = tf_prefix.substr(tf_prefix_index) + "/" + frame_id.substr(frame_id_prefix_index);
   }
 }
 

include/robot_localization/measurement.hpp

@@ -31,19 +31,17 @@
  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
-
 #ifndef ROBOT_LOCALIZATION__MEASUREMENT_HPP_
 #define ROBOT_LOCALIZATION__MEASUREMENT_HPP_
 
-#include <Eigen/Dense>
-#include <rclcpp/duration.hpp>
-#include <rclcpp/macros.hpp>
-#include <rclcpp/time.hpp>
-
 #include <limits>
+#include <memory>
 #include <string>
 #include <vector>
-#include <memory>
+
+#include "Eigen/Dense"
+#include "rclcpp/time.hpp"
+#include "robot_localization/measurement.hpp"
 
 namespace robot_localization
 {

include/robot_localization/navsat_conversions.hpp

@@ -36,7 +36,6 @@
 * https://github.com/austin-robot/utexas-art-ros-pkg/blob/afd147a1eb944fc3dbd138574c39699813f797bf/stacks/art_vehicle/art_common/include/art/UTM.h
 * https://github.com/austin-robot/utexas-art-ros-pkg/blob/afd147a1eb944fc3dbd138574c39699813f797bf/stacks/art_vehicle/art_common/include/art/conversions.h
 */
-
 #ifndef ROBOT_LOCALIZATION__NAVSAT_CONVERSIONS_HPP_
 #define ROBOT_LOCALIZATION__NAVSAT_CONVERSIONS_HPP_
 

include/robot_localization/navsat_transform.hpp

@@ -29,29 +29,29 @@
  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
-
 #ifndef ROBOT_LOCALIZATION__NAVSAT_TRANSFORM_HPP_
 #define ROBOT_LOCALIZATION__NAVSAT_TRANSFORM_HPP_
 
-#include <robot_localization/srv/set_datum.hpp>
-#include <robot_localization/srv/to_ll.hpp>
-#include <robot_localization/srv/from_ll.hpp>
-
-#include <Eigen/Dense>
-#include <GeographicLib/Geocentric.hpp>
-#include <GeographicLib/LocalCartesian.hpp>
-#include <nav_msgs/msg/odometry.hpp>
-#include <rclcpp/rclcpp.hpp>
-#include <sensor_msgs/msg/imu.hpp>
-#include <sensor_msgs/msg/nav_sat_fix.hpp>
-#include <tf2/LinearMath/Transform.h>
-#include <tf2_ros/buffer.h>
-#include <tf2_ros/static_transform_broadcaster.h>
-#include <tf2_ros/transform_listener.h>
-
 #include <memory>
 #include <string>
 
+#include "Eigen/Dense"
+#include "GeographicLib/LocalCartesian.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/timer.hpp"
+#include "robot_localization/srv/from_ll.hpp"
+#include "robot_localization/srv/set_datum.hpp"
+#include "robot_localization/srv/to_ll.hpp"
+#include "sensor_msgs/msg/imu.hpp"
+#include "sensor_msgs/msg/nav_sat_fix.hpp"
+#include "tf2/LinearMath/Quaternion.h"
+#include "tf2/LinearMath/Transform.h"
+#include "tf2/LinearMath/Vector3.h"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/static_transform_broadcaster.h"
+#include "tf2_ros/transform_listener.h"
+
 namespace robot_localization
 {
 

include/robot_localization/robot_localization_estimator.hpp

@@ -29,19 +29,17 @@
  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
-
 #ifndef ROBOT_LOCALIZATION__ROBOT_LOCALIZATION_ESTIMATOR_HPP_
 #define ROBOT_LOCALIZATION__ROBOT_LOCALIZATION_ESTIMATOR_HPP_
 
-#include <boost/circular_buffer.hpp>
-#include <Eigen/Dense>
-#include <iostream>
+#include <ostream>
 #include <memory>
 #include <vector>
 
+#include "boost/circular_buffer.hpp"
+#include "Eigen/Dense"
 #include "robot_localization/filter_base.hpp"
 #include "robot_localization/filter_common.hpp"
-#include "robot_localization/filter_utilities.hpp"
 
 namespace robot_localization
 {

include/robot_localization/ros_filter.hpp

@@ -29,45 +29,40 @@
  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
-
 #ifndef ROBOT_LOCALIZATION__ROS_FILTER_HPP_
 #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>
-#include <geometry_msgs/msg/transform_stamped.hpp>
-#include <geometry_msgs/msg/twist.hpp>
-#include <geometry_msgs/msg/twist_stamped.hpp>
-#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
-#include <nav_msgs/msg/odometry.hpp>
-#include <rclcpp/rclcpp.hpp>
-#include <sensor_msgs/msg/imu.hpp>
-#include <std_msgs/msg/string.hpp>
-#include <std_srvs/srv/empty.hpp>
-#include <tf2/LinearMath/Transform.h>
-#include <tf2_ros/transform_broadcaster.h>
-#include <tf2_ros/transform_listener.h>
-#include <diagnostic_msgs/msg/diagnostic_status.hpp>
-#include <diagnostic_updater/diagnostic_updater.hpp>
-#include <diagnostic_updater/publisher.hpp>
-#include <robot_localization/filter_base.hpp>
-#include <robot_localization/filter_common.hpp>
-#include <robot_localization/ros_filter_utilities.hpp>
-
-#include <Eigen/Dense>
-
 #include <deque>
 #include <fstream>
 #include <map>
-#include <numeric>
+#include <memory>
 #include <queue>
 #include <string>
-#include <memory>
 #include <vector>
 
+#include "diagnostic_msgs/msg/diagnostic_status.hpp"
+#include "diagnostic_updater/diagnostic_updater.hpp"
+#include "diagnostic_updater/publisher.hpp"
+#include "Eigen/Dense"
+#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
+#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "geometry_msgs/msg/twist_with_covariance_stamped.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "robot_localization/filter_state.hpp"
+#include "robot_localization/measurement.hpp"
+#include "robot_localization/srv/toggle_filter_processing.hpp"
+#include "robot_localization/srv/set_pose.hpp"
+#include "sensor_msgs/msg/imu.hpp"
+#include "std_srvs/srv/empty.hpp"
+#include "tf2/LinearMath/Transform.h"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_broadcaster.h"
+#include "tf2_ros/transform_listener.h"
+
 namespace robot_localization
 {