From 9bc3b8395721c7e924937d625e8ee9028324e1bd Mon Sep 17 00:00:00 2001 From: flipflip Date: Thu, 4 Jun 2026 09:25:38 +0200 Subject: [PATCH] update FP_A-ODOMSTATUS --- fixposition-sdk | 2 +- .../fixposition_driver_msgs/data_to_ros.hpp | 14 +++++++------- fixposition_driver_msgs/msg/FpaConsts.msg | 4 ++-- fixposition_driver_msgs/msg/FpaOdomstatus.msg | 4 +++- fixposition_driver_ros1/src/data_to_ros1.cpp | 2 ++ fixposition_driver_ros2/src/data_to_ros2.cpp | 2 ++ 6 files changed, 17 insertions(+), 11 deletions(-) diff --git a/fixposition-sdk b/fixposition-sdk index bd34a92b..89b0495d 160000 --- a/fixposition-sdk +++ b/fixposition-sdk @@ -1 +1 @@ -Subproject commit bd34a92b25284c4e2b84e3a345da0c9927dc0d79 +Subproject commit 89b0495d454e72ab779e86086635eb400199ba72 diff --git a/fixposition_driver_msgs/include/fixposition_driver_msgs/data_to_ros.hpp b/fixposition_driver_msgs/include/fixposition_driver_msgs/data_to_ros.hpp index 7687879c..1ace8375 100644 --- a/fixposition_driver_msgs/include/fixposition_driver_msgs/data_to_ros.hpp +++ b/fixposition_driver_msgs/include/fixposition_driver_msgs/data_to_ros.hpp @@ -269,13 +269,13 @@ template inline int FpaCamStatusToMsg(const RosMsgT& msg, const fpsdk::common::parser::fpa::FpaCamStatus cam_status) { // clang-format off switch (cam_status) { - case fpsdk::common::parser::fpa::FpaCamStatus::UNSPECIFIED: return msg.consts.CAM_STATUS_UNSPECIFIED; - case fpsdk::common::parser::fpa::FpaCamStatus::CAM_UNAVL: return msg.consts.CAM_STATUS_CAM_UNAVL; - case fpsdk::common::parser::fpa::FpaCamStatus::BAD_FEAT: return msg.consts.CAM_STATUS_BAD_FEAT; - case fpsdk::common::parser::fpa::FpaCamStatus::RESERVED2: return msg.consts.CAM_STATUS_RESERVED2; - case fpsdk::common::parser::fpa::FpaCamStatus::RESERVED3: return msg.consts.CAM_STATUS_RESERVED3; - case fpsdk::common::parser::fpa::FpaCamStatus::RESERVED4: return msg.consts.CAM_STATUS_RESERVED4; - case fpsdk::common::parser::fpa::FpaCamStatus::GOOD: return msg.consts.CAM_STATUS_GOOD; + case fpsdk::common::parser::fpa::FpaCamStatus::UNSPECIFIED: return msg.consts.CAM_STATUS_UNSPECIFIED; + case fpsdk::common::parser::fpa::FpaCamStatus::CAM_UNAVL: return msg.consts.CAM_STATUS_CAM_UNAVL; + case fpsdk::common::parser::fpa::FpaCamStatus::BAD_FEAT: return msg.consts.CAM_STATUS_BAD_FEAT; + case fpsdk::common::parser::fpa::FpaCamStatus::NOT_CONVERGED: return msg.consts.CAM_STATUS_NOT_CONVERGED; + case fpsdk::common::parser::fpa::FpaCamStatus::WARMSTARTED: return msg.consts.CAM_STATUS_WARMSTARTED; + case fpsdk::common::parser::fpa::FpaCamStatus::RESERVED4: return msg.consts.CAM_STATUS_RESERVED4; + case fpsdk::common::parser::fpa::FpaCamStatus::GOOD: return msg.consts.CAM_STATUS_GOOD; } // clang-format on return msg.consts.CAM_STATUS_UNSPECIFIED; diff --git a/fixposition_driver_msgs/msg/FpaConsts.msg b/fixposition_driver_msgs/msg/FpaConsts.msg index 0093102b..b5f13523 100644 --- a/fixposition_driver_msgs/msg/FpaConsts.msg +++ b/fixposition_driver_msgs/msg/FpaConsts.msg @@ -99,8 +99,8 @@ int8 CORR_STATUS_RESERVED9 = 9 # Reserved int8 CAM_STATUS_UNSPECIFIED = -1 # Unspecified int8 CAM_STATUS_CAM_UNAVL = 0 # Camera not available int8 CAM_STATUS_BAD_FEAT = 1 # Camera available, but not usable (e.g. too dark) -int8 CAM_STATUS_RESERVED2 = 2 # Reserved -int8 CAM_STATUS_RESERVED3 = 3 # Reserved +int8 CAM_STATUS_NOT_CONVERGED = 2 # Camera not converged +int8 CAM_STATUS_WARMSTARTED = 3 # Camera warmstarted (previously converged) int8 CAM_STATUS_RESERVED4 = 4 # Reserved int8 CAM_STATUS_GOOD = 5 # Camera working and available diff --git a/fixposition_driver_msgs/msg/FpaOdomstatus.msg b/fixposition_driver_msgs/msg/FpaOdomstatus.msg index f6b3b2a1..b347a619 100644 --- a/fixposition_driver_msgs/msg/FpaOdomstatus.msg +++ b/fixposition_driver_msgs/msg/FpaOdomstatus.msg @@ -9,7 +9,8 @@ int8 fusion_imu # Fusion measurement status: IMU, s int8 fusion_gnss1 # Fusion measurement status: GNSS 1, see consts.MEAS_STATUS_... int8 fusion_gnss2 # Fusion measurement status: GNSS 2, see consts.MEAS_STATUS_... int8 fusion_corr # Fusion measurement status: GNSS corrections, see consts.MEAS_STATUS_... -int8 fusion_cam1 # Fusion measurement status: camera, see consts.MEAS_STATUS_... +int8 fusion_cam1 # Fusion measurement status: camera 1, see consts.MEAS_STATUS_... +int8 fusion_cam2 # Fusion measurement status: camera 2, see consts.MEAS_STATUS_... int8 fusion_ws # Fusion measurement status: wheelspeed, see consts.MEAS_STATUS_... int8 fusion_markers # Fusion measurement status: markers, see consts.MEAS_STATUS_... int8 imu_status # IMU bias status, see consts.IMU_STATUS_... @@ -20,6 +21,7 @@ int8 gnss2_status # GNSS 2 fix status, see consts.GNS int8 baseline_status # Baseline status, see consts.BASELINE_STATUS_... int8 corr_status # GNSS correction status, see consts.CORR_STATUS_... int8 cam1_status # Camera 1 status, see consts.CAM_STATUS_... +int8 cam2_status # Camera 2 status, see consts.CAM_STATUS_... int8 ws_status # Wheelspeed status, see consts.WS_STATUS_... int8 ws_conv # Wheelspeed convergence status, see consts.WS_CONV_... int8 markers_status # Markers status, see consts.MARKERS_STATUS_... diff --git a/fixposition_driver_ros1/src/data_to_ros1.cpp b/fixposition_driver_ros1/src/data_to_ros1.cpp index 84bbdfeb..0731d3a5 100644 --- a/fixposition_driver_ros1/src/data_to_ros1.cpp +++ b/fixposition_driver_ros1/src/data_to_ros1.cpp @@ -236,6 +236,7 @@ static void FpaOdomstatusToMsg(const fpa::FpaOdomstatusPayload& payload, fixposi msg.fusion_gnss2 = FpaMeasStatusToMsg(msg, payload.fusion_gnss2); msg.fusion_corr = FpaMeasStatusToMsg(msg, payload.fusion_corr); msg.fusion_cam1 = FpaMeasStatusToMsg(msg, payload.fusion_cam1); + msg.fusion_cam2 = FpaMeasStatusToMsg(msg, payload.fusion_cam2); msg.fusion_ws = FpaMeasStatusToMsg(msg, payload.fusion_ws); msg.fusion_markers = FpaMeasStatusToMsg(msg, payload.fusion_markers); msg.imu_status = FpaImuStatusToMsg(msg, payload.imu_status); @@ -246,6 +247,7 @@ static void FpaOdomstatusToMsg(const fpa::FpaOdomstatusPayload& payload, fixposi msg.baseline_status = FpaBaselineStatusToMsg(msg, payload.baseline_status); msg.corr_status = FpaCorrStatusToMsg(msg, payload.corr_status); msg.cam1_status = FpaCamStatusToMsg(msg, payload.cam1_status); + msg.cam2_status = FpaCamStatusToMsg(msg, payload.cam2_status); msg.ws_status = FpaWsStatusToMsg(msg, payload.ws_status); msg.ws_conv = FpaWsConvToMsg(msg, payload.ws_conv); msg.markers_status = FpaMarkersStatusToMsg(msg, payload.markers_status); diff --git a/fixposition_driver_ros2/src/data_to_ros2.cpp b/fixposition_driver_ros2/src/data_to_ros2.cpp index 94c2b696..5af16d2e 100644 --- a/fixposition_driver_ros2/src/data_to_ros2.cpp +++ b/fixposition_driver_ros2/src/data_to_ros2.cpp @@ -241,6 +241,7 @@ static void FpaOdomstatusToMsg(const fpa::FpaOdomstatusPayload& payload, fpmsgs: msg.fusion_gnss2 = FpaMeasStatusToMsg(msg, payload.fusion_gnss2); msg.fusion_corr = FpaMeasStatusToMsg(msg, payload.fusion_corr); msg.fusion_cam1 = FpaMeasStatusToMsg(msg, payload.fusion_cam1); + msg.fusion_cam2 = FpaMeasStatusToMsg(msg, payload.fusion_cam2); msg.fusion_ws = FpaMeasStatusToMsg(msg, payload.fusion_ws); msg.fusion_markers = FpaMeasStatusToMsg(msg, payload.fusion_markers); msg.imu_status = FpaImuStatusToMsg(msg, payload.imu_status); @@ -251,6 +252,7 @@ static void FpaOdomstatusToMsg(const fpa::FpaOdomstatusPayload& payload, fpmsgs: msg.baseline_status = FpaBaselineStatusToMsg(msg, payload.baseline_status); msg.corr_status = FpaCorrStatusToMsg(msg, payload.corr_status); msg.cam1_status = FpaCamStatusToMsg(msg, payload.cam1_status); + msg.cam2_status = FpaCamStatusToMsg(msg, payload.cam2_status); msg.ws_status = FpaWsStatusToMsg(msg, payload.ws_status); msg.ws_conv = FpaWsConvToMsg(msg, payload.ws_conv); msg.markers_status = FpaMarkersStatusToMsg(msg, payload.markers_status);