Merge pull request #122 from gabor-kovacs/master

added static camera position config option
stereolabs · Nov 30, 2022 · 72ebe20 · 72ebe20
2 parents 60e5b66 + 89fe420
commit 72ebe20
Show file tree

Hide file tree

Showing 3 changed files with 7 additions and 0 deletions.
diff --git a/zed_components/src/zed_camera/include/zed_camera_component.hpp b/zed_components/src/zed_camera/include/zed_camera_component.hpp
@@ -343,6 +343,7 @@ class ZedCamera : public rclcpp::Node
   double mPathPubRate = 2.0;
   double mTfOffset = 0.05;
   double mPosTrackDepthMinRange = 0.0;
+  bool mSetAsStatic = false;
   bool mSetGravityAsOrigin = false;
   int mPathMaxCount = -1;
   bool mPublishPoseCov = true;

diff --git a/zed_components/src/zed_camera/src/zed_camera_component.cpp b/zed_components/src/zed_camera/src/zed_camera_component.cpp
@@ -1048,6 +1048,10 @@ void ZedCamera::getPosTrackingParams()
   getParam(
     "pos_tracking.area_memory_db_path", mAreaMemoryDbPath, mAreaMemoryDbPath,
     " * Area Memory DB: ");
+  getParam("pos_tracking.set_as_static", mSetAsStatic, mSetAsStatic);
+  RCLCPP_INFO_STREAM(
+      get_logger(),
+      " * Camera is static: " << (mSetAsStatic ? "TRUE" : "FALSE"));
   getParam("pos_tracking.set_gravity_as_origin", mSetGravityAsOrigin, mSetGravityAsOrigin);
   RCLCPP_INFO_STREAM(
     get_logger(),
@@ -2771,6 +2775,7 @@ bool ZedCamera::startPosTracking()
   trackParams.initial_world_transform = mInitialPoseSl;
   trackParams.set_floor_as_origin = mFloorAlignment;
   trackParams.depth_min_range = mPosTrackDepthMinRange;
+  trackParams.set_as_static = mSetAsStatic;
   trackParams.set_gravity_as_origin = mSetGravityAsOrigin;
 
   sl::ERROR_CODE err = mZed.enablePositionalTracking(trackParams);

diff --git a/zed_wrapper/config/common.yaml b/zed_wrapper/config/common.yaml
@@ -70,6 +70,7 @@
             area_memory_db_path: ""
             area_memory: true # Enable to detect loop closure
             depth_min_range: 0.0 # Set this value for removing fixed zones of the robot in the FoV of the camerafrom the visual odometry evaluation
+            set_as_static: false # If 'true' the camera will be static and not move in the environment
             set_gravity_as_origin: false # If 'true' align the positional tracking world to imu gravity measurement. Keep the yaw from the user initial pose.
             floor_alignment: false # Enable to automatically calculate camera/floor offset
             initial_base_pose: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Initial position of the `base_frame` -> [X, Y, Z, R, P, Y]