diff --git a/fixposition-sdk b/fixposition-sdk index e7b00fe..53fa5b5 160000 --- a/fixposition-sdk +++ b/fixposition-sdk @@ -1 +1 @@ -Subproject commit e7b00fea2d1be2ea7bab9b2ce8215f0738c999b4 +Subproject commit 53fa5b5ce523614bacec31a36a65388e45a69258 diff --git a/fixposition_driver_msgs/msg/FpaImu.msg b/fixposition_driver_msgs/msg/FpaImu.msg new file mode 100644 index 0000000..c0b4e36 --- /dev/null +++ b/fixposition_driver_msgs/msg/FpaImu.msg @@ -0,0 +1,12 @@ +# Copyright (c) Fixposition AG (www.fixposition.com) and contributors +# License: see the LICENSE file +# +# FP_A-RAWIMU or FP_A-CORRIMU data + +bool bias_comp # Signal is bias compensated (true) or not (false), always false for RAWIMU, may be true for CORRIMU +int8 imu_status # IMU bias status, see consts.IMU_STATUS_... +sensor_msgs/Imu data # IMU data + +# Note that bias_comp and imu_status are available since version 2.119.0 of the Vision-RTK 2 software. + +fixposition_driver_msgs/FpaConsts consts # Constants for enums used in FP_A diff --git a/fixposition_driver_msgs/msg/FpaOdomstatus.msg b/fixposition_driver_msgs/msg/FpaOdomstatus.msg index 79c2487..f6b3b2a 100644 --- a/fixposition_driver_msgs/msg/FpaOdomstatus.msg +++ b/fixposition_driver_msgs/msg/FpaOdomstatus.msg @@ -12,7 +12,7 @@ int8 fusion_corr # Fusion measurement status: GNSS c int8 fusion_cam1 # Fusion measurement status: camera, 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 IMU_STATUS_... +int8 imu_status # IMU bias status, see consts.IMU_STATUS_... int8 imu_noise # IMU variance status, see consts.IMU_NOISE_... int8 imu_conv # IMU convergence status, see consts.IMU_CONV_... int8 gnss1_status # GNSS 1 fix status, see consts.GNSS_STATUS_... diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/ros1_msgs.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/ros1_msgs.hpp index d860d6c..4e8286d 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/ros1_msgs.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/ros1_msgs.hpp @@ -35,6 +35,7 @@ #include #include #include +#include #include #include #include diff --git a/fixposition_driver_ros1/src/data_to_ros1.cpp b/fixposition_driver_ros1/src/data_to_ros1.cpp index 07a7e86..b9e792f 100644 --- a/fixposition_driver_ros1/src/data_to_ros1.cpp +++ b/fixposition_driver_ros1/src/data_to_ros1.cpp @@ -397,16 +397,20 @@ static void FpaImuPayloadToRos(const SomeFpaImuPayload& payload, sensor_msgs::Im void PublishFpaRawimu(const fpa::FpaRawimuPayload& payload, ros::Publisher& pub) { if (pub.getNumSubscribers() > 0) { - sensor_msgs::Imu msg; - FpaImuPayloadToRos(payload, msg); + fixposition_driver_msgs::FpaImu msg; + FpaImuPayloadToRos(payload, msg.data); + msg.bias_comp = payload.bias_comp; + msg.imu_status = FpaImuStatusToMsg(msg, payload.imu_status); pub.publish(msg); } } void PublishFpaCorrimu(const fpa::FpaCorrimuPayload& payload, ros::Publisher& pub) { if (pub.getNumSubscribers() > 0) { - sensor_msgs::Imu msg; - FpaImuPayloadToRos(payload, msg); + fixposition_driver_msgs::FpaImu msg; + FpaImuPayloadToRos(payload, msg.data); + msg.bias_comp = payload.bias_comp; + msg.imu_status = FpaImuStatusToMsg(msg, payload.imu_status); pub.publish(msg); } } diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp index 626a784..d0e5f22 100644 --- a/fixposition_driver_ros1/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros1/src/fixposition_driver_node.cpp @@ -222,7 +222,7 @@ bool FixpositionDriverNode::StartNode() { // FP_A-RAWIMU if (params_.MessageEnabled(fpa::FpaRawimuPayload::MSG_NAME)) { - _PUB(rawimu_pub_, sensor_msgs::Imu, output_ns + "/fpa/rawimu", 5); + _PUB(rawimu_pub_, fixposition_driver_msgs::FpaImu, output_ns + "/fpa/rawimu", 5); driver_.AddFpaObserver(fpa::FpaRawimuPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) { PublishFpaRawimu(dynamic_cast(payload), rawimu_pub_); }); @@ -230,7 +230,7 @@ bool FixpositionDriverNode::StartNode() { // FP_A-CORRIMU if (params_.MessageEnabled(fpa::FpaCorrimuPayload::MSG_NAME)) { - _PUB(corrimu_pub_, sensor_msgs::Imu, output_ns + "/fpa/corrimu", 5); + _PUB(corrimu_pub_, fixposition_driver_msgs::FpaImu, output_ns + "/fpa/corrimu", 5); driver_.AddFpaObserver(fpa::FpaCorrimuPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) { PublishFpaCorrimu(dynamic_cast(payload), corrimu_pub_); }); diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp index d57a0d0..f3ac657 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp @@ -63,9 +63,9 @@ void PublishFpaTp(const fpsdk::common::parser::fpa::FpaTpPayload& payload, void PublishFpaText(const fpsdk::common::parser::fpa::FpaTextPayload& payload, rclcpp::Publisher::SharedPtr& pub); void PublishFpaRawimu(const fpsdk::common::parser::fpa::FpaRawimuPayload& payload, - rclcpp::Publisher::SharedPtr& pub); + rclcpp::Publisher::SharedPtr& pub); void PublishFpaCorrimu(const fpsdk::common::parser::fpa::FpaCorrimuPayload& payload, - rclcpp::Publisher::SharedPtr& pub); + rclcpp::Publisher::SharedPtr& pub); bool PublishNovbBestgnsspos(const fpsdk::common::parser::novb::NovbHeader* header, const fpsdk::common::parser::novb::NovbBestgnsspos* payload, diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp index 682ba22..45f1457 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp @@ -105,9 +105,9 @@ class FixpositionDriverNode { //! Euler angles pitch-roll as estimated from the IMU in local horizontal rclcpp::Publisher::SharedPtr eul_imu_pub_; // - IMU - rclcpp::Publisher::SharedPtr rawimu_pub_; //!< Raw IMU data in IMU frame - rclcpp::Publisher::SharedPtr corrimu_pub_; //!< Bias corrected IMU data in IMU frame - rclcpp::Publisher::SharedPtr poiimu_pub_; //!< Bias corrected IMU data in POI frame + rclcpp::Publisher::SharedPtr rawimu_pub_; //!< Raw IMU data in IMU frame + rclcpp::Publisher::SharedPtr corrimu_pub_; //!< Bias corrected IMU data in IMU frame + rclcpp::Publisher::SharedPtr poiimu_pub_; //!< Bias corrected IMU data in POI frame // - GNSS rclcpp::Publisher::SharedPtr nmea_epoch_pub_; //!< NMEA epoch data rclcpp::Publisher::SharedPtr navsatfix_gnss1_pub_; //!< GNSS1 position and status diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp index 3068c3f..656617b 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include #include diff --git a/fixposition_driver_ros2/src/data_to_ros2.cpp b/fixposition_driver_ros2/src/data_to_ros2.cpp index ce7297f..a09d2e4 100644 --- a/fixposition_driver_ros2/src/data_to_ros2.cpp +++ b/fixposition_driver_ros2/src/data_to_ros2.cpp @@ -403,19 +403,22 @@ static void FpaImuPayloadToRos(const SomeFpaImuPayload& payload, sensor_msgs::ms } } -void PublishFpaRawimu(const fpa::FpaRawimuPayload& payload, rclcpp::Publisher::SharedPtr& pub) { +void PublishFpaRawimu(const fpa::FpaRawimuPayload& payload, rclcpp::Publisher::SharedPtr& pub) { if (pub->get_subscription_count() > 0) { - sensor_msgs::msg::Imu msg; - FpaImuPayloadToRos(payload, msg); + fpmsgs::FpaImu msg; + FpaImuPayloadToRos(payload, msg.data); + msg.bias_comp = payload.bias_comp; + msg.imu_status = FpaImuStatusToMsg(msg, payload.imu_status); pub->publish(msg); } } -void PublishFpaCorrimu(const fpa::FpaCorrimuPayload& payload, - rclcpp::Publisher::SharedPtr& pub) { +void PublishFpaCorrimu(const fpa::FpaCorrimuPayload& payload, rclcpp::Publisher::SharedPtr& pub) { if (pub->get_subscription_count() > 0) { - sensor_msgs::msg::Imu msg; - FpaImuPayloadToRos(payload, msg); + fpmsgs::FpaImu msg; + FpaImuPayloadToRos(payload, msg.data); + msg.bias_comp = payload.bias_comp; + msg.imu_status = FpaImuStatusToMsg(msg, payload.imu_status); pub->publish(msg); } } diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp index 3e127dc..8c21b3a 100644 --- a/fixposition_driver_ros2/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros2/src/fixposition_driver_node.cpp @@ -250,7 +250,7 @@ bool FixpositionDriverNode::StartNode() { // FP_A-RAWIMU if (params_.MessageEnabled(fpa::FpaRawimuPayload::MSG_NAME)) { - _PUB(rawimu_pub_, sensor_msgs::msg::Imu, output_ns + "/fpa/rawimu", qos_settings_); + _PUB(rawimu_pub_, fpmsgs::FpaImu, output_ns + "/fpa/rawimu", qos_settings_); driver_.AddFpaObserver(fpa::FpaRawimuPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) { PublishFpaRawimu(dynamic_cast(payload), rawimu_pub_); }); @@ -258,7 +258,7 @@ bool FixpositionDriverNode::StartNode() { // FP_A-CORRIMU if (params_.MessageEnabled(fpa::FpaCorrimuPayload::MSG_NAME)) { - _PUB(corrimu_pub_, sensor_msgs::msg::Imu, output_ns + "/fpa/corrimu", qos_settings_); + _PUB(corrimu_pub_, fpmsgs::FpaImu, output_ns + "/fpa/corrimu", qos_settings_); driver_.AddFpaObserver(fpa::FpaCorrimuPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) { PublishFpaCorrimu(dynamic_cast(payload), corrimu_pub_); });