squashed mapping bugs

README.md

@@ -6,6 +6,20 @@
 Glider is a G-INS system built on [GTSAM](https://github.com/borglab/gtsam). It currently takes in GPS and 9-DOF IMU and provides a full
 state estimate up to the rate of you IMU. Glider is designed to be configured to your system. 
 
+## Building Glider
+Glider can be build with colcon as a ROS2 package with `colcon build --packages-select glider`. If you only want the api you can build with:
+```
+cmake -S . -B build -DBUILD_ROS=OFF
+cmake --build build 
+```
+
+## Running Glider
+To run glider with ros use `ros2 run glider glider-node.launch.py`. If you are running with a bag file use:
+```
+ros2 bag run <bag_file_name> --clock
+ros2 launch glider glider-node.launch.py use_sim_time:=true
+```
+
 ## Hardware Setup
 You're setup needs a GPS and a 9-DOF IMU, that is an IMU that provides a full orientation. The IMU orientation should be provided in the IMU's frame
 as this is standard for robotics, but we are working on supporting the NED frame. We use a VectorNav VN100T IMU. It is important make sure your IMU magnetometer is calibrated, if it is not aligned correctly the heading output of glider will be incorrect.
@@ -18,7 +32,7 @@ the parameters mean:
  - `publishers.viz.use`: if true will publish an `Odometry` topic for visualization centered around the origin.
  - `publishers.viz.origin_easting`: the easting value you want to viz odometry to center around.
  - `publishers.viz.origin_northing`: the northing value you want the viz odometry to center around.
- - `subscribers.use_odom`: Still under development
+ - `subscribers.use_odom`: Still under development.
 
 ## Glider Setup
 You can configure glider itself in `config/glider-params.yaml`, this is where you can specify the parameters for the factor graph. Here's more detail on each parameter:

glider/config/glider-params.yaml

@@ -14,7 +14,7 @@ frame:
   imu: "enu"
 logging:
   stdout: true
-  directory: "/home/jason/.ros/log/glider"
+  directory: "/tmp/glider"
 optimizer:
   smooth: true
   lag_time: 5.0

glider/include/ros/glider_node.hpp

@@ -71,19 +71,13 @@ class GliderNode : public rclcpp::Node
 
         // parameters
         bool initialized_;
-        bool use_sim_time_;
         bool publish_nsf_;
         bool viz_;
         std::string utm_zone_;
-        double declination_;
         double origin_easting_;
         double origin_northing_;
-        int gps_counter_;
-        int gps_init_count_;
 
         // tracker
         Glider::OdometryWithCovariance current_state_;
-
-        int64_t latest_imu_timestamp_;
 };
 }

glider/launch/glider-node.launch.py

@@ -7,37 +7,30 @@
 """
 
 import os
+import yaml
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
+from ament_index_python.packages import get_package_share_directory
 
 
 def generate_launch_description():
-    # create logging directory
-    os.makedirs("/home/jason/.ros/log/glider", exist_ok=True)
-
     # Declare launch arguments
     use_sim_time_arg = DeclareLaunchArgument(
         'use_sim_time',
         default_value='false',
         description='Use simulation time'
     )
 
-    use_odom_arg = DeclareLaunchArgument(
-        'use_odom',
-        default_value='false',
-        description='Use odometry'
-    )
-
     # Get launch configurations
     use_sim_time = LaunchConfiguration('use_sim_time')
-    use_odom = LaunchConfiguration('use_odom')
 
     # Find package share directory
     glider_share = FindPackageShare('glider')
-
+    glider_share_dir = get_package_share_directory('glider')
+
     # Path to parameter files
     ros_params_file = PathJoinSubstitution([
         glider_share,
@@ -50,7 +43,12 @@ def generate_launch_description():
         'config',
         'glider-params.yaml'
     ])
-
+
+    # create logging directory
+    with open(os.path.join(glider_share_dir, "config", "glider-params.yaml")) as f:
+        config = yaml.safe_load(f)
+    os.makedirs(config["logging"]["directory"], exist_ok=True)
+
     # Create the glider node
     glider_node = Node(
         package='glider',
@@ -61,7 +59,7 @@ def generate_launch_description():
             ros_params_file,
             {'path': graph_params_file,
              'use_sim_time': use_sim_time,
-             'use_odom': use_odom}
+             'use_odom': False}
         ],
         remappings=[
             ('/gps', '/ublox/fix'),
@@ -70,4 +68,4 @@ def generate_launch_description():
         ]
     )
 
-    return LaunchDescription([use_sim_time_arg, use_odom_arg, glider_node])
+    return LaunchDescription([use_sim_time_arg, glider_node])

glider/ros/glider_node.cpp

@@ -31,9 +31,6 @@ GliderNode::GliderNode(const rclcpp::NodeOptions& options) : rclcpp::Node("glide
     bool use_odom = this->get_parameter("subscribers.use_odom").as_bool();
 
     std::string path = this->get_parameter("path").as_string();
-    use_sim_time_ = this->get_clock()->get_clock_type() == RCL_ROS_TIME;
-
-    latest_imu_timestamp_ = 0;
 
     glider_ = std::make_unique<Glider::Glider>(path);
     current_state_ = Glider::OdometryWithCovariance::Uninitialized();
@@ -109,8 +106,6 @@ void GliderNode::imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg)
     int64_t timestamp = getTime(msg->header.stamp);
 
     glider_->addImu(timestamp, accel, gyro, orient);
-
-    latest_imu_timestamp_ = timestamp;
 }
 
 void GliderNode::gpsCallback(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg)

glider/src/glider.cpp

@@ -25,14 +25,15 @@ Glider::Glider(const std::string& path)
 
     LOG(INFO) << "[GLIDER] Using IMU frame: " << frame_;
     LOG(INFO) << "[GLIDER] Using Fixed Lag Smoother: " << std::boolalpha << params.smooth;
+    LOG(INFO) << "[GLIDER] Logging to: " << params.log_dir;    
     LOG(INFO) << "[GLIDER] Glider initialized";
 }
 
 void Glider::initializeLogging(const Parameters& params) const
 {
     // initialize GLog
     google::InitGoogleLogging("Glider");
-    // TODO fix hard coding
+
     FLAGS_log_dir = params.log_dir;
     if (params.log) FLAGS_alsologtostderr = 1;
 }