Lint

CMakeLists.txt

@@ -183,7 +183,7 @@ ament_target_dependencies(libsurvive_ros2_poser_node rclcpp)
 
 # Install python packages
 
-ExternalProject_Get_Property(gtsam binary_dir)
+externalproject_get_property(gtsam binary_dir)
 install(
   DIRECTORY
     ${binary_dir}/python/gtsam
@@ -205,7 +205,7 @@ install(
 install(
   TARGETS
     libsurvive_ros2_driver_node
-    libsurvive_ros2_poser_node 
+    libsurvive_ros2_poser_node
   DESTINATION lib/${PROJECT_NAME})
 
 # Install helper programs
@@ -236,16 +236,12 @@ if(BUILD_TESTING)
 
   # Lint tests
   ament_lint_auto_find_test_dependencies()
-  
+
   # Unit tests
   ament_add_ros_isolated_pytest_test(test_config_tools
       test/test_config_tools.py
     TIMEOUT 120)
 
-  # Integration tests
-  ament_add_ros_isolated_pytest_test(test_metaposer
-      test/test_metaposer.py
-    TIMEOUT 120)
 endif()
 
 # EXPORTS

Dockerfile

@@ -40,6 +40,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends
         libsciplot-dev                                                          \
         libusb-1.0-0-dev                                                        \
         libx11-dev                                                              \
+        libyaml-cpp-dev                                                         \
         sudo                                                                    \
         valgrind                                                                \
         zlib1g-dev                                                              \

docker-compose.yml

@@ -33,13 +33,10 @@ services:
     volumes:
       - /dev:/dev
       - .:/home/ubuntu/ros2_ws/src/libsurvive_ros2
-      - ~/data:/home/ubuntu/.ros
+      - ./data:/home/ubuntu/.ros
     build: .
     ports:
       - "8765:8765"
     working_dir: /home/ubuntu/ros2_ws
     entrypoint: /home/ubuntu/ros2_ws/entrypoint.sh
-    stdin_open: true
-    tty: true 
-    command: /bin/bash
-
+    command: ros2 launch libsurvive_ros2 libsurvive_ros2.launch.py

include/libsurvive_ros2/driver_component.hpp

@@ -80,15 +80,17 @@ class DriverComponent : public rclcpp::Node
   SurviveContext * ctx_;
   std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
   std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_static_broadcaster_;
-  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr tracker_pose_publisher_;
-  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr lighthouse_pose_publisher_;
+  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
+    tracker_pose_publisher_;
+  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
+    lighthouse_pose_publisher_;
   rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher_;
   rclcpp::Publisher<sensor_msgs::msg::Joy>::SharedPtr button_publisher_;
   rclcpp::Publisher<libsurvive_ros2::msg::Angle>::SharedPtr angle_publisher_;
   rclcpp::Publisher<libsurvive_ros2::msg::Lighthouse>::SharedPtr lighthouse_publisher_;
   rclcpp::Publisher<libsurvive_ros2::msg::Tracker>::SharedPtr tracker_publisher_;
-  std::unordered_map<std::string,libsurvive_ros2::msg::Lighthouse> lighthouses_;
-  std::unordered_map<std::string,libsurvive_ros2::msg::Tracker> trackers_;
+  std::unordered_map<std::string, libsurvive_ros2::msg::Lighthouse> lighthouses_;
+  std::unordered_map<std::string, libsurvive_ros2::msg::Tracker> trackers_;
   rclcpp::TimerBase::SharedPtr timer_;
   std::thread worker_thread_;
   rclcpp::Time last_base_station_update_;

include/libsurvive_ros2/poser_component.hpp

@@ -57,18 +57,17 @@
 #include <optional>
 #include <string>
 #include <unordered_map>
+#include <utility>
 
-// GTSAM includes
-#include <gtsam/inference/Symbol.h>
-#include <gtsam/navigation/ImuBias.h>
-#include <gtsam/navigation/ImuFactor.h>
-#include <gtsam/nonlinear/ISAM2.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/PriorFactor.h>
-#include <gtsam/slam/BetweenFactor.h>
-
-// Message abstractions
+// Project includes
 #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "gtsam/inference/Symbol.h"
+#include "gtsam/navigation/ImuBias.h"
+#include "gtsam/navigation/ImuFactor.h"
+#include "gtsam/nonlinear/ISAM2.h"
+#include "gtsam/nonlinear/NonlinearFactorGraph.h"
+#include "gtsam/nonlinear/PriorFactor.h"
+#include "gtsam/slam/BetweenFactor.h"
 #include "libsurvive_ros2/msg/angle.hpp"
 #include "libsurvive_ros2/msg/lighthouse.hpp"
 #include "libsurvive_ros2/msg/tracker.hpp"
@@ -83,8 +82,8 @@ namespace libsurvive_ros2
 typedef int64_t Timestamp;
 
 // Encodes body information
-struct BodyInfo {
-
+struct BodyInfo
+{
   // Sequence of poses of the body in the global frame
   std::map<Timestamp, gtsam::Key> gTb;
 
@@ -93,12 +92,11 @@ struct BodyInfo {
 
   // Tracker head to body transforms for all sensors
   std::unordered_map<std::string, gtsam::Pose3> bTh;
-
 };
 
 // Encodes tracker information
-struct TrackerInfo {
-  
+struct TrackerInfo
+{
   // Associated body -- we don't store a pointer here because there are no
   // guarantees about how a std::unordered_map organizes its memory. It's
   // cheap to look up on-demand, though.
@@ -110,35 +108,34 @@ struct TrackerInfo {
 
   // Sensor <locations, normals> in the tracking frame.
   std::unordered_map<uint8_t, std::pair<gtsam::Point3, gtsam::Point3>> t_sensors;
-  
+
   // Constant scale and bias factors for the IMU, in the imu frame.
   gtsam::Vector3 accel_scale;
   gtsam::Vector3 accel_bias;
   gtsam::Vector3 omega_scale;
   gtsam::Vector3 omega_bias;
-  
+
   // IMU pre-integrator
   std::shared_ptr<gtsam::PreintegratedImuMeasurements> preintegrator;
 
   // Time varying IMU accelerometer and gyroscope bias in the body frame.
   std::map<Timestamp, gtsam::Key> b_B;
-
 };
 
 // Encodes lighthouse information
-struct LighthouseInfo {
-  
+struct LighthouseInfo
+{
   // Static pose of this lighthouse
   gtsam::Key gTl;
-
 };
 
 // Helper to find the closest key lower that the supplied value. This is used to map
 // pose corrections to their nearest IMU timestamp.
 std::optional<gtsam::Key>
-find_key_for_closest_stamp(std::map<Timestamp, gtsam::Key> & m, Timestamp t) {
+find_key_for_closest_stamp(std::map<Timestamp, gtsam::Key> & m, Timestamp t)
+{
   std::map<Timestamp, gtsam::Key>::iterator it = m.lower_bound(t);
-  it = it != m.begin() ? --it :  m.end();
+  it = it != m.begin() ? --it : m.end();
   if (it != m.end()) {
     return it->second;
   }
@@ -148,7 +145,7 @@ find_key_for_closest_stamp(std::map<Timestamp, gtsam::Key> & m, Timestamp t) {
 class PoserComponent : public rclcpp::Node
 {
 public:
-  // Create a new metaposer class to track 
+  // Create a new metaposer class to track
   explicit PoserComponent(const rclcpp::NodeOptions & options);
   virtual ~PoserComponent();
 
@@ -161,7 +158,7 @@ class PoserComponent : public rclcpp::Node
 
   // Estimates from the low-level driver, which we treat as priors
   void add_tracker_pose(const geometry_msgs::msg::PoseWithCovarianceStamped & msg);
-  void add_lighthouse_pose(const  geometry_msgs::msg::PoseWithCovarianceStamped & msg);
+  void add_lighthouse_pose(const geometry_msgs::msg::PoseWithCovarianceStamped & msg);
 
   // Calculate and publish a solution
   void solution_callback();
@@ -195,9 +192,11 @@ class PoserComponent : public rclcpp::Node
   rclcpp::Subscription<libsurvive_ros2::msg::Lighthouse>::SharedPtr lighthouse_subscriber_;
   rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_subscriber_;
   rclcpp::Subscription<libsurvive_ros2::msg::Angle>::SharedPtr angle_subscriber_;
-  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr tracker_pose_subscriber_;
-  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr lighthouse_pose_subscriber_;
-
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
+    tracker_pose_subscriber_;
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
+    lighthouse_pose_subscriber_;
+
   // Solution publisher
   rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr body_pose_publisher_;