added gps based position correction

CMakeLists.txt

@@ -49,6 +49,7 @@ add_library(${LIBRARY_TARGET_NAME}
   ${PROJECT_SOURCE_DIR}/src/filter/base_correction.cpp
   ${PROJECT_SOURCE_DIR}/src/filter/inekf/correction/legged_kinematics_correction.cpp
   ${PROJECT_SOURCE_DIR}/src/filter/inekf/correction/velocity_correction.cpp
+  ${PROJECT_SOURCE_DIR}/src/filter/inekf/correction/position_correction.cpp
   ${PROJECT_SOURCE_DIR}/src/filter/inekf/propagation/imu_propagation.cpp
   ${PROJECT_SOURCE_DIR}/src/filter/inekf/inekf.cpp
   ${PROJECT_SOURCE_DIR}/src/imu_filter/imu_ang_vel_ekf.cpp
@@ -66,8 +67,8 @@ target_include_directories(${LIBRARY_TARGET_NAME} PUBLIC
 # Build Examples                 #
 # #################################################
 # if(BUILD_EXAMPLES)
-#   message("Building examples...")
-#   add_subdirectory(examples)
+# message("Building examples...")
+# add_subdirectory(examples)
 # endif(BUILD_EXAMPLES)
 
 # #################################################

README.md

@@ -106,6 +106,12 @@ rosrun drift mini_cheetah
 rosrun drift girona500
 ```
 
+**WAMV (Surface vehicle):**
+* With GPS-based position correction:
+```
+rosrun drift wamv_gps
+```
+
 ## Run the repo with your own robots:
 Please refer to the tutorial here: https://umich-curly.github.io/DRIFT_Website/tutorials/.
 
@@ -137,4 +143,4 @@ If you find this work useful, please kindly cite the following papers
 ```
 
 # License
-DRIFT is released under a [BSD 3-Clause License](https://github.com/UMich-CURLY/drift/blob/main/LICENSE). 
+DRIFT is released under a [BSD 3-Clause License](https://github.com/UMich-CURLY/drift/blob/main/LICENSE). 

ROS/drift/CMakeLists.txt

@@ -67,6 +67,7 @@ rosbuild_add_library(drift
   ${PROJECT_SOURCE_DIR}/../../src/filter/base_correction.cpp
   ${PROJECT_SOURCE_DIR}/../../src/filter/inekf/correction/legged_kinematics_correction.cpp
   ${PROJECT_SOURCE_DIR}/../../src/filter/inekf/correction/velocity_correction.cpp
+  ${PROJECT_SOURCE_DIR}/../../src/filter/inekf/correction/position_correction.cpp
   ${PROJECT_SOURCE_DIR}/../../src/filter/inekf/propagation/imu_propagation.cpp
   ${PROJECT_SOURCE_DIR}/../../src/filter/inekf/inekf.cpp
   ${PROJECT_SOURCE_DIR}/../../src/imu_filter/imu_ang_vel_ekf.cpp
@@ -83,13 +84,12 @@ rosbuild_add_library(ros_communication_lib
   ${ros_publisher}
 )
 
-## baselines
+# # baselines
 include_directories(
   ${PROJECT_SOURCE_DIR}/../../baselines/include)
 rosbuild_add_library(baselines
   ${PROJECT_SOURCE_DIR}/../../baselines/src/wheel_odometry/wheel_odometry.cpp)
 
-
 # example for legged robot kinemtaics correction:
 rosbuild_add_executable(mini_cheetah
   ${PROJECT_SOURCE_DIR}/examples/mini_cheetah.cpp)
@@ -134,7 +134,6 @@ target_link_libraries(fetch_gyro_filter
   yaml-cpp
 )
 
-
 rosbuild_add_executable(husky_gyro_filter
   ${PROJECT_SOURCE_DIR}/examples/husky_gyro_filter.cpp)
 
@@ -145,7 +144,6 @@ target_link_libraries(husky_gyro_filter
   yaml-cpp
 )
 
-
 rosbuild_add_executable(husky_odom
   ${PROJECT_SOURCE_DIR}/examples/husky_odom.cpp)
 
@@ -156,6 +154,35 @@ target_link_libraries(husky_odom
   yaml-cpp
 )
 
+rosbuild_add_executable(wamv_gtodom_vel
+  ${PROJECT_SOURCE_DIR}/examples/wamv_gtodom_vel.cpp)
+
+target_link_libraries(wamv_gtodom_vel
+  ${LIBS}
+  ros_communication_lib
+  drift
+  yaml-cpp
+)
+
+rosbuild_add_executable(wamv_gtodom
+  ${PROJECT_SOURCE_DIR}/examples/wamv_gtodom.cpp)
+
+target_link_libraries(wamv_gtodom
+  ${LIBS}
+  ros_communication_lib
+  drift
+  yaml-cpp
+)
+
+rosbuild_add_executable(wamv_gps
+  ${PROJECT_SOURCE_DIR}/examples/wamv_gps.cpp)
+
+target_link_libraries(wamv_gps
+  ${LIBS}
+  ros_communication_lib
+  drift
+  yaml-cpp
+)
 
 rosbuild_add_executable(girona500
   ${PROJECT_SOURCE_DIR}/examples/girona500.cpp)
@@ -197,8 +224,7 @@ target_link_libraries(neya_gyro_filter
   yaml-cpp
 )
 
-
-### baselines ###
+# ## baselines ###
 rosbuild_add_executable(fetch_wheel_odom
   ${PROJECT_SOURCE_DIR}/baselines/fetch_wheel_odom.cpp)
 

ROS/drift/bag_gtodom_pub.launch

@@ -0,0 +1,7 @@
+<launch>
+    <!-- Play rosbag with clock option (using absolute path) -->
+    <node pkg="rosbag" type="play" name="rosbag_play" args="1_rectangle.bag --start 15.44854" output="screen" />
+
+    <!-- Run Python script (using relative path within a package) -->
+    <node pkg="/home/neofelis/VRX/drift/ROS/drift" type="odom_pub.py" name="odom_pub" output="screen" cwd="$(find your_package)/launch" />
+</launch>

ROS/drift/config/husky_odom/ros_comm.yaml

@@ -2,7 +2,9 @@ subscribers:
   # Name all the topics to subscribe from in this section. You may also redefine the variable name
   # as long as you can load the yaml variable in your main file
   imu_topic: "/gx5_1/imu/data"
-  odom_topic: "/qualisys_synced/husky_center_odom"
+  # odom_topic: "/gx5_0/nav/odom"
+  # odom_topic: "/qualisys_synced/husky_center_odom"
+  odom_topic: "/zed_node/odom"
   translation_odom_source_to_body: [0,0,0]
   rotation_odom_source_to_body: [1,0,0,0]
 

ROS/drift/config/wamv_gps/ros_comm.yaml

@@ -0,0 +1,23 @@
+subscribers:
+  imu_topic: "/wamv/sensors/imu/imu/data"
+  gps_topic: "/wamv/sensors/gps/gps/fix"
+  reference_position: [-33.7227694, 150.6739842, 1.256729] #for output.bag (wamv robot)
+  # reference_position: [-33.72280028614153, 150.6740427241651, 1.2450301479548216] #for circle.bag (wamv robot)
+  # reference_position: [-33.72276878149592, 150.67399044470977, 1.2302990267053246] #for straight_path.bag (wamv robot)
+
+  # imu_topic: "/gx5_1/imu/data"
+  # gps_topic: "/gps/fix"
+  # reference_position: [42.29444028, -83.71046088166666, 239.99] #for 1_rectangle.bag (husky robot)
+  # translation_gps_source_to_body: [0, 0, 0]
+  # rotation_gps_source_to_body: [0.7071, 0, 0, 0.7071] #for husky
+
+publishers:
+  # Define publish rate and topics for publishers
+  pose_publish_rate: 1000 # Hz
+  pose_publish_topic: "/wamv/inekf_odom_correction/pose"
+  pose_frame: "/map"
+
+  path_publish_rate: 100 # Hz
+  path_publish_topic: "/wamv/inekf_odom_correction/path"
+
+  enable_slip_publisher: true

ROS/drift/config/wamv_gtodom/ros_comm.yaml

@@ -0,0 +1,26 @@
+subscribers:
+  # Name all the topics to subscribe from in this section. You may also redefine the variable name
+  # as long as you can load the yaml variable in your main file
+  imu_topic: "/wamv/sensors/imu/imu/data"
+  odom_topic: "/wamv/sensors/position/ground_truth_odometry"
+  translation_odom_source_to_body: [-532.64,161.91,-0.07] #for output.bag
+  # translation_odom_source_to_body: [-527.127, 158.427, -0.085] #for circle.bag
+  # translation_odom_source_to_body: [0, 0, 0]
+  # rotation_odom_source_to_body: [0.877,-0.002,0.004,0.479]
+  rotation_odom_source_to_body: [1, 0, 0, 0]
+  # imu_topic: "/gx5_1/imu/data"
+  # odom_topic: "/gt_odom"
+  # translation_odom_source_to_body: [0, 0, 0]
+  # # rotation_odom_source_to_body: [0.877,-0.002,0.004,0.479]
+  # rotation_odom_source_to_body: [1, 0, 0, 0]
+
+publishers:
+  # Define publish rate and topics for publishers
+  pose_publish_rate: 1000 # Hz
+  pose_publish_topic: "/wamv/inekf_odom_correction/pose"
+  pose_frame: "/map"
+
+  path_publish_rate: 100 # Hz
+  path_publish_topic: "/wamv/inekf_odom_correction/path"
+
+  enable_slip_publisher: true

ROS/drift/config/wamv_gtodom_vel/ros_comm.yaml

@@ -0,0 +1,22 @@
+subscribers:
+  # Name all the topics to subscribe from in this section. You may also redefine the variable name
+  # as long as you can load the yaml variable in your main file
+  imu_topic: "/gx5_1/imu/data"
+  odom_topic: "/gt_odom"
+  translation_odom_source_to_body: [0, 0, 0]
+  # rotation_odom_source_to_body: [1,0,0,0]
+  # translation_odom_source_to_body: [-532.64,161.91,-0.07]
+  # rotation_odom_source_to_body: [0.877,-0.002,0.004,0.479]
+  # translation_odom_source_to_body: [-532.64,161.91,-0.07]
+  # rotation_odom_source_to_body: [0.8776,-0.0040,0.0060,-0.4794]
+  rotation_odom_source_to_body: [1, 0, 0, 0]
+publishers:
+  # Define publish rate and topics for publishers
+  pose_publish_rate: 1000 # Hz
+  pose_publish_topic: "/wamv/inekf_odom_correction/pose"
+  pose_frame: "/map"
+
+  path_publish_rate: 100 # Hz
+  path_publish_topic: "/wamv/inekf_odom_correction/path"
+
+  enable_slip_publisher: true

ROS/drift/examples/wamv_gps.cpp

@@ -0,0 +1,138 @@
+/* ----------------------------------------------------------------------------
+ * Copyright 2024, CURLY Lab, University of Michigan
+ * All Rights Reserved
+ * See LICENSE for the license information
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file   wamv_gps.cpp
+ *  @author Surya Singh
+ *  @brief  Test file for WAMV robot setup (IMU Propagation +
+ *  Ground-truth based Position Correction)
+ *  @date   Oct 11, 2024
+ **/
+
+#include <ros/ros.h>
+#include <filesystem>
+#include <iostream>
+
+#include "communication/ros_publisher.h"
+#include "communication/ros_subscriber.h"
+#include "drift/estimator/inekf_estimator.h"
+
+using namespace std;
+using namespace state;
+using namespace estimator;
+
+
+int main(int argc, char** argv) {
+  /// TUTORIAL: Initialize ROS node
+  ros::init(argc, argv, "wamv_gps");
+
+  std::cout << "The subscriber is on!" << std::endl;
+
+  /// TUTORIAL: Initialize ROS node handle. ROS handle handles the
+  /// start/shutdown for us
+  ros::NodeHandle nh;
+
+  /// TUTORIAL: Create a ROS subscriber
+  ros_wrapper::ROSSubscriber ros_sub(&nh);
+
+  /// TUTORIAL: Load your yaml file
+  // Find current path
+  std::string file{__FILE__};
+  std::string project_dir{file.substr(0, file.rfind("ROS/drift/examples/"))};
+  std::cout << "Project directory: " << project_dir << std::endl;
+
+  std::string ros_config_file
+      = project_dir + "/ROS/drift/config/wamv_gps/ros_comm.yaml";
+  YAML::Node config = YAML::LoadFile(ros_config_file);
+  std::string imu_topic = config["subscribers"]["imu_topic"].as<std::string>();
+  std::string gps_topic = config["subscribers"]["gps_topic"].as<std::string>();
+  std::vector<double> translation_gpssrc2body
+      = config["subscribers"]["translation_gps_source_to_body"]
+            .as<std::vector<double>>();
+  std::vector<double> rotation_gpssrc2body
+      = config["subscribers"]["rotation_gps_source_to_body"]
+            .as<std::vector<double>>();
+  // Define the reference position vector: (latitude, longitude, altitude)
+  Eigen::Vector3d reference_position;
+
+  try {
+    const YAML::Node& ref_pos_node
+        = config["subscribers"]["reference_position"];
+
+    // Access the values and assign them to reference_position
+    double x = ref_pos_node[0].as<double>();
+    double y = ref_pos_node[1].as<double>();
+    double z = ref_pos_node[2].as<double>();
+
+    reference_position = Eigen::Vector3d(x, y, z);
+  } catch (const YAML::Exception& e) {
+    throw std::runtime_error(
+        "Error reading reference_position: expected a sequence of 3 doubles.");
+  }
+
+  /// TUTORIAL: Add a subscriber for IMU data and get its queue and mutex
+  auto qimu_and_mutex = ros_sub.AddIMUSubscriber(imu_topic);
+  auto qimu = qimu_and_mutex.first;
+  auto qimu_mutex = qimu_and_mutex.second;
+
+  /// TUTORIAL: Add a subscriber for velocity data and get its queue and mutex
+  auto qp_and_mutex = ros_sub.AddGPS2PositionSubscriber(
+      gps_topic, translation_gpssrc2body, rotation_gpssrc2body,
+      reference_position);
+  auto qp = qp_and_mutex.first;
+  auto qp_mutex = qp_and_mutex.second;
+
+  /// TUTORIAL: Start the subscriber thread
+  ros_sub.StartSubscribingThread();
+
+  /// TUTORIAL: Define some configurations for the state estimator
+  inekf::ErrorType error_type = RightInvariant;
+
+  /// TUTORIAL: Create a state estimator
+  InekfEstimator inekf_estimator(
+      error_type, project_dir + "/config/wamv_gps/inekf_estimator.yaml");
+
+  /// TUTORIAL: Add a propagation and correction(s) methods to the state
+  /// estimator. Here is an example of IMU propagation and position correction
+  /// for WAMV robot
+  inekf_estimator.add_imu_propagation(
+      qimu, qimu_mutex, project_dir + "/config/wamv_gps/imu_propagation.yaml");
+  inekf_estimator.add_position_correction(
+      qp, qp_mutex, project_dir + "/config/wamv_gps/position_correction.yaml");
+
+
+  /// TUTORIAL: Get the robot state queue and mutex from the state estimator
+  RobotStateQueuePtr robot_state_queue_ptr
+      = inekf_estimator.get_robot_state_queue_ptr();
+  std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr
+      = inekf_estimator.get_robot_state_queue_mutex_ptr();
+
+  /// TUTORIAL: Create a ROS publisher and start the publishing thread
+  ros_wrapper::ROSPublisher ros_pub(
+      &nh, robot_state_queue_ptr, robot_state_queue_mutex_ptr, ros_config_file);
+  ros_pub.StartPublishingThread();
+
+  /// TUTORIAL: Run the state estimator. Initialize the bias first, then
+  /// initialize the state. After that, the state estimator will be enabled.
+  /// The state estimator should be run in a loop. Users can use RVIZ to
+  /// visualize the path. The path topic will be something like
+  /// "/robot/*/path"
+  while (ros::ok()) {
+    // Step behavior
+    if (inekf_estimator.is_enabled()) {
+      inekf_estimator.RunOnce();
+    } else {
+      if (inekf_estimator.BiasInitialized()) {
+        inekf_estimator.InitState();
+      } else {
+        inekf_estimator.InitBias();
+      }
+    }
+    ros::spinOnce();
+  }
+
+  return 0;
+}