From f28fd1c3e563fa3c441884b5da3de2a1a0f08427 Mon Sep 17 00:00:00 2001
From: "mona.goebel" <mona.goebel@tu-darmstadt.de>
Date: Wed, 24 Apr 2024 15:17:50 +0200
Subject: [PATCH] Update ekf_navsat.yaml, pub_camGPS_msc2ros.py, and 17 more
 files...

---
 backpack_pkg/launch/backpack.launch           |  46 +++-
 backpack_pkg/launch/backpack_sensors.launch   |  16 +-
 backpack_pkg/params/FL_velodyne_vlp1.yaml     |  10 +-
 backpack_pkg/params/camera_UserSet1.cfg       | 111 ++++++++++
 backpack_pkg/params/cameracalib.ini           |  33 +++
 backpack_pkg/params/cameracalib.yml           |  26 +++
 backpack_pkg/params/ekf_navsat.yaml           | 125 -----------
 backpack_pkg/params/gq7.yml                   | 100 +++++++++
 ...tain_config.yml => microstrain_config.yml} |  66 ++++--
 backpack_pkg/params/stcamera_node.yaml        |  65 ++++++
 backpack_pkg/src/cam_off.py                   |  23 --
 backpack_pkg/src/cam_start_capture.py         |  35 ----
 .../src/overwrite_parameters_from_file.py     |  84 ++++++++
 backpack_pkg/src/pub_OdomInfo_ros2msc.py      | 196 ------------------
 backpack_pkg/src/pub_camGPS_msc2ros.py        |  47 -----
 backpack_pkg/src/pub_camInfo_msc2ros.py       | 105 ----------
 backpack_pkg/src/show_ROM_features.py         |  40 ++++
 backpack_pkg/src/sub_camera_piexif.py         | 113 ++++++++++
 18 files changed, 672 insertions(+), 569 deletions(-)
 mode change 100755 => 100644 backpack_pkg/launch/backpack.launch
 mode change 100755 => 100644 backpack_pkg/launch/backpack_sensors.launch
 create mode 100644 backpack_pkg/params/camera_UserSet1.cfg
 create mode 100644 backpack_pkg/params/cameracalib.ini
 create mode 100644 backpack_pkg/params/cameracalib.yml
 delete mode 100644 backpack_pkg/params/ekf_navsat.yaml
 create mode 100644 backpack_pkg/params/gq7.yml
 rename backpack_pkg/params/{microstain_config.yml => microstrain_config.yml} (91%)
 create mode 100644 backpack_pkg/params/stcamera_node.yaml
 delete mode 100644 backpack_pkg/src/cam_off.py
 delete mode 100644 backpack_pkg/src/cam_start_capture.py
 create mode 100644 backpack_pkg/src/overwrite_parameters_from_file.py
 delete mode 100644 backpack_pkg/src/pub_OdomInfo_ros2msc.py
 delete mode 100644 backpack_pkg/src/pub_camGPS_msc2ros.py
 delete mode 100644 backpack_pkg/src/pub_camInfo_msc2ros.py
 create mode 100644 backpack_pkg/src/show_ROM_features.py
 create mode 100644 backpack_pkg/src/sub_camera_piexif.py

diff --git a/backpack_pkg/launch/backpack.launch b/backpack_pkg/launch/backpack.launch
old mode 100755
new mode 100644
index 50ddb1e..93b90ec
--- a/backpack_pkg/launch/backpack.launch
+++ b/backpack_pkg/launch/backpack.launch
@@ -1,19 +1,47 @@
 <launch>
   <arg name="record" default="false" />
-  
-  <node pkg="tf" type="static_transform_publisher" name="base_link_to_vlp201" args="-0.02 0 0.83 3.14 0.785 0 /base_link /vlp201 100" />
-  <node pkg="tf" type="static_transform_publisher" name="base_link_to_vlp202" args="-0.37 0 -0.15 0 -1.57 0 /base_link /vlp202 100" />
-  <node pkg="tf" type="static_transform_publisher" name="base_link_to_imu" args="0.0 0 0.0 0 0 0 /base_link /imu_link 100" /> 
-  <node pkg="tf" type="static_transform_publisher" name="base_link_to_gnss1" args="-0.1 0 0.83 0 0 0 /base_link /gnss1_link 10" />
-  
+  <arg name="cam_connected" default="true" />
+  
+  <!-- LIDAR -->
+  <node pkg="tf" type="static_transform_publisher" name="base_link_to_vlp201" args="-0.02 0 0.86 3.14 0.698 0 /base_link /vlp201 100" />
+  <node pkg="tf" type="static_transform_publisher" name="base_link_to_vlp202" args="-0.37 0 -0.15 1.57 -1.57 1.57 /base_link /vlp202 100" />
+  <!-- IMU -->
+  <node pkg="tf" type="static_transform_publisher" name="base_link_gq7_link_static_transform" args="0 0 0 0 0 0 base_link gq7_link 1000" /> 
+  -->
+  <!-- GNSS maybe handled by MicroStrain
+  <node pkg="tf" type="static_transform_publisher" name="base_link_to_gnss1" args="0 0 0.3 0 0 0 /sensor_wgs84_enu /base_link 200" />
+  node pkg="tf" type="static_transform_publisher" name="base_link_to_gnss2" args="-0.01 -0.11 0.86 0 0 0 /base_link /gnss2_link 10" / -->
+  
+  <!-- map -->
+  <node pkg="tf" type="static_transform_publisher" name="map_to_base_link" args="0 0 0.3 0 0 0 /map /base_link 1000" />
+  <node pkg="tf" type="static_transform_publisher" name="map_to_sensor_wgs84_enu" args="0 0 0 0 0 0 /map /earth 1000" />
   
   <include file="$(find backpack_pkg)/launch/backpack_sensors.launch" >
     <arg name="record" value="$(arg record)" />
+    <arg name="cam_connected" value="$(arg cam_connected)" />
   </include>
   
-  <!-- node pkg="backpack_pkg" name="pub_camGPS_msc2ros" type="pub_camGPS_msc2ros.py" output="screen" /> 
-  <node pkg="backpack_pkg" name="pub_camInfo_msc2ros" type="pub_camInfo_msc2ros.py" output="screen" / --> 
-  <node pkg="backpack_pkg" name="inform_camera" type="pub_OdomInfo_ros2msc.py" output="screen" /> 
+
+    <!-- run ekf nodes: (1) EKF: odom > base_link (2) EKF: map > odom (depends on (1)); Navsat GPS > map>
+  <rosparam command="load" file="$(find backpack_pkg)/params/ekf_navsat.yaml" />
+
+  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_odom" clear_params="true"/>
+  
+  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_map" clear_params="true">
+    <remap from="odometry/filtered" to="odometry/filtered_map"/>
+  </node>       -->
+  
+  
+  <!-- node pkg="backpack_pkg" name="pub_camGPS_msc2ros" type="pub_camGPS_msc2ros.py" output="screen" / --> 
+  <!--node name="pub_camInfo_msc2ros" pkg="backpack_pkg" type="pub_camInfo_msc2ros.py" output="screen" ns="/" if="$(arg cam_connected)"/> 
+  <node name="pub_OdomInfo_ros2msc" pkg="backpack_pkg" type="pub_OdomInfo_ros2msc.py" output="screen" ns="/" if="$(arg cam_connected)"/ --> 
+  
+  
+  <!-- Node for the GPS attached on IMU >
+  <node pkg="robot_localization" type="navsat_transform_node" name="navsat_imu" clear_params="true" ns="imu">
+    <remap from="odometry/filtered" to="odometry/filtered_map"/>
+    <remap from="gps/fix" to="/gnss1/fix"/>
+  </node -->
   
   <!-- include file="$(find backpack_pkg)/launch/laser_assemble.launch" / -->  
   <!--include file="$(find backpack_pkg)/launch/gps_velo_pub.launch" /-->  
diff --git a/backpack_pkg/launch/backpack_sensors.launch b/backpack_pkg/launch/backpack_sensors.launch
old mode 100755
new mode 100644
index 29ffe0b..95dc345
--- a/backpack_pkg/launch/backpack_sensors.launch
+++ b/backpack_pkg/launch/backpack_sensors.launch
@@ -1,5 +1,6 @@
 <launch>
    <arg name="record" default="false" />
