diff --git a/fixposition-sdk b/fixposition-sdk index bd34a92..89b0495 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 7687879..1ace837 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 0093102..b5f1352 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 f6b3b2a..b347a61 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 84bbdfe..0731d3a 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 94c2b69..5af16d2 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);