From d11b643ebeefad2fd16dd474dca274f915eb4a4f Mon Sep 17 00:00:00 2001
From: Luisa Mao <luisa.maocp@gmail.com>
Date: Wed, 29 May 2024 12:56:34 -0500
Subject: [PATCH] add roll

---
 config/jackal_bev_node.lua    | 47 +++++++++++++++++++++++++++++++++++
 src/bev_node/bev_node_main.cc |  5 ++--
 src/shared                    |  2 +-
 3 files changed, 51 insertions(+), 3 deletions(-)
 create mode 100644 config/jackal_bev_node.lua

diff --git a/config/jackal_bev_node.lua b/config/jackal_bev_node.lua
new file mode 100644
index 0000000..6808cca
--- /dev/null
+++ b/config/jackal_bev_node.lua
@@ -0,0 +1,47 @@
+function deg2rad(deg)
+  return deg * (math.pi / 180)
+end
+
+
+-- https://stackoverflow.com/a/23535333 Returns the absolute path with a
+-- trailing slash to the directory containing this script.
+function config_dir()
+  local script_path = debug.getinfo(2, "S").source:sub(2)
+  return script_path:match("(.*/)")
+end
+
+
+BEVParameters = {
+    camera_calibration_config_path = config_dir() .. "camera_calibration_kinect.yaml";
+
+    input_image_topic = "/camera/rgb/image_raw";
+    input_image_width = 1280;
+    input_image_height = 720;
+
+    bev_image_topic = "/bev/single"; -- image_transport takes the base topic name
+    bev_pixels_per_meter = 150.0;
+    bev_horizon_distance = 5.0;
+
+    bev_empty_region_rgb = {0, 0, 0};
+
+    stitched_bev_image_topic = "/bev/stitched";
+    stitched_bev_angle_topic = "/bev/stitched_angle";
+    stitched_bev_horizon_distance = 6.0;
+    stitched_bev_ema_gamma = 0.1;
+    stitched_bev_update_distance = 1.0; -- meters
+    stitched_bev_update_angle = 10; -- degrees
+
+
+    -- stitching is not required for the spot at this time
+    -- pose_topic = "/husky/inekf_estimation/pose";
+    pose_topic = "/foo_9498125";
+
+    cv_num_threads = 2;
+
+    T_ground_camera = {
+        x = -0.06;
+        y = 0.05;
+        z = 0.53;
+        pitch = deg2rad(-2.6);
+    };
+}
diff --git a/src/bev_node/bev_node_main.cc b/src/bev_node/bev_node_main.cc
index e54b1de..de25005 100644
--- a/src/bev_node/bev_node_main.cc
+++ b/src/bev_node/bev_node_main.cc
@@ -46,6 +46,7 @@ CONFIG_FLOAT(T_ground_camera_x, "BEVParameters.T_ground_camera.x");
 CONFIG_FLOAT(T_ground_camera_y, "BEVParameters.T_ground_camera.y");
 CONFIG_FLOAT(T_ground_camera_z, "BEVParameters.T_ground_camera.z");
 CONFIG_FLOAT(T_ground_camera_pitch, "BEVParameters.T_ground_camera.pitch");
+CONFIG_FLOAT(T_ground_camera_roll, "BEVParameters.T_ground_camera.roll");
 
 CONFIG_UINT(cv_num_threads, "BEVParameters.cv_num_threads");
 
@@ -62,8 +63,8 @@ Eigen::Affine3f Read_T_ground_camera() {
   return Eigen::Translation3f(CONFIG_T_ground_camera_x,
                               CONFIG_T_ground_camera_y,
                               CONFIG_T_ground_camera_z) *
-         Eigen::AngleAxisf(CONFIG_T_ground_camera_pitch,
-                           Eigen::Vector3f::UnitY());
+         Eigen::AngleAxisf(CONFIG_T_ground_camera_roll, Eigen::Vector3f::UnitX()) *
+         Eigen::AngleAxisf(CONFIG_T_ground_camera_pitch, Eigen::Vector3f::UnitY());
 }
 
 Eigen::Matrix3f ReadIntrinsicMatrix() {
diff --git a/src/shared b/src/shared
index 899429d..3a34563 160000
--- a/src/shared
+++ b/src/shared
@@ -1 +1 @@
-Subproject commit 899429d47d18fdad6b2cd6266ded192ee9702e75
+Subproject commit 3a345631b4d9a5b030e146fbb2f0063018066bf8