+   <arg name="cam_connected" default="false" />
 
    <group ns="vlp1">
       <!-- Start the Velodyne driver (source to adjust two velodynes https://github.com/ros-drivers/velodyne/issues/108) -->
@@ -30,10 +31,21 @@
        
    </group> 
 
+   <group ns="/">
+      <!-- Start the camera driver -->
+       <node name="camera" pkg="omronsentech_camera" type="stcamera_node" output="screen">
 
+          <!-- Load the default params file. -->
+          <rosparam file="$(find backpack_pkg)/params/stcamera_node.yaml"  command="load" />
+       </node>
+       
+   </group> 
+   
+   
+   
 <!-- Start the bag file -->
-<node pkg="rosbag" type="record" name="rosbag_record_imu_lidar" args="record -o $(find backpack_pkg)/bagfiles/backp_imu_lidar.bag -a" if="$(arg record)"/>
-
+<!-- node pkg="rosbag" type="record" name="rosbag_record_imu_lidar" args="record -o $(find backpack_pkg)/bagfiles/backp_imu_lidar.bag /vlp1/velodyne_points /vlp2/velodyne_points /nav/fix /imu/data /camera/dev_142122508751/camera_info /tf " if="$(arg record)"/ -->
+   <node pkg="rosbag" type="record" name="rosbag_record_imu_lidar" args="record -o $(find backpack_pkg)/bagfiles/backp_imu_lidar.bag -a" if="$(arg record)"/>
 
 </launch>
 
diff --git a/backpack_pkg/params/FL_velodyne_vlp1.yaml b/backpack_pkg/params/FL_velodyne_vlp1.yaml
index dcdbf43..b5ff0d1 100644
--- a/backpack_pkg/params/FL_velodyne_vlp1.yaml
+++ b/backpack_pkg/params/FL_velodyne_vlp1.yaml
@@ -2,7 +2,7 @@ common:
     lid_topic:  "/vlp1/velodyne_points"
     imu_topic:  "/imu/data"
     time_sync_en: false         # ONLY turn on when external time synchronization is really not possible
-    time_offset_lidar_to_imu: -0.10873383 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README).
+    time_offset_lidar_to_imu: -0.002256   #-0.10873383 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README).
                                   # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0
 
 preprocess:
@@ -20,10 +20,10 @@ mapping:
     fov_degree:    360
     det_range:     100.0
     extrinsic_est_en:  false      # true: enable the online estimation of IMU-LiDAR extrinsic,
-    extrinsic_T: [ 0.160012 -0.088218  0.757596]
-    extrinsic_R: [ 0.7207992, -0.0501856,  0.6913247,
-   0.0694568,  0.9975849,  0.0000000,
-  -0.6896552,  0.0480172,  0.7225442 ]
+    extrinsic_T: [ 0 0.785  1.571]  #extrinsic_T: [ 0.160012 -0.088218  0.757596]
+    extrinsic_R: [ -0.0001, -0.7074, 0.7068,  #extrinsic_R: [ 0.7207992, -0.0501856,  0.6913247,
+   1.0000, -0.0002, 0.0000,     #0.0694568,  0.9975849,  0.0000000,
+  0.0001, 0.7068, 0.7074 ]    #-0.6896552,  0.0480172,  0.7225442 ]
 
 publish:
     path_en:  true
