diff --git a/README.md b/README.md index 4a515a5..abbc220 100644 --- a/README.md +++ b/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 --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: diff --git a/glider/config/glider-params.yaml b/glider/config/glider-params.yaml index b762568..c43b931 100644 --- a/glider/config/glider-params.yaml +++ b/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 diff --git a/glider/include/ros/glider_node.hpp b/glider/include/ros/glider_node.hpp index b15fcac..5feda06 100644 --- a/glider/include/ros/glider_node.hpp +++ b/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_; }; } diff --git a/glider/launch/glider-node.launch.py b/glider/launch/glider-node.launch.py index 42ba77a..e03de5e 100644 --- a/glider/launch/glider-node.launch.py +++ b/glider/launch/glider-node.launch.py @@ -7,17 +7,16 @@ """ 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', @@ -25,19 +24,13 @@ def generate_launch_description(): 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]) diff --git a/glider/ros/glider_node.cpp b/glider/ros/glider_node.cpp index 7c79497..59f13ce 100644 --- a/glider/ros/glider_node.cpp +++ b/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(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) diff --git a/glider/src/glider.cpp b/glider/src/glider.cpp index e19cc1d..bceb720 100644 --- a/glider/src/glider.cpp +++ b/glider/src/glider.cpp @@ -25,6 +25,7 @@ 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"; } @@ -32,7 +33,7 @@ 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; }