From 89fe420b114f5f7f3897c991612919d6120fee9f Mon Sep 17 00:00:00 2001 From: gabor-kovacs Date: Wed, 30 Nov 2022 17:39:55 +0100 Subject: [PATCH] added static camera position config option --- .../src/zed_camera/include/zed_camera_component.hpp | 1 + zed_components/src/zed_camera/src/zed_camera_component.cpp | 5 +++++ zed_wrapper/config/common.yaml | 1 + 3 files changed, 7 insertions(+) diff --git a/zed_components/src/zed_camera/include/zed_camera_component.hpp b/zed_components/src/zed_camera/include/zed_camera_component.hpp index f8c2fba3..3fd17d5b 100644 --- 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 index 589b8f96..6f52ae22 100644 --- 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 index ca6c508a..d43a218d 100644 --- 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]