diff --git a/backpack_pkg/params/camera_UserSet1.cfg b/backpack_pkg/params/camera_UserSet1.cfg
new file mode 100644
index 0000000..6d70130
--- /dev/null
+++ b/backpack_pkg/params/camera_UserSet1.cfg
@@ -0,0 +1,111 @@
+# {05D8C294-F295-4dfb-9D01-096BD04049F4}
+# GenApi persistence file (version 3.4.0)
+# RemoteDevice = Sentech::STC_U3V_CM -- Sentech Camera Interface -- Device version = 2.0.15 -- Product GUID = D0E8AF41-B99A-45D6-B660-6C23C4F5957F -- Product version GUID = 178CC30C-E80A-4AF4-A64C-5113725CA70B
+DeviceLinkThroughputLimitMode	{DeviceLinkSelector=0}	Off
+DeviceLinkSelector	0
+DeviceLinkThroughputLimit	{DeviceLinkSelector=0}	398831548
+DeviceLinkSelector	0
+SensorShutterMode	Global
+BinningHorizontalMode	{BinningSelector=Sensor}	Average
+BinningSelector	Sensor
+BinningHorizontal	{BinningSelector=Sensor}	1
+BinningSelector	Sensor
+BinningVerticalMode	{BinningSelector=Sensor}	Average
+BinningSelector	Sensor
+BinningVertical	{BinningSelector=Sensor}	1
+BinningSelector	Sensor
+DecimationHorizontal	1
+DecimationVertical	1
+RegionMode	{RegionSelector=Region1}	Off
+RegionMode	{RegionSelector=Region2}	Off
+RegionMode	{RegionSelector=Region3}	Off
+RegionMode	{RegionSelector=Region4}	Off
+RegionMode	{RegionSelector=Region5}	Off
+RegionMode	{RegionSelector=Region6}	Off
+RegionMode	{RegionSelector=Region7}	Off
+RegionSelector	Region0
+Width	{RegionSelector=Region0}	2048
+Width	{RegionSelector=Region1}	0
+Width	{RegionSelector=Region2}	0
+Width	{RegionSelector=Region3}	0
+Width	{RegionSelector=Region4}	0
+Width	{RegionSelector=Region5}	0
+Width	{RegionSelector=Region6}	0
+Width	{RegionSelector=Region7}	0
+RegionSelector	Region0
+Height	{RegionSelector=Region0}	2048
+Height	{RegionSelector=Region1}	0
+Height	{RegionSelector=Region2}	0
+Height	{RegionSelector=Region3}	0
+Height	{RegionSelector=Region4}	0
+Height	{RegionSelector=Region5}	0
+Height	{RegionSelector=Region6}	0
+Height	{RegionSelector=Region7}	0
+RegionSelector	Region0
+OffsetX	{RegionSelector=Region0}	0
+OffsetX	{RegionSelector=Region1}	0
+OffsetX	{RegionSelector=Region2}	0
+OffsetX	{RegionSelector=Region3}	0
+OffsetX	{RegionSelector=Region4}	0
+OffsetX	{RegionSelector=Region5}	0
+OffsetX	{RegionSelector=Region6}	0
+OffsetX	{RegionSelector=Region7}	0
+RegionSelector	Region0
+OffsetY	{RegionSelector=Region0}	0
+OffsetY	{RegionSelector=Region1}	0
+OffsetY	{RegionSelector=Region2}	0
+OffsetY	{RegionSelector=Region3}	0
+OffsetY	{RegionSelector=Region4}	0
+OffsetY	{RegionSelector=Region5}	0
+OffsetY	{RegionSelector=Region6}	0
+OffsetY	{RegionSelector=Region7}	0
+RegionSelector	Region0
+PixelFormat	{ImageComponentSelector=Intensity}	Mono8
+ImageComponentSelector	Intensity
+ReverseX	0
+ReverseY	0
+MultiROIsInMultiPayloads	Combined
+AcquisitionFrameRate	20.0007
+TriggerMode	{TriggerSelector=FrameStart}	Off
+TriggerMode	{TriggerSelector=ExposureStart}	Off
+TriggerSelector	FrameStart
+TriggerSelector	FrameStart
+TriggerSelector	FrameStart
+ExposureMode	Off
+ExposureTimeSelector	Common
+ExposureAuto	Off
+LineMode	{LineSelector=Line0}	Input
+LineMode	{LineSelector=Line1}	Input
+LineMode	{LineSelector=Line2}	Input
+LineSelector	Line0
+LineInverter	{LineSelector=Line0}	1
+LineInverter	{LineSelector=Line1}	1
+LineInverter	{LineSelector=Line2}	1
+LineSelector	Line0
+LineSelector	Line0
+UserOutputValue	{UserOutputSelector=UserOutput0}	0
+UserOutputValue	{UserOutputSelector=UserOutput1}	0
+UserOutputValue	{UserOutputSelector=UserOutput2}	0
+UserOutputSelector	UserOutput0
+StrobeOutDelay	30
+StrobeOutOnTime	32
+TriggerOutDelay	0
+TriggerOutOnTime	32
+LineDeviceResetMode	Off
+EventNotification	{EventSelector=ExposureEnd}	Off
+EventSelector	ExposureEnd
+Gain	{GainSelector=DigitalAll}	0
+GainSelector	DigitalAll
+DigitalGainOffsetMode	On
+GainAuto	{GainSelector=DigitalAll}	Off
+GainSelector	DigitalAll
+GainSelector	DigitalAll
+GainSelector	DigitalAll
+GainSelector	DigitalAll
+BlackLevel	{ImageComponentSelector=Intensity	BlackLevelSelector=AnalogAll}	10
+ImageComponentSelector	Intensity
+BlackLevelSelector	AnalogAll
+ChunkModeActive	0
+ChunkEnable	{ChunkSelector=Image}	1
+ChunkSelector	Image
+HDRMode	Off
diff --git a/backpack_pkg/params/cameracalib.ini b/backpack_pkg/params/cameracalib.ini
new file mode 100644
index 0000000..0652220
--- /dev/null
+++ b/backpack_pkg/params/cameracalib.ini
@@ -0,0 +1,33 @@
+# produced by rosrun camera_calibration cameracalibrator.py --size 7x5 --square 0.95 image:=/stcamera_node/dev_142122508751/image_raw camera:=/stcamera_node/dev_142122508751 --no-service-check
+
+[image]
+
+width
+2048
+
+height
+2048
+
+[camera/dev_142122508751]
+
+camera matrix
+1513.838016 0.000000 1036.847889
+0.000000 1505.835130 997.648866
+0.000000 0.000000 1.000000
+
+
+distortion
+-0.161560 0.082874 0.000936 -0.000917 0.000000
+
+
+rectification
+1.000000 0.000000 0.000000
+0.000000 1.000000 0.000000
+0.000000 0.000000 1.000000
+
+projection
+1420.022949 0.000000 1035.151324 0.000000
+0.000000 1411.911621 997.190455 0.000000
+0.000000 0.000000 1.000000 0.000000
+
+
diff --git a/backpack_pkg/params/cameracalib.yml b/backpack_pkg/params/cameracalib.yml
new file mode 100644
index 0000000..3a3c7f7
--- /dev/null
+++ b/backpack_pkg/params/cameracalib.yml
@@ -0,0 +1,26 @@
+image_width: 2048
+image_height: 2048
+camera_name: camera/dev_142122508751
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [1513.83802,    0.     , 1036.84789,
+            0.     , 1505.83513,  997.64887,
+            0.     ,    0.     ,    1.     ]
+distortion_model: plumb_bob
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [-0.161560, 0.082874, 0.000936, -0.000917, 0.000000]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1., 0., 0.,
+         0., 1., 0.,
+         0., 0., 1.]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [1420.02295,    0.     , 1035.15132,    0.     ,
+            0.     , 1411.91162,  997.19046,    0.     ,
+            0.     ,    0.     ,    1.     ,    0.     ]
diff --git a/backpack_pkg/params/ekf_navsat.yaml b/backpack_pkg/params/ekf_navsat.yaml
deleted file mode 100644
index ddc0ade..0000000
--- a/backpack_pkg/params/ekf_navsat.yaml
+++ /dev/null
@@ -1,125 +0,0 @@
-
-ekf_local:
-  frequency: 10
-  sensor_timeout: 0.4
-  two_d_mode: false
-  transform_time_offset: 0.0
-  transform_timeout: 0.0
-  print_diagnostics: true
-  debug: false
-
-  map_frame: map
-  odom_frame: odom
-  base_link_frame: base_link
-  world_frame: odom
-
-  odom0: nav/odom
-  odom0_config: [true,  true,  true,
-                 true, true, true,
-                 false, false, false,
-                 false, false, false,
-                 false, false, false]
-  odom0_queue_size: 0
-  odom0_nodelay: true
-  odom0_differential: false
-  odom0_relative: false
-  stamped_control: false
-
-  #odom1: vlp2/slam/Odometry
-  #odom1_config: [true,  true,  true,
-  #               true, true, true,
-  #               false, false, false,
-   #              false, false, false,
-  #               false, false, false]
-  #odom1_queue_size: 0
-  #odom1_nodelay: true
-  #odom1_differential: false
-  #odom1_relative: false
-  #stamped_control: false
-  
-  imu0: imu/data
-  imu0_config: [false, false, false,
-                true,  true,  true,
-                false, false, false,
-                true,  true,  true,
-                true,  true,  true]
-  imu0_nodelay: false
-  imu0_differential: false
-  imu0_relative: false
-  imu0_queue_size: 10
-  imu0_remove_gravitational_acceleration: true
-  use_control: true
-
-ekf_global:
-  frequency: 100
-  sensor_timeout: 0.4
-  two_d_mode: false
-  transform_time_offset: 0.0
-  transform_timeout: 0.0
-  print_diagnostics: true
-  debug: false
-
-  map_frame: map
-  odom_frame: odom
-  base_link_frame: base_link
-  world_frame: map
-
-  odom0: nav/odom
-  odom0_config: [true,  true,  true,
-                 true, true, true,
-                 false, false, false,
-                 false, false, false,
-                 false, false, false]
-  odom0_queue_size: 0
-  odom0_nodelay: true
-  odom0_differential: false
-  odom0_relative: false
-  stamped_control: false
-
-  #odom1: vlp2/slam/Odometry
-  #odom1_config: [true,  true,  true,
-  #               true, true, true,
-  #               false, false, false,
-  #               false, false, false,
-  #               false, false, false]
-  #odom1_queue_size: 0
-  #odom1_nodelay: true
-  #odom1_differential: false
-  #odom1_relative: false
-  #stamped_control: false
-  
-  odom2: ekf/odometry/gps   #published by navsat_transform_node
-  odom2_config: [true,  true,  false,
-                 false, false, false,
-                 false, false, false,
-                 false, false, false,
-                 false, false, false]
-  odom2_queue_size: 0
-  odom2_nodelay: true
-  odom2_differential: false
-  odom2_relative: false
-  
-  imu0: imu/data
-  imu0_config: [false, false, false,
-                true,  true,  true,
-                false, false, false,
-                true,  true,  true,
-                true,  true,  true]
-  imu0_nodelay: false
-  imu0_differential: false
-  imu0_relative: false
-  imu0_queue_size: 10
-  imu0_remove_gravitational_acceleration: true
-  use_control: true
-  
-navsat_transform:
-  frequency: 100
-  delay: 3.0
-  magnetic_declination_radians: 0.0551524  # For lat/long 49.862097999999996 N, 8.6790738 E
-  yaw_offset: 0  # IMU should read 0 for yaw when facing east. If not, set 1.570796327 (== 90°) if is reading 0 facing north
-  zero_altitude: false
-  broadcast_cartesian_transform: false
-  publish_filtered_gps: true
-  use_odometry_yaw: true
-  wait_for_datum: false
-
diff --git a/backpack_pkg/params/gq7.yml b/backpack_pkg/params/gq7.yml
new file mode 100644
index 0000000..f669f76
--- /dev/null
+++ b/backpack_pkg/params/gq7.yml
@@ -0,0 +1,100 @@
+# You should change this section of config to match your setup
+port : '/dev/microstrain_main'
+baudrate : 115200
+
+# This will cause the node to convert any NED measurements to ENU
+# This will also cause the node to convert any vehicle frame measurements to the ROS definition of a vehicle frame
+use_enu_frame : True
+
+# Configure some frame IDs
+frame_id          : 'gq7_link'  # Frame ID of all of the filter messages. Represents the location of the CV7-INS in the tf tree
+map_frame_id      : "map"           # Frame ID of the local tangent plane.
+earth_frame_id    : "earth"         # Frame ID of the global frame
+gnss1_frame_id    : "gq7_gnss_1_antenna_link"
+gnss2_frame_id    : "gq7_gnss_2_antenna_link"
+odometer_frame_id : "gq7_odometer_link"
+target_frame_id   : "base_link"     # Frame ID that we will publish a transform to.
+
+# Disable the transform from the mount to frame id transform as it will be handled in the launch file
+publish_mount_to_frame_id_transform : False
+
+# We will use relative transform mode, meaning that we will publish the following transforms from this node
+#   earth_frame_id -> map_frame_id
+#   map_frame_id -> target_frame_id
+# This helps ROS standard tools consume and display position information produced by the device
+tf_mode: 2
+
+# Enable the RTK dongle interface for communication with the 3DM-RTK.
+# Note: Even if you do not have a 3DM-RTK connected to the aux port, this boolean can remain true
+rtk_dongle_enable : True
+
+# We will use base station relative position configuration for this example. This configuration does a couple things:
+#   We will setup a local tangent plane at the location of the RTK base station being used by the GQ7.
+#   We will publish this location as the transform from the earth_frame_id to map_frame_id frame
+# Note: This configuration requires a 3DM-RTK to be connected to the aux port of the GQ7.
+#       If you do not have that device, see https://wiki.ros.org/microstrain_inertial_driver/relative_position_configuration for more options.
+filter_relative_position_config : True
+filter_relative_position_source : 2
+
+# Set the antenna offsets.
+# Note: These should be changed for you setup, otherwise dual antenna heading will not initialize.
+gnss1_antenna_offset : [0.0, -0.7, -1.0]
+gnss2_antenna_offset : [0.0, 0.7, -1.0]
+
+# This will set the heading alignment to be dual antenna
+filter_auto_heading_alignment_selector : 1
+
+# This will contain the raw IMU data, NOT the filtered IMU data
+imu_data_rate : 100
+
+# The default is to publish LLH position and velocity, but rviz has a hard time displaying that.
+# Instead we will publish the position in the ECEF frame
+gnss1_llh_position_data_rate   : 0
+gnss1_velocity_data_rate       : 0
+gnss1_odometry_earth_data_rate : 2
+gnss2_llh_position_data_rate   : 0
+gnss2_velocity_data_rate       : 0
+gnss2_odometry_earth_data_rate : 2
+
+# We will only publish the odometry messages from the filter in this example.
+# Also publish the human readable message which can be echoed from the command line
+filter_human_readable_status_data_rate : 1
+filter_odometry_earth_data_rate        : 10
+filter_odometry_map_data_rate          : 10
+
+# Turning on this will cause the transform between gq7_link and gnss_1_antenna_link/gnss_2_antenna_link to be updated from the filter
+mip_filter_multi_antenna_offset_correction_data_rate : 2
+
+#################################################################################################################
+# Optional odometer config. Useful if using a wheeled vehicle with a wheel encoder connected to the GQ7 over GPIO
+# This configuration will not cause any issues if you do not have an odometer connected
+#################################################################################################################
+
+# Enable the hardware odometer
+enable_hardware_odometer : True
+odometer_scaling         : 1.0
+odometer_uncertainty     : 0.15
+
+# Allow the filter to use the hardware odometer in it's estimates
+filter_enable_odometer_aiding : True
+
+# For this example, we will assume that we are in a wheeled vehicle such as a car.
+# Setting this option will give us better heading performance especially in GNSS outages
+# If you are operating in a non wheeled vehicle make sure to disable this
+filter_enable_wheeled_vehicle_constraint : True
+
+# Set the location of the odometer
+# Note: These should be changed for you setup
+filter_speed_lever_arm : [0.0, 0.0, 0.0]
+
+# This GPIO config is specific to this setup, and requires that you use GPIO pins 1 and 2 for encoder A and B respectively.
+# You should update for your setup appropriately
+gpio_config    : True
+
+gpio1_feature  : 3  # ENCODER feature
+gpio1_behavior : 1  # ENCODER_A behavior
+gpio1_pin_mode : 0  # NONE pin_mode
+
+gpio2_feature  : 3  # ENCODER feature
+gpio2_behavior : 2  # ENCODER_B behavior
+gpio2_pin_mode : 0  # NONE pin_mode
\ No newline at end of file
diff --git a/backpack_pkg/params/microstain_config.yml b/backpack_pkg/params/microstrain_config.yml
similarity index 91%
rename from backpack_pkg/params/microstain_config.yml
rename to backpack_pkg/params/microstrain_config.yml
index 2076c29..4a434f4 100644
--- a/backpack_pkg/params/microstain_config.yml
+++ b/backpack_pkg/params/microstrain_config.yml
@@ -28,12 +28,12 @@ set_baud : False
 #
 # filter_frame_id and filter_child_frame_id are specifically useful as the node will also publish a transform to the /tf topic
 # that contains the transform between these two frames. Many ROS tools such as RViz will use the /tf topic to display things like robot position.
-imu_frame_id          : "imu_link_raw"
+imu_frame_id          : "gq7_link"
 gnss1_frame_id        : "gnss1_link"
 gnss2_frame_id        : "gnss2_link"
-filter_frame_id       : "imu_link"
-filter_child_frame_id : "imu_link_raw"
-nmea_frame_id         : "nmea"
+filter_frame_id       : "map"
+filter_child_frame_id : "gq7_odometer_link"
+nmea_frame_id         : "earth"
 
 # Waits for a configurable amount of time until the device exists
 # If poll_max_tries is set to -1 we will poll forever until the device exists
@@ -41,6 +41,13 @@ poll_port      : False
 poll_rate_hz   : 1.0
 poll_max_tries : 60
 
+# Number of times to attempt to reconnect to the device if it disconnects while running
+# If configure_after_reconnect is true, we will also reconfigure the device after we reconnect
+#      NOTE: It is strongly recommended to configure after reconnect unless device_setup is set to false
+#            as the device will likely initialize in a different state otherwise
+reconnect_attempts : 5
+configure_after_reconnect : True
+
 # Controls if the driver outputs data with-respect-to ENU frame
 #      false - position, velocity, and orientation are WRT the NED frame (native device frame)
 #      true  - position, velocity, and orientation are WRT the ENU frame
@@ -54,18 +61,18 @@ device_setup : True
 # Controls if the driver-defined settings are saved
 #      false - Do not save the settings
 #      true  - Save the settings in the device's non-volatile memory
-save_settings : False
+save_settings : True
 
 # Controls if the driver uses the device generated timestamp (if available) for timestamping messages
 #      false - Use PC received time for timestamping
 #      true  - Use device generated timestamp
-use_device_timestamp : False
+use_device_timestamp : True
 
 # Controls if the driver uses ROS time for timestamping messages. Can be useful when running under simulation.
 # NOTE: This can be used in conjunction with use_device_timestamp in which case, the device timestmap will be used for sensor_msgs/TimeReference messages, and ROS time will be used for all other messages
 #       false - Use PC received time for timestamping
 #       true  - Use ROS time for timestamping all non sensor_msgs/TimeReference messages
-use_ros_time : False
+use_ros_time : True
 
 # Controls if the driver creates a raw binary file
 #      false - Do not create the file
@@ -94,7 +101,7 @@ raw_file_directory : "/home/your_name"
 # ****************************************************************** 
 # IMU Settings 
 # ****************************************************************** 
-imu_data_rate : 100
+imu_data_rate : 200
 
 # The speed at which the individual IMU publishers will publish at.
 #      If set to -1, will use imu_data_rate to determine the rate at which to stream and publish
@@ -107,9 +114,9 @@ imu_overrange_status_data_rate : -1  # Rate of imu/overrange_status topic
 
 # Static IMU message covariance values (the device does not generate these) 
 # Since internally these are std::vector we need to use the rosparam tags 
-imu_orientation_cov : [0.01, 0, 0, 0, 0.01, 0, 0, 0, 0.01]
-imu_linear_cov      : [0.01, 0, 0, 0, 0.01, 0, 0, 0, 0.01]
-imu_angular_cov     : [0.01, 0, 0, 0, 0.01, 0, 0, 0, 0.01]
+imu_orientation_cov : [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
+imu_linear_cov      : [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
+imu_angular_cov     : [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
 
 # ****************************************************************** 
 # GNSS Settings (only applicable for devices with GNSS) 
@@ -134,7 +141,7 @@ gnss1_rf_error_detection_data_rate : -1  # Rate of gnss1/rf_error_detection topi
 #     For GQ7 - in the vehicle frame wrt IMU origin (meters)
 #     For all other models - in the IMU frame wrt IMU origin (meters)
 #     Note: Make this as accurate as possible for good performance
-gnss1_antenna_offset : [0.0, 0.16, 0.86]
+gnss1_antenna_offset : [-0.015, -0.12, -0.85]
 
 # GNSS2 settings are only applicable for multi-GNSS systems (e.g. GQ7) 
 gnss2_data_rate : 2
@@ -154,7 +161,12 @@ gnss2_rf_error_detection_data_rate : -1  # Rate of gnss1/rf_error_detection topi
 
 # Antenna #2 lever arm offset vector (In the vehicle frame wrt IMU origin (meters) )
 # Note: Make this as accurate as possible for good performance 
-gnss2_antenna_offset : [0.0, -0.16, 0.86]
+gnss2_antenna_offset : [-0.015, 0.12, -0.85]
+
+# GNSS signal configuration
+gnss_glonass_enable : True
+gnss_galileo_enable : True
+gnss_beidou_enable  : True
 
 # (GQ7 Only) Enable RTK dongle interface
 # Note: Enabling this will cause the node to publish on one of two topics depending
@@ -183,7 +195,7 @@ publish_nmea   : False
 # ****************************************************************** 
 # Kalman Filter Settings (only applicable for devices with a Kalman Filter) 
 # ****************************************************************** 
-filter_data_rate : 100
+filter_data_rate : 500
 
 # The speed at which the individual Filter publishers will publish at.
 #      If set to -1, will use filter_data_rate to determine the rate at which to stream and publish
@@ -191,14 +203,18 @@ filter_data_rate : 100
 #
 #      Note: The parameters' "filter_odom_data_rate", "filter_imu_data_rate", "filter_relative_odom_data_rate" associated ROS messages share several MIP fields,
 #            so if more than one is set to stream, and they are set to stream at different rates, messages will be published on both topics, at the higher rate
+#      Note: The parameters' "filter_navsatfix_data_rate", and "filter_aiding_status_data_rate" associated ROS messages share several MIP fields,
+#            so if more than one is set to stream, and they are set to stream at different rates, messages will be published on both topics, at the higher rate
 filter_status_data_rate                     : -1  # Rate of nav/status topic
 filter_heading_data_rate                    : -1  # Rate of nav/heading topic
 filter_heading_state_data_rate              : -1  # Rate of nav/heading_state topic
+filter_navsatfix_data_rate                  : -1  # Rate of nav/fix topic
 filter_odom_data_rate                       : -1  # Rate of nav/odom topic
 filter_imu_data_rate                        : -1  # Rate of nav/filtered_imu/data
 filter_relative_odom_data_rate              : -1  # Rate of nav/relative_pos topic and the transform between filter_frame_id and filter_child_frame_id
                                                   # Note that this data rate will also control the rate at which the transform between "filter_frame_id" and "filter_child_frame_id" will be published at
 filter_aiding_status_data_rate              : -1  # Rate of gnss1/aiding_status and gnss2/aiding_status topics
+filter_antenna_offset_correction_data_rate  : -1  # Rate of gnss1/antenna_offset_corection and gnss2/antenna_offset_correction topics
 filter_gnss_dual_antenna_data_rate          : -1  # Rate of nav/dual_antenna_status topic
 filter_aiding_measurement_summary_data_rate : -1  # Rate of nav/aiding_summary topic
 
@@ -268,10 +284,11 @@ gps_leap_seconds                       : 18.0
 #      Notes: This subscription will be disabled if enable_hardware_odometer is set to true
 filter_external_speed_topic : "/external_speed"
 
+# NOTE: This actually rotates the velocity into the sensor frame, not the vehicle frame.
 # Velocity frame control.
 #      If set to false, the velocity will be reported in the NED/ENU frame
 #      If set to true, the velocity will be reported in the vehicle frame
-filter_vel_in_vehicle_frame : False
+filter_vel_in_vehicle_frame : True
 
 # (GQ7 Only) Reference point lever arm offset control.
 #   Note: This offset will affect the position and velocity measurements in the following topics: nav/odom, nav/relative_pos/odom
@@ -363,7 +380,7 @@ gpio4_pin_mode : 0
 #   2 - WGS84 Latitude, Longitude, height above ellipsoid position, NED velocity and attitude
 filter_init_condition_src              : 0
 filter_auto_heading_alignment_selector : 0,1
-filter_init_reference_frame            : 2
+filter_init_reference_frame            : 1
 filter_init_position : [0.0, 0.0, 0.0]
 filter_init_velocity : [0.0, 0.0, 0.0]
 filter_init_attitude : [0.0, 0.0, 0.0]
@@ -373,11 +390,16 @@ filter_init_attitude : [0.0, 0.0, 0.0]
 #   1 - Relative ECEF position
 #   2 - Relative LLH position
 #
+#   Source =
+#   0 - Position will be reported relative to the base station. filter_relative_position_ref will be ignored
+#   1 - Position will be reported relative to filter_relative_position_ref
+#
 #   Reference position - Units provided by reference frame (ECEF - meters, LLH - deg, deg, meters)
 #   Note: prior to firmware version 1.0.06 this command will fail for non-positive heights.  1.0.06 fixes this)
-filter_relative_position_config : False
-filter_relative_position_frame  : 2
-filter_relative_position_ref    : [0, 0, 0.01]
+filter_relative_position_config : True
+filter_relative_position_frame  : 1
+filter_relative_position_source : 2
+filter_relative_position_ref    : [0.0, 0.0, 0.01]
 
 # (GQ7 only) Speed Lever Arm Configuration
 #   Lever Arm - In vehicle reference frame (meters)
@@ -392,11 +414,11 @@ filter_enable_wheeled_vehicle_constraint : False
 #    Note: When enabled and no valid GNSS measurements are available, the filter uses the accelerometers to track
 #          pitch and roll under the assumption that the sensor platform is not undergoing linear acceleration.
 #          This constraint is useful to maintain accurate pitch and roll during GNSS signal outages.
-filter_enable_vertical_gyro_constraint : False
+filter_enable_vertical_gyro_constraint : True
 
 # (GQ7 only) GNSS Antenna Calibration Control
 # Note: When enabled, the filter will enable lever arm error tracking, up to the maximum offset specified in meters.
-filter_enable_gnss_antenna_cal     : False
+filter_enable_gnss_antenna_cal     : True
 filter_gnss_antenna_cal_max_offset : 0.1
 
 # (GQ7/CV7 only) PPS Source
@@ -428,7 +450,7 @@ nmea_message_allow_duplicate_talker_ids: False
 # NMEA messages in the sensor (IMU) descriptor set
 # In order to enable a message, set nmea_message_config to true, and then change the 'data_rate' of the sentences you want to the desired rate in hertz
 # Note: In order to publish, 'publish_nmea' must also be set to True
-imu_nmea_prkr_data_rate: 0
+imu_nmea_prkr_data_rate: 500
 
 # NMEA messages in the GNSS1 descriptor set
 # In order to enable a message, set nmea_message_config to true, and then change the 'data_rate' of the sentences you want to the desired rate in hertz
diff --git a/backpack_pkg/params/stcamera_node.yaml b/backpack_pkg/params/stcamera_node.yaml
new file mode 100644
index 0000000..17a7c93
--- /dev/null
+++ b/backpack_pkg/params/stcamera_node.yaml
@@ -0,0 +1,65 @@
+# camera_to_connect: [values]
+# values (case sensitive):
+#   ["all"]                : connect to all detected camera
+#   [<MODEL(SERIAL)>, ...] : connect to only the specified camera with 
+#                          <MODEL(SERIAL)>. Model with serial number SN
+#   [<ID>, ...]          : connect to only the specified camera with ID of ID 
+#                          (U3V GUID or hardware address of GigEVision camera)
+# default value is empty (not specified).
+#
+# To connect to multiple cameras, separate the values with whitespace.
+# If no value is specified, the first found camera will be used and the 
+# camera ID is used as camera namespace to identify the camera.
+# If any value is specified, the specified name (either Model(SN) or ID) is used
+# as camera namespace to identify the camera.
+#
+# The format of the camera namespace for the topics and services correspond to
+# the connected camera is as follows:
+# dev_{camera_id} or dev_{camera_model_serial_}, where:
+# {camera_id} or {camera_model_serial_} are either camera ID or MODEL(SERIAL)
+# with non-alphanumeric characters replaced with underscore "_".
+# 
+# Example usage:
+# camera_to_connect : ["all"]
+# #namespace would be dev_{camera_id} of the connected camera.
+#
+# camera_to_connect : ["STC-MCS510U3V(00C0XXX)", "STC_SCS241POE(15DBXXX)"]
+# #namespace would be: dev_STC_MCS510U3V_00C0XXX_ and STC_SCS241POE_15DBXXX_
+#
+# camera_to_connect : ["142100030510", "00:11:1c:f6:yy:xx"]
+# #namespace would be: dev_142100030510 and dev_00_11_1c_f6_yy_xx
+#
+# camera_to_connect : ["STC-MCS510U3V(00C0XX)", "00:11:1c:f6:yy:xx"]
+# #namespace would be: dev_STC_MCS510U3V_00C0XXX_ and dev_00_11_1c_f6_yy_xx
+
+camera_to_connect : ["all"]
+
+
+default_calibration_file: file:///home/deepforest/catkin_ws/src/backpack_pkg/params/cameracalib.yml
+# value:
+#   <full path to the calibration file for any connected camera>
+# default value is empty (not specified).
+#
+# if not specified, no calibration is performed.
+# Please refer to http://wiki.ros.org/camera_calibration for the detail process 
+# for performing calibration.
+# Tutorial for generating calibration file and rectifying image:
+#   http://wiki.ros.org/camera_calibration/Tutorials/MonocularCalibration
+#   http://wiki.ros.org/camera_calibration_parsers
+#   http://wiki.ros.org/image_proc
+# Example usage:
+#default_calibration_file: file:///home/my_data/calibration/calibration.xml
+
+
+# {camera_namespace}/calibration_file: value 
+# {camera_namespace} is determined based on the camera_to_connect values. Please
+# refer to the camera_to_connect description above.
+# value:
+#   The same format as default_calibration_file. 
+# default value is empty (not specified).
+#
+# If not specified, default_calibration_file is used.
+#
+# Example usage:
+#dev_STC_MCS510U3V_00C0XXX_/calibration_file: file:///home/my_data/calibration/calibration_1.yaml
+#dev_00_11_1c_f6_yy_xx/calibration_file: file:///home/my_data/calibration/calibration_2.xml
diff --git a/backpack_pkg/src/cam_off.py b/backpack_pkg/src/cam_off.py
deleted file mode 100644
index 8eb5d41..0000000
--- a/backpack_pkg/src/cam_off.py
+++ /dev/null
@@ -1,23 +0,0 @@
-import requests
-
-import rospy
-
-
-
-def end_capture():
-        rospy.loginfo("Camera is ending auto capture.")
-        stop_capture = requests.post("http://192.168.1.83/timer?enable=false")
-        print(stop_capture.json())
-        shutdown = requests.post("http://192.168.1.83/powerdownready?power_down=true")
-        print(shutdown.json())   
-
-if __name__ == '__main__':
-  try:
-        end_capture()      
-
-  except rospy.ROSInterruptException: 
-
-        pass
-
- 
- 
diff --git a/backpack_pkg/src/cam_start_capture.py b/backpack_pkg/src/cam_start_capture.py
deleted file mode 100644
index 47c791e..0000000
--- a/backpack_pkg/src/cam_start_capture.py
+++ /dev/null
@@ -1,35 +0,0 @@
-import requests
-
-from argparse import ArgumentParser
-
-class StartEndCam:
-
-    parser = ArgumentParser()
-    parser.add_argument("--autocapture", type=bool, help="Choose if camera auto capture mode starts or stops", default=False)
-    parser.add_argument("--shutdown", type=bool, help="Choose if camera should prepare for shutdown (detaches storage)", default=False)
-    args = parser.parse_args()
-    print(args.autocapture, args.shutdown)
-
-    if args.autocapture == True:
-        # Post a message to the camera commanding a capture, block until complete
-        capture_params = { 'store_capture' : True, 'block' : True, 'use_post_capture_state': True , 'preview': True} ## use cached jpeg to show in rviz
-        config_params = { 'auto_cap_mode' : 'timer', 'timer_period' : 1.0}
-        requests.post("http://192.168.1.83/config", json=config_params)
-        requests.post("http://192.168.1.83/capture", json=capture_params)
-        capture = requests.post("http://192.168.1.83/timer?enable=true")
-        print(capture.json())
-        while True:
-            status = requests.get("http://192.168.1.83/status")
-            print(status.json())
-
-    else:
-        stop_capture = requests.post("http://192.168.1.83/timer?enable=false")
-        print(stop_capture.json())
-        
-    if args.shutdown ==True:    
-        shutdown = requests.post("http://192.168.1.83/powerdownready?power_down=true")
-        print(shutdown.json())
-    else: 
-        pass
-
-
diff --git a/backpack_pkg/src/overwrite_parameters_from_file.py b/backpack_pkg/src/overwrite_parameters_from_file.py
new file mode 100644
index 0000000..53546e6
--- /dev/null
+++ b/backpack_pkg/src/overwrite_parameters_from_file.py
@@ -0,0 +1,84 @@
+#!/usr/bin/env python
+
+"""
+
+ This sample shows how to save/load camera setting with using featureBag.
+
+ The following points will be demonstrated in this sample code:
+
+ - Initialize StApi
+
+ - Connect to camera
+
+ - Save/load camera setting to/from file
+
+ - Apply the loaded setting to camera
+
+"""
+
+import os
+import stapipy as st
+
+file_path = os.path.join(os.path.dirname(__file__), '..', '..', 'camera_UserSet1.cfg')
+
+def set_enumeration(nodemap, enum_name, entry_name):
+    enum_node = st.PyIEnumeration(nodemap.get_node(enum_name))
+    entry_node = st.PyIEnumEntry(enum_node[entry_name])
+    enum_node.set_entry_value(entry_node)
+
+try:
+    # Initialize StApi before using.
+    st.initialize()
+
+    # Create a system object for device scan and connection.
+    st_system = st.create_system()
+
+    # Connect to first detected device.
+    st_device = st_system.create_first_device()
+
+    # Display DisplayName of the device.
+    print('Device=', st_device.info.display_name)
+
+    filename = file_path
+
+    # Get the remote nodemap.
+    nodemap = st_device.remote_port.nodemap
+
+    # Create feature bag object.
+    featurebag = st.create_featurebag()
+
+    # Save the current settings to FeatureBag.
+    print("Saving current settings to FeatureBag...")
+    featurebag.store_file_to_bag(filename)
+    print("Saved current settings to FeatureBag")
+
+    
+    # Load the UserSet that is stored in the ROM to the camera.
+    print("Loading settings from ROM to the camera...")
+    nodemap.get_node('UserSetLoad').get().execute()
+    print("Loaded settings from ROM")
+
+    # Load the settings in the FeatureBag to the camera.
+    print("Loading settings from FeatureBag to the camera...")
+    featurebag.load(nodemap)
+    print("Loaded settings from FeatureBag")
+    
+    # Set the UserSet for UserSetSelector.
+    set_enumeration(nodemap, 'UserSetSelector', 'UserSet1')
+
+    # Save the current settings to UserSet1.
+    print("Saving current settings to UserSet1...")
+    nodemap.get_node('UserSetSave').get().execute()
+    print("Saved current settings to UserSet1")
+
+    # Set UserSet1 as the new default.
+    print("Setting UserSet1 as new default...")
+    set_enumeration(nodemap, 'UserSetDefault', 'UserSet1')
+    print("UserSet1 is set as new default")
+
+    # Display all settings.
+    features = featurebag.save_to_string()
+    print(features)
+
+except Exception as exception:
+    print(exception)
diff --git a/backpack_pkg/src/pub_OdomInfo_ros2msc.py b/backpack_pkg/src/pub_OdomInfo_ros2msc.py
deleted file mode 100644
index b2ccb67..0000000
--- a/backpack_pkg/src/pub_OdomInfo_ros2msc.py
+++ /dev/null
@@ -1,196 +0,0 @@
-#!/usr/bin/env python
-
-import requests
-from requests.adapters import HTTPAdapter
-from requests.exceptions import ConnectionError
-
-import datetime
-import math
-import rospy
-from nav_msgs.msg import Odometry
-from std_msgs.msg import String
-
-
-class ros2camera:
-
-    def __init__(self):
-    
-        #for connection with ROS
-        rospy.init_node('inform_camera')
-        self.rate = rospy.Rate(1)
-        
-        #for connection to HTTP
-        self.t = 200  #timeout, to wait until connection is established, in seconds
-        self.adapter = HTTPAdapter(max_retries=5)  # will retry a connection after it failed
-        self.session = requests.Session()
-        self.session.mount("http://192.168.1.83/", self.adapter)
-        try:
-            status = self.session.get("http://192.168.1.83/status", timeout=self.t)
-            rospy.loginfo(str(status.json()))
-            capture_params = { 'store_capture' : True, 'block' : True, 'use_post_capture_state': True , 'preview': True}
-            config_params = { 'auto_cap_mode' : 'timer', 'timer_period' : 1.0,}
-            self.session.post("http://192.168.1.83/config", json=config_params, timeout=self.t)
-            self.session.post("http://192.168.1.83/capture", json=capture_params, timeout=self.t)
-            capture = self.session.post("http://192.168.1.83/timer?enable=true", timeout=self.t)
-            rospy.loginfo("Camera is starting auto capture.")
-            rospy.loginfo(str(capture.json()))
-        except ConnectionError as ce:
-            rospy.logwarn(ce)
-        
-        while not rospy.is_shutdown() :
-          rospy.Subscriber("/nav/odom", Odometry, self.sending2camera)
-          self.pub = rospy.Publisher("/msc/msc_send_status", String, queue_size=10)
-        rospy.on_shutdown(self.end_capture)
-
-
-    def quaternion2euler(self, x,y,z,w):
-        t0 = +2.0 * (w * x + y * z)
-        t1 = +1.0 - 2.0 * (x * x + y * y)
-        roll_x = math.atan2(t0, t1)
-     
-        t2 = +2.0 * (w * y - z * x)
-        t2 = +1.0 if t2 > +1.0 else t2
-        t2 = -1.0 if t2 < -1.0 else t2
-        pitch_y = math.asin(t2)
-     
-        t3 = +2.0 * (w * z + x * y)
-        t4 = +1.0 - 2.0 * (y * y + z * z)
-        yaw_z = math.atan2(t3, t4)
-     
-        return roll_x, pitch_y, yaw_z # in radians
-        
-
-    def sending2camera(self, data):
-      
-        # position
-        latitude = data.pose.pose.position.x
-        latitude = math.radians(latitude)
-        longitude = data.pose.pose.position.y
-        longitude = math.radians(longitude)
-        altitude = data.pose.pose.position.z
-        
-        #orientation
-        x = data.pose.pose.orientation.x
-        y = data.pose.pose.orientation.y
-        z = data.pose.pose.orientation.z
-        w = data.pose.pose.orientation.w
-
-        phi, theta, psi = self.quaternion2euler(x,y,z,w)
-
-        utc_time = datetime.datetime.utcnow()
-        utc_time = "{}-{:02d}-{:02d}T{}:{}:{}.{}".format(utc_time.year, utc_time.month, utc_time.day, utc_time.hour, utc_time.minute, utc_time.second, str(utc_time.microsecond)[:3])
-        
-        
-
-        ################# sending to camera
-        
-        position_params = { 'aircraft_phi' : phi,
-                            'aircraft_theta' : theta,
-                            'aircraft_psi' : psi, 
-                            'camera_phi' : -1.5707963,
-                            'camera_theta' : -1.5707963,
-                            'camera_psi' : 0.0,
-                            'latitude' : latitude, 
-                            'longitude': longitude, 
-                            'altitude': altitude, 
-                            'utc_time' : utc_time
-                            }
-        rospy.loginfo("sending the information to the camera ...")
-        try:
-            capture_data_time = self.session.post("http://192.168.1.83/capture_state", json=position_params, timeout=self.t)
-        except ConnectionError as ce:
-            rospy.logwarn(ce)
-        txt = str(capture_data_time.json())
-        self.pub.publish(txt)
-        txt = None
-
-        
-        # orientation_params = { 'aircraft_phi' : phi,
-        #                     'aircraft_theta' : theta,
-        #                     'aircraft_psi' : psi, 
-        #                     'camera_phi' : -1.5707963,
-        #                     'camera_theta' : -1.5707963,
-        #                     'camera_psi' : 0,
-                            
-        #                     }
-        # #print("sending the information to the camera ...")
-        # capture_data_time = requests.post("http://192.168.1.83/orientation", json=orientation_params)
-        # print(capture_data_time.json())
-
-        try:
-            cam_status = self.session.get("http://192.168.1.83/status", timeout=self.t)
-        except ConnectionError as ce:
-            rospy.logwarn(ce)
-        
-        self.pub.publish("Currently camera SD Storage left: ", str(cam_status.json()["sd_gb_free"])[:7], " / ", str(cam_status.json()["sd_gb_total"])[:7])
-        cam_sd_red = cam_status.json()["sd_warn"]
-        if cam_sd_red == True: 
-          rospy.logwarn("SD Card of red camera almost full!")
-
-        self.rate.sleep()
-        
-    def end_capture(self):
-        rospy.loginfo("Camera is ending auto capture.")
-        try:
-            stop_capture = self.session.post("http://192.168.1.83/timer?enable=false", timeout=self.t)
-            txt = str(stop_capture.json())
-            rospy.loginfo(txt)
-            txt = None
-            
-        except ConnectionError as ce:
-            rospy.logwarn(ce)
-        
-
-if __name__ == '__main__':
-  try:
-        ros2camera()
-
-  except rospy.ROSInterruptException: 
-        pass
-
- 
-  
-
-
-# -----------------
-
-
-'''
-OUTPUT OF /imu/nav/odom 
-
-header: 
-  seq: 1671
-  stamp: 
-    secs: 1675513527    #TIME
-    nsecs: 360754406
-  frame_id: "sensor_wgs84"
-child_frame_id: "sensor"
-pose: 
-  pose: 
-    position: 
-      x: 49.862092389858105   #GPS lat
-      y: 8.679037934163667         #GPS LONG
-      z: 240.8105145785355      # GPS ALT
-    orientation: 
-      x: 0.0018981278408318758
-      y: -0.00014660877059213817
-      z: 0.04816616699099541
-      w: 0.9988375306129456
-  covariance: [3582.8365157387598, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3582.8365157387598, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3582.8365157387598, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.973756077432382e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.9469057083309365e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0059109938767898385]
-twist: 
-  twist: 
-    linear: 
-      x: -0.15309178829193115
-      y: 0.09651438146829605
-      z: 0.2371663600206375
-    angular: 
-      x: 0.00015465420437976718
-      y: 0.0018760506063699722
-      z: -0.0005795593024231493
-  covariance: [5.159766909499922, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 5.154747770559936, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.9731549150776715, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
-
-
-
-'''
-
-
diff --git a/backpack_pkg/src/pub_camGPS_msc2ros.py b/backpack_pkg/src/pub_camGPS_msc2ros.py
deleted file mode 100644
index 5c6c79c..0000000
--- a/backpack_pkg/src/pub_camGPS_msc2ros.py
+++ /dev/null
@@ -1,47 +0,0 @@
-#!/usr/bin/env python
-
-import requests
-
-import rospy
-#from rospy_message_converter import json_message_converter
-from sensor_msgs.msg import NavSatFix
-
-pub = rospy.Publisher('gps', NavSatFix, queue_size=1)
-
-rospy.init_node('msc_gps_data')
-
-# Convert JSON information into ROS messages
-def convertJSON2NavSatFix(data):
-
-    latitude_b = float(data.json()['latitude'])
-    longitude_b = float(data.json()['longitude'])
-    altitude_b = float(data.json()['altitude'])
-
-    return latitude_b, longitude_b, altitude_b
-
-
-def talker():
-    while not rospy.is_shutdown() :
-        gps_data = requests.get('http://192.168.1.83/gps')   #Read the GPS data from the camera
-        json = convertJSON2NavSatFix(gps_data)
-        pub.publish(latitude=json[0], longitude=json[1], altitude=json[2])
-        rospy.sleep(1.0)
-
-
-
-if __name__ == '__main__':
-    try:
-        talker()
-    except rospy.ROSInterruptException: pass
-
-
-#### Raw Output of Camera JSON ###
-
-'''
->> 03.02.2023
-Raw data : {"altitude":0,"fix3d":false,"latitude":0,"longitude":0,"p_acc":0,"utc_time":"1970-01-01T00:00:00Z","utc_time_valid":false,"v_acc":0,"vel_d":0,"vel_e":0,"vel_n":0}
-
-
-'''
-
-
diff --git a/backpack_pkg/src/pub_camInfo_msc2ros.py b/backpack_pkg/src/pub_camInfo_msc2ros.py
deleted file mode 100644
index 5302208..0000000
--- a/backpack_pkg/src/pub_camInfo_msc2ros.py
+++ /dev/null
@@ -1,105 +0,0 @@
-#!/usr/bin/env python
-
-import requests
-from requests.adapters import HTTPAdapter
-from requests.exceptions import ConnectionError
-import rospy
-
-from sensor_msgs.msg import CameraInfo
-
-class reportInfos(): 
-    def __init__(self):
-        
-        pub_1 = rospy.Publisher('/msc/info_band1', CameraInfo, queue_size=1)
-        pub_2 = rospy.Publisher('/msc/info_band2', CameraInfo, queue_size=1)
-        pub_3 = rospy.Publisher('/msc/info_band3', CameraInfo, queue_size=1)
-        pub_4 = rospy.Publisher('/msc/info_band4', CameraInfo, queue_size=1)
-        pub_5 = rospy.Publisher('/msc/info_band5', CameraInfo, queue_size=1)
-        self.pub_list = [pub_1, pub_2, pub_3, pub_4, pub_5]
-        rospy.init_node('msc_bands')
-        
-        self.t = 2  #timeout, to wait until connection is established, in seconds
-        self.adapter = HTTPAdapter(max_retries=5)  # will retry a connection after it failed
-        self.session = requests.Session()
-        self.session.mount("http://192.168.1.83/", self.adapter)
-        
-        # GET camera information and distortion
-        try: 
-            self.capture_infos = self.session.get("http://192.168.1.83/camera_info", timeout=self.t)
-            self.capture_distortion = self.session.get("http://192.168.1.83/calibration/distortion", timeout=self.t)
-            self.talker()
-        except ConnectionError as ce:
-            rospy.logwarn(ce)
-
-        
-    # Convert JSON information into ROS messages
-    def convertJSON2CameraInfo(self,cam_info, cam_dist, bandnumber):
-        '''
-        height_1, width_1: image dimensions of band 1
-        distortion_model_1: distortion model for band 1 used. Supported models are listed in sensor_msgs/distortion_models.h
-        D_plumb_bob_1 = plump blob distortion model for band 1. For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3).
-        intrinsic_1 = Intrinsic camera matrix for the raw (distorted) images.
-                     [fx  0 cx]
-                 K = [ 0 fy cy]
-                     [ 0  0  1]
-                    Projects 3D points in the camera coordinate frame to 2D pixel coordinates using the focal lengths (fx, fy) and principal point (cx, cy).
-    '''
-        b = str(bandnumber)
-        #header_b = capture_infos.json()[b]['bandname']
-        height_b = int(cam_info.json()[b]['image_height'])
-        width_b = int(cam_info.json()[b]['image_width'])
-        distortion_model_b = 'plumb_bob'+'_'+cam_info.json()[b]['bandname']
-        D_plumb_bob_b = [cam_dist.json()[b]['p'][0], cam_dist.json()[b]['p'][1], 
-                        cam_dist.json()[b]['k'][0], cam_dist.json()[b]['k'][1], cam_dist.json()[b]['k'][2]]
-        D_plumb_bob_b = [float(item) for item in D_plumb_bob_b]
-
-        intrinsic_b = [cam_dist.json()[b]['fx'], 0, cam_dist.json()[b]['cx'], 
-                        0, cam_dist.json()[b]['fy'], cam_dist.json()[b]['cy'], 
-                        0,0,1]
-        intrinsic_b = [float(item) for item in intrinsic_b]
-        return height_b, width_b, distortion_model_b, D_plumb_bob_b, intrinsic_b
-
-
-    def talker(self):
-
-        while not rospy.is_shutdown() :
-            for b in range(5):
-                json = self.convertJSON2CameraInfo(self.capture_infos, self.capture_distortion, b+1)
-                self.pub_list[b].publish(height=json[0], width=json[1], distortion_model=json[2], D=json[3], K=json[4])
-            rospy.sleep(10.0)
-
-
-if __name__ == '__main__':
-    
-    try:
-        reportInfos()
-    except rospy.ROSInterruptException:
-        pass
-
-
-#### Raw Output of Camera JSON ###
-'''
->> 03.02.2023
-
--------------camera_info---------------
-{
-'1': {'bandname': 'Blue', 'bandwidth_nm': 32, 'center_nm': 475, 'focal_length_px': 1466.666666666667, 'image_height': 960, 'image_width': 1280, 'type': 'bandpass'}, 
-'2': {'bandname': 'Green', 'bandwidth_nm': 27, 'center_nm': 560, 'focal_length_px': 1466.666666666667, 'image_height': 960, 'image_width': 1280, 'type': 'bandpass'}, 
-'3': {'bandname': 'Red', 'bandwidth_nm': 14, 'center_nm': 668, 'focal_length_px': 1466.666666666667, 'image_height': 960, 'image_width': 1280, 'type': 'bandpass'}, 
-'4': {'bandname': 'NIR', 'bandwidth_nm': 57, 'center_nm': 842, 'focal_length_px': 1466.666666666667, 'image_height': 960, 'image_width': 1280, 'type': 'bandpass'}, 
-'5': {'bandname': 'Red edge', 'bandwidth_nm': 12, 'center_nm': 717, 'focal_length_px': 1466.666666666667, 'image_height': 960, 'image_width': 1280, 'type': 'bandpass'}
-}
-------------calibration/distortion----------------
-{
-'1: {'cx': 632.5075, 'cy': 488.2897, 'fx': 1451.736, 'fy': 1451.7795, 'k': [-0.1047797, 0.1936219, -0.1258823], 'p': [0.0009945083, -0.0002144772]}, 
-'2': {'cx': 628.5667, 'cy': 478.4457, 'fx': 1445.8885, 'fy': 1445.7964, 'k': [-0.1064943, 0.169645, -0.07656262], 'p': [0.001048874, -0.0004467681]}, 
-'3': {'cx': 634.9805, 'cy': 478.7468, 'fx': 1451.3264, 'fy': 1451.1871, 'k': [-0.1100523, 0.1903069, -0.1222212], 'p': [0.0003323937, -0.0008141776]}, 
-'4': {'cx': 642.1021, 'cy': 478.8695, 'fx': 1445.3117, 'fy': 1445.3297, 'k': [-0.1120625, 0.1891504, -0.1246147], 'p': [0.0005723328, -0.0002671686]}, 
-'5': {'cx': 627.0835, 'cy': 490.9755, 'fx': 1445.6292, 'fy': 1445.5382, 'k': [-0.1115623, 0.1922929, -0.1185325], 'p': [0.0005413496, 0.0002610356]}
-}
-
-
-
-'''
-
-
diff --git a/backpack_pkg/src/show_ROM_features.py b/backpack_pkg/src/show_ROM_features.py
new file mode 100644
index 0000000..c0057b6
--- /dev/null
+++ b/backpack_pkg/src/show_ROM_features.py
@@ -0,0 +1,40 @@
+"""
+ This sample shows how to save/load camera setting with using featureBag.
+ The following points will be demonstrated in this sample code:
+ - Initialize StApi
+ - Connect to camera
+ - Save/load camera setting to/from file
+ - Apply the loaded setting to camera
+"""
+
+import os
+import tempfile
+
+import stapipy as st
+
+try:
+    # Initialize StApi before using.
+    st.initialize()
+
+    # Create a system object for device scan and connection.
+    st_system = st.create_system()
+
+    # Connect to first detected device.
+    st_device = st_system.create_first_device()
+
+    # Display DisplayName of the device.
+    print('Device=', st_device.info.display_name)
+
+    # Get the remote nodemap.
+    nodemap = st_device.remote_port.nodemap
+
+    # Create feature bag object.
+    featurebag = st.create_featurebag()
+    featurebag.store_nodemap_to_bag(nodemap)
+
+    # Display all settings.
+    features = featurebag.save_to_string()
+    print(features)
+
+except Exception as exception:
+    print(exception)
diff --git a/backpack_pkg/src/sub_camera_piexif.py b/backpack_pkg/src/sub_camera_piexif.py
new file mode 100644
index 0000000..74ab351
--- /dev/null
+++ b/backpack_pkg/src/sub_camera_piexif.py
@@ -0,0 +1,113 @@
+#!/usr/bin/env python
+
+import rospy
+from sensor_msgs.msg import Image, NavSatFix
+from cv_bridge import CvBridge, CvBridgeError
+import cv2
+import numpy as np
+import os
+from datetime import datetime
+import piexif
+
+class ImageGPSProcessor:
+    def __init__(self, save_path):
+        rospy.init_node('image_gps_processor', anonymous=True)
+
+        # Create folder name for all images
+        folder_time = datetime.now().strftime("%Y-%m-%d_%H-%M")
+        self.save_path = os.path.join(save_path, folder_time)
+        os.makedirs(self.save_path, exist_ok=True)
+        rospy.loginfo(f"Saving images here {str(self.save_path)}.")
+
+        self.bridge = CvBridge()
+
+        # Subscribe to image topic
+        self.image_sub = rospy.Subscriber("/camera/dev_142122508751/image_raw", Image, self.image_callback)
+
+        # Subscribe to GPS topic
+        self.gps_sub = rospy.Subscriber("/nav/fix", NavSatFix, self.gps_callback)
+
+        self.gps_info = None
+
+    def image_callback(self, image_msg):
+        try:
+            # Convert ROS Image message to OpenCV image
+            cv_image = self.bridge.imgmsg_to_cv2(image_msg, desired_encoding="passthrough")
+
+            # Extract timestamp from image message
+            timestamp_sec = image_msg.header.stamp.to_sec()
+
+            # Save the image with a unique filename including milliseconds
+            image_id = image_msg.header.seq
+            image_name = f"image_with_gps_{image_id}.jpg"
+            image_path = os.path.join(self.save_path, image_name)
+
+            # Save image as GeoTIFF
+            self.save_geotiff(cv_image, timestamp_sec, image_path)
+
+        except CvBridgeError as e:
+            rospy.logerr(f"CV Bridge Error: {e}")
+
+    def gps_callback(self, gps_msg):
+        # Store GPS information for later use
+        self.gps_info = gps_msg
+
+    def save_geotiff(self, image, timestamp_sec, image_path):
+        if image is None: 
+            rospy.logwarn("No image received...") 
+        
+        else:
+            # Create the 0th dictionary
+            timestamp_dt = datetime.fromtimestamp(timestamp_sec)
+            zeroth_ifd = {
+                piexif.ImageIFD.Model: b"OMRON SENTECH STC-MBCM401U3V",
+                piexif.ImageIFD.XResolution: (2048, 1),
+                piexif.ImageIFD.YResolution: (2048, 1),
+                piexif.ImageIFD.DateTime: timestamp_dt.strftime("%Y-%m-%d_%H-%M-%S"),
+                piexif.ImageIFD.BitsPerSample: 8,  # Mono8
+                piexif.ImageIFD.ExposureTime: (11116, 1000000),  # microseconds to rational
+            }
+            # Convert dictionary to EXIF format
+            exif_dict = {"0th": zeroth_ifd}
+            
+            if self.gps_info is not None:
+                # Extract GPS information
+                latitude = self.gps_info.latitude
+                longitude = self.gps_info.longitude
+
+                # Create the GPS dictionary
+                gps_ifd = {
+                    piexif.GPSIFD.GPSLatitudeRef: 'N' if latitude >= 0 else 'S',
+                    piexif.GPSIFD.GPSLatitude: self.decimal_degrees_to_rational(abs(latitude)),
+                    piexif.GPSIFD.GPSLongitudeRef: 'E' if longitude >= 0 else 'W',
+                    piexif.GPSIFD.GPSLongitude: self.decimal_degrees_to_rational(abs(longitude)),
+                }
+
+                exif_dict["GPS"] = gps_ifd
+            else:
+                rospy.logwarn("No GPS information available. Image will be saved without GPS metadata.")    
+            
+            exif_bytes = piexif.dump(exif_dict)
+
+            # Save image and insert EXIF data into the image
+            cv2.imwrite(image_path, image)
+            piexif.insert(exif_bytes, image_path)
+
+        
+
+    def decimal_degrees_to_rational(self, decimal_degrees):
+        degrees = int(decimal_degrees)
+        minutes_float = (decimal_degrees - degrees) * 60
+        minutes = int(minutes_float)
+        seconds_float = (minutes_float - minutes) * 60 * 1000  # Multiply by 1000 to get milliseconds
+        seconds = int(seconds_float)
+
+        # Rational format: (numerator, denominator)
+        return [(degrees, 1), (minutes, 1), (seconds, 1000)]
+
+    def run(self):
+        rospy.spin()
+
+if __name__ == '__main__':
+    image_gps_processor = ImageGPSProcessor(save_path="/home/deepforest/catkin_ws/src/backpack_pkg/imagefiles")
+    image_gps_processor.run()
-- 
GitLab