diff --git a/CHANGELOG.rst b/CHANGELOG.rst index a3cb807c..4f961c61 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package septentrio_gnss_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.4.2 (upcoming) +------------------ +* Fixes + * Add export of compiler directives (thanks to @oysstu) + 1.4.1 (2024-08-04) ------------------ * Fixes diff --git a/CMakeLists.txt b/CMakeLists.txt index b5b9682e..d9b4d5ed 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -33,7 +33,6 @@ find_package(catkin QUIET) if(catkin_FOUND) ## ROS 1 ----------------------------------------------------------------------------------------------- - add_compile_definitions(ROS1) set(ENV{ROS_VERSION} "1") configure_file(${CMAKE_SOURCE_DIR}/msg_cache/BlockHeaderRos1.msg ${CMAKE_SOURCE_DIR}/msg/BlockHeader.msg COPYONLY) @@ -129,6 +128,7 @@ if(catkin_FOUND) ${libpcap_LIBRARIES} ${GeographicLib_LIBRARIES} ) + target_compile_definitions(${PROJECT_NAME}_node PUBLIC ROS1) # Installation install(TARGETS ${PROJECT_NAME}_node @@ -143,7 +143,6 @@ if(catkin_FOUND) #------------------------------------------------------------------------------------------------------- elseif(ament_cmake_FOUND) ## ROS 2 ----------------------------------------------------------------------------------------------- - add_compile_definitions(ROS2) set(ENV{ROS_VERSION} "2") configure_file(${CMAKE_SOURCE_DIR}/msg_cache/BlockHeaderRos2.msg ${CMAKE_SOURCE_DIR}/msg/BlockHeader.msg COPYONLY) @@ -231,6 +230,7 @@ elseif(ament_cmake_FOUND) src/septentrio_gnss_driver/parsers/parsing_utilities.cpp src/septentrio_gnss_driver/parsers/string_utilities.cpp ) + target_compile_definitions(${library_name} PUBLIC ROS2) target_include_directories(${library_name} PUBLIC "$" "$" @@ -252,7 +252,7 @@ elseif(ament_cmake_FOUND) rosidl_get_typesupport_target( cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") target_link_libraries(${library_name} "${cpp_typesupport_target}") - add_compile_definitions(ROS2_VER_N250) + target_compile_definitions(${library_name} PUBLIC ROS2_VER_N250) endif() ## executable diff --git a/include/septentrio_gnss_driver/abstraction/typedefs.hpp b/include/septentrio_gnss_driver/abstraction/typedefs.hpp index 59a30c00..0d3566c8 100644 --- a/include/septentrio_gnss_driver/abstraction/typedefs.hpp +++ b/include/septentrio_gnss_driver/abstraction/typedefs.hpp @@ -109,6 +109,7 @@ typedef geometry_msgs::msg::Quaternion QuaternionMsg; typedef geometry_msgs::msg::PoseWithCovarianceStamped PoseWithCovarianceStampedMsg; typedef geometry_msgs::msg::TwistWithCovarianceStamped TwistWithCovarianceStampedMsg; typedef geometry_msgs::msg::TransformStamped TransformStampedMsg; +typedef geometry_msgs::msg::Vector3 Vector3Msg; typedef gps_msgs::msg::GPSFix GpsFixMsg; typedef gps_msgs::msg::GPSStatus GpsStatusMsg; typedef sensor_msgs::msg::NavSatFix NavSatFixMsg; diff --git a/include/septentrio_gnss_driver/abstraction/typedefs_ros1.hpp b/include/septentrio_gnss_driver/abstraction/typedefs_ros1.hpp index 0994ae57..ef5df100 100644 --- a/include/septentrio_gnss_driver/abstraction/typedefs_ros1.hpp +++ b/include/septentrio_gnss_driver/abstraction/typedefs_ros1.hpp @@ -101,6 +101,7 @@ typedef geometry_msgs::Quaternion QuaternionMsg; typedef geometry_msgs::PoseWithCovarianceStamped PoseWithCovarianceStampedMsg; typedef geometry_msgs::TwistWithCovarianceStamped TwistWithCovarianceStampedMsg; typedef geometry_msgs::TransformStamped TransformStampedMsg; +typedef geometry_msgs::Vector3 Vector3Msg; typedef gps_common::GPSFix GpsFixMsg; typedef gps_common::GPSStatus GpsStatusMsg; typedef sensor_msgs::NavSatFix NavSatFixMsg; @@ -186,7 +187,8 @@ class ROSaicNodeBase { public: ROSaicNodeBase() : - pNh_(new ros::NodeHandle("~")), tfListener_(tfBuffer_), lastTfStamp_(0) + pNh_(std::make_shared("~")), tfListener_(tfBuffer_), + lastTfStamp_(0) { } diff --git a/include/septentrio_gnss_driver/communication/async_manager.hpp b/include/septentrio_gnss_driver/communication/async_manager.hpp index d09d1362..a5a591a6 100644 --- a/include/septentrio_gnss_driver/communication/async_manager.hpp +++ b/include/septentrio_gnss_driver/communication/async_manager.hpp @@ -162,7 +162,7 @@ namespace io { template AsyncManager::AsyncManager(ROSaicNodeBase* node, TelegramQueue* telegramQueue) : - node_(node), ioService_(new boost::asio::io_service), + node_(node), ioService_(std::make_shared()), ioInterface_(node, ioService_), telegramQueue_(telegramQueue) { node_->log(log_level::DEBUG, "AsyncManager created."); @@ -305,7 +305,7 @@ namespace io { template void AsyncManager::resync() { - telegram_.reset(new Telegram); + telegram_ = std::make_shared(); readSync<0>(); } @@ -589,7 +589,7 @@ namespace io { { case SYNC_BYTE_1: { - telegram_.reset(new Telegram); + telegram_ = std::make_shared(); telegram_->message[0] = buf_[0]; telegram_->stamp = node_->getTime(); node_->log( diff --git a/include/septentrio_gnss_driver/communication/io.hpp b/include/septentrio_gnss_driver/communication/io.hpp index 4a5323b1..6e99c5ea 100644 --- a/include/septentrio_gnss_driver/communication/io.hpp +++ b/include/septentrio_gnss_driver/communication/io.hpp @@ -88,9 +88,9 @@ namespace io { private: void connect() { - socket_.reset(new boost::asio::ip::udp::socket( + socket_ = std::make_unique( ioService_, - boost::asio::ip::udp::endpoint(boost::asio::ip::udp::v4(), port_))); + boost::asio::ip::udp::endpoint(boost::asio::ip::udp::v4(), port_)); asyncReceive(); @@ -120,7 +120,7 @@ namespace io { { while ((bytes_recvd - idx) > 2) { - std::shared_ptr telegram(new Telegram); + auto telegram = std::make_shared(); telegram->stamp = stamp; /*node_->log(log_level::DEBUG, "Buffer: " + std::string(telegram->message.begin(), @@ -283,7 +283,7 @@ namespace io { return false; } - stream_.reset(new boost::asio::ip::tcp::socket(*ioService_)); + stream_ = std::make_unique(*ioService_); node_->log(log_level::INFO, "Connecting to tcp://" + node_->settings()->device_tcp_ip + ":" + @@ -372,7 +372,7 @@ namespace io { flowcontrol_(node->settings()->hw_flow_control), baudrate_(node->settings()->baudrate) { - stream_.reset(new boost::asio::serial_port(*ioService_)); + stream_ = std::make_unique(*ioService_); } ~SerialIo() { stream_->close(); } @@ -582,8 +582,8 @@ namespace io { try { - stream_.reset( - new boost::asio::posix::stream_descriptor(*ioService_)); + stream_ = std::make_unique( + *ioService_); stream_->assign(fd); } catch (std::runtime_error& e) @@ -631,8 +631,8 @@ namespace io { node_->log(log_level::INFO, "Opening pcap file stream" + node_->settings()->device + "..."); - stream_.reset( - new boost::asio::posix::stream_descriptor(*ioService_)); + stream_ = std::make_unique( + *ioService_); pcap_ = pcap_open_offline(node_->settings()->device.c_str(), errBuff_.data()); diff --git a/include/septentrio_gnss_driver/parsers/parsing_utilities.hpp b/include/septentrio_gnss_driver/parsers/parsing_utilities.hpp index 8168e86e..ef5cec74 100644 --- a/include/septentrio_gnss_driver/parsers/parsing_utilities.hpp +++ b/include/septentrio_gnss_driver/parsers/parsing_utilities.hpp @@ -56,7 +56,7 @@ namespace parsing_utilities { - constexpr double pi_half = boost::math::constants::pi() / 2.0; + const double pihalf = boost::math::constants::half_pi(); /*********************************************************************** * Square value @@ -98,7 +98,8 @@ namespace parsing_utilities { /*********************************************************************** * Convert Euler angles to rotation matrix **********************************************************************/ - [[nodiscard]] inline Eigen::Matrix3d rpyToRot(double roll, double pitch, double yaw) + [[nodiscard]] inline Eigen::Matrix3d rpyToRot(double roll, double pitch, + double yaw) { Eigen::Matrix3d M; double sa, ca, sb, cb, sc, cc; @@ -118,7 +119,10 @@ namespace parsing_utilities { * @brief Wraps an angle between -180 and 180 degrees * @param[in] angle The angle to be wrapped */ - [[nodiscard]] double wrapAngle180to180(double angle); + [[nodiscard]] inline double wrapAngle180to180(double angle) + { + return std::remainder(angle, 360.0); + } /** * @brief Converts an 8-byte-buffer into a double @@ -183,7 +187,8 @@ namespace parsing_utilities { * 10 * @return True if all went fine, false if not */ - [[nodiscard]] bool parseInt16(const std::string& string, int16_t& value, int32_t base = 10); + [[nodiscard]] bool parseInt16(const std::string& string, int16_t& value, + int32_t base = 10); /** * @brief Converts a 4-byte-buffer into a signed 32-bit integer @@ -206,7 +211,8 @@ namespace parsing_utilities { * 10 * @return True if all went fine, false if not */ - [[nodiscard]] bool parseInt32(const std::string& string, int32_t& value, int32_t base = 10); + [[nodiscard]] bool parseInt32(const std::string& string, int32_t& value, + int32_t base = 10); /** * @brief Interprets the contents of "string" as a unsigned integer number of @@ -222,7 +228,8 @@ namespace parsing_utilities { * 10 * @return True if all went fine, false if not */ - [[nodiscard]] bool parseUInt8(const std::string& string, uint8_t& value, int32_t base = 10); + [[nodiscard]] bool parseUInt8(const std::string& string, uint8_t& value, + int32_t base = 10); /** * @brief Converts a 2-byte-buffer into an unsigned 16-bit integer @@ -245,7 +252,8 @@ namespace parsing_utilities { * 10 * @return True if all went fine, false if not */ - [[nodiscard]] bool parseUInt16(const std::string& string, uint16_t& value, int32_t base = 10); + [[nodiscard]] bool parseUInt16(const std::string& string, uint16_t& value, + int32_t base = 10); /** * @brief Converts a 4-byte-buffer into an unsigned 32-bit integer @@ -268,7 +276,8 @@ namespace parsing_utilities { * 10 * @return True if all went fine, false if not */ - [[nodiscard]] bool parseUInt32(const std::string& string, uint32_t& value, int32_t base = 10); + [[nodiscard]] bool parseUInt32(const std::string& string, uint32_t& value, + int32_t base = 10); /** * @brief Converts UTC time from the without-colon-delimiter format to the @@ -309,8 +318,24 @@ namespace parsing_utilities { * @param[in] roll Roll about the new y-axis [rad] * @return quaternion */ - [[nodiscard]] Eigen::Quaterniond convertEulerToQuaternion(double roll, double pitch, - double yaw); + //! The rotational sequence convention we adopt here (and Septentrio receivers' + //! pitch, roll, yaw definition too) is the yaw-pitch-roll sequence, i.e. the + //! 3-2-1 sequence: The body first does yaw around the z-axis, then pitches + //! around the new y-axis and finally rolls around the new x-axis. + [[nodiscard]] inline Eigen::Quaterniond + convertEulerToQuaternion(double roll, double pitch, double yaw) + { + double cy = std::cos(yaw * 0.5); + double sy = std::sin(yaw * 0.5); + double cp = std::cos(pitch * 0.5); + double sp = std::sin(pitch * 0.5); + double cr = std::cos(roll * 0.5); + double sr = std::sin(roll * 0.5); + + return Eigen::Quaterniond( + cr * cp * cy + sr * sp * sy, sr * cp * cy - cr * sp * sy, + cr * sp * cy + sr * cp * sy, cr * cp * sy - sr * sp * cy); + } /** * @brief Transforms Euler angles to a QuaternionMsg @@ -319,14 +344,44 @@ namespace parsing_utilities { * @param[in] roll Roll about the new x-axis [rad] * @return ROS message representing a quaternion */ - [[nodiscard]] QuaternionMsg convertEulerToQuaternionMsg(double roll, double pitch, double yaw); + [[nodiscard]] inline QuaternionMsg + quaternionToQuaternionMsg(const Eigen::Quaterniond& q) + { + QuaternionMsg qm; + + qm.w = q.w(); + qm.x = q.x(); + qm.y = q.y(); + qm.z = q.z(); + + return qm; + } /** * @brief Convert Eigen quaternion to a QuaternionMsg * @param[in] q Eigen quaternion * @return ROS message representing a quaternion */ - [[nodiscard]] QuaternionMsg quaternionToQuaternionMsg(const Eigen::Quaterniond& q); + [[nodiscard]] inline QuaternionMsg + convertEulerToQuaternionMsg(double roll, double pitch, double yaw) + { + return quaternionToQuaternionMsg(convertEulerToQuaternion(roll, pitch, yaw)); + } + + inline void setQuaternionNaN(QuaternionMsg& quaternion) + { + quaternion.w = std::numeric_limits::quiet_NaN(); + quaternion.x = std::numeric_limits::quiet_NaN(); + quaternion.y = std::numeric_limits::quiet_NaN(); + quaternion.z = std::numeric_limits::quiet_NaN(); + } + + inline void setVector3NaN(Vector3Msg& v) + { + v.x = std::numeric_limits::quiet_NaN(); + v.y = std::numeric_limits::quiet_NaN(); + v.z = std::numeric_limits::quiet_NaN(); + } /** * @brief Generates the quaternion to rotate from local ENU to ECEF @@ -334,7 +389,15 @@ namespace parsing_utilities { * @param[in] lon geodetic longitude [rad] * @return quaternion */ - [[nodiscard]] Eigen::Quaterniond q_enu_ecef(double lat, double lon); + [[nodiscard]] inline Eigen::Quaterniond q_enu_ecef(double lat, double lon) + { + double sr = sin((pihalf - lat) / 2.0); + double cr = cos((pihalf - lat) / 2.0); + double sy = sin((lon + pihalf) / 2.0); + double cy = cos((lon + pihalf) / 2.0); + + return Eigen::Quaterniond(cr * cy, sr * cy, sr * sy, cr * sy); + } /** * @brief Generates the quaternion to rotate from local NED to ECEF @@ -342,7 +405,15 @@ namespace parsing_utilities { * @param[in] lon geodetic longitude [rad] * @return rotation matrix */ - [[nodiscard]] Eigen::Quaterniond q_ned_ecef(double lat, double lon); + [[nodiscard]] inline Eigen::Quaterniond q_ned_ecef(double lat, double lon) + { + double sp = sin((-lat - pihalf) / 2.0); + double cp = cos((-lat - pihalf) / 2.0); + double sy = sin(lon / 2.0); + double cy = cos(lon / 2.0); + + return Eigen::Quaterniond(cp * cy, -sp * sy, sp * cy, cp * sy); + } /** * @brief Generates the matrix to rotate from local ENU to ECEF @@ -350,7 +421,27 @@ namespace parsing_utilities { * @param[in] lon geodetic longitude [rad] * @return rotation matrix */ - [[nodiscard]] Eigen::Matrix3d R_enu_ecef(double lat, double lon); + [[nodiscard]] inline Eigen::Matrix3d R_enu_ecef(double lat, double lon) + { + Eigen::Matrix3d R; + + double sin_lat = sin(lat); + double cos_lat = cos(lat); + double sin_lon = sin(lon); + double cos_lon = cos(lon); + + R(0, 0) = -sin_lon; + R(0, 1) = -cos_lon * sin_lat; + R(0, 2) = cos_lon * cos_lat; + R(1, 0) = cos_lon; + R(1, 1) = -sin_lon * sin_lat; + R(1, 2) = sin_lon * cos_lat; + R(2, 0) = 0.0; + R(2, 1) = cos_lat; + R(2, 2) = sin_lat; + + return R; + } /** * @brief Generates the matrix to rotate from local NED to ECEF @@ -358,7 +449,27 @@ namespace parsing_utilities { * @param[in] lon geodetic longitude [rad] * @return rotation matrix */ - [[nodiscard]] Eigen::Matrix3d R_ned_ecef(double lat, double lon); + [[nodiscard]] inline Eigen::Matrix3d R_ned_ecef(double lat, double lon) + { + Eigen::Matrix3d R; + + double sin_lat = sin(lat); + double cos_lat = cos(lat); + double sin_lon = sin(lon); + double cos_lon = cos(lon); + + R(0, 0) = -cos_lon * sin_lat; + R(0, 1) = -sin_lon; + R(0, 2) = -cos_lon * cos_lat; + R(1, 0) = -sin_lon * sin_lat; + R(1, 1) = cos_lon; + R(1, 2) = -sin_lon * cos_lat; + R(2, 0) = cos_lat; + R(2, 1) = 0.0; + R(2, 2) = -sin_lat; + + return R; + } /** * @brief Transforms the input polling period [milliseconds] into a std::string @@ -409,4 +520,14 @@ namespace parsing_utilities { * @return SBF GPS week counter */ [[nodiscard]] uint16_t getWnc(const std::vector& message); + + inline double convertAutoCovariance(double val) + { + return std::isnan(val) ? -1.0 : deg2radSq(val); + } + + inline double convertCovariance(double val) + { + return std::isnan(val) ? 0.0 : deg2radSq(val); + } } // namespace parsing_utilities \ No newline at end of file diff --git a/msg/BlockHeader.msg b/msg/BlockHeader.msg deleted file mode 100644 index e7d04f9e..00000000 --- a/msg/BlockHeader.msg +++ /dev/null @@ -1,11 +0,0 @@ -# Blockheader including time header for all ROS messages that publish SBF blocks - -uint8 sync_1 36 # 0x24 -uint8 sync_2 64 # 0x40 -uint16 crc -uint16 id -uint8 revision -uint16 length - -uint32 tow 4294967295 # ms since week start -uint16 wnc 65535 # weeks since Jan 06, 1980 at 00:00:00 UTC \ No newline at end of file diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp index 67f71221..b8fd184a 100644 --- a/src/septentrio_gnss_driver/communication/communication_core.cpp +++ b/src/septentrio_gnss_driver/communication/communication_core.cpp @@ -77,7 +77,7 @@ namespace io { resetSettings(); running_ = false; - std::shared_ptr telegram(new Telegram); + auto telegram = std::make_shared(); telegramQueue_.push(telegram); processingThread_.join(); } @@ -214,7 +214,8 @@ namespace io { node_->log(log_level::DEBUG, "Called initializeIo() method"); if ((settings_->tcp_port != 0) && (!settings_->tcp_ip_server.empty())) { - tcpClient_.reset(new AsyncManager(node_, &telegramQueue_)); + tcpClient_ = + std::make_unique>(node_, &telegramQueue_); tcpClient_->setPort(std::to_string(settings_->tcp_port)); if (!settings_->configure_rx) tcpClient_->connect(); @@ -222,8 +223,8 @@ namespace io { } if ((settings_->udp_port != 0) && (!settings_->udp_ip_server.empty())) { - udpClient_.reset( - new UdpClient(node_, settings_->udp_port, &telegramQueue_)); + udpClient_ = std::make_unique(node_, settings_->udp_port, + &telegramQueue_); client = true; } @@ -231,22 +232,25 @@ namespace io { { case device_type::TCP: { - manager_.reset(new AsyncManager(node_, &telegramQueue_)); + manager_ = std::make_unique>(node_, &telegramQueue_); break; } case device_type::SERIAL: { - manager_.reset(new AsyncManager(node_, &telegramQueue_)); + manager_ = + std::make_unique>(node_, &telegramQueue_); break; } case device_type::SBF_FILE: { - manager_.reset(new AsyncManager(node_, &telegramQueue_)); + manager_ = + std::make_unique>(node_, &telegramQueue_); break; } case device_type::PCAP_FILE: { - manager_.reset(new AsyncManager(node_, &telegramQueue_)); + manager_ = + std::make_unique>(node_, &telegramQueue_); break; } default: @@ -974,7 +978,8 @@ namespace io { send("sdio, " + settings_->ins_vsm.ip_server + ", NMEA, none\x0D"); - tcpVsm_.reset(new AsyncManager(node_, &telegramQueue_)); + tcpVsm_ = std::make_unique>(node_, + &telegramQueue_); tcpVsm_->setPort( std::to_string(settings_->ins_vsm.ip_server_port)); tcpVsm_->connect(); diff --git a/src/septentrio_gnss_driver/communication/message_handler.cpp b/src/septentrio_gnss_driver/communication/message_handler.cpp index 87e8d71c..1ea8e400 100644 --- a/src/septentrio_gnss_driver/communication/message_handler.cpp +++ b/src/septentrio_gnss_driver/communication/message_handler.cpp @@ -55,16 +55,6 @@ using parsing_utilities::square; namespace io { - double convertAutoCovariance(double val) - { - return std::isnan(val) ? -1.0 : deg2radSq(val); - } - - double convertCovariance(double val) - { - return std::isnan(val) ? 0.0 : deg2radSq(val); - } - void MessageHandler::assemblePoseWithCovarianceStamped() { if (!settings_->publish_pose) @@ -109,24 +99,17 @@ namespace io { deg2rad(roll), deg2rad(pitch), deg2rad(yaw)); } else { - msg.pose.pose.orientation.w = - std::numeric_limits::quiet_NaN(); - msg.pose.pose.orientation.x = - std::numeric_limits::quiet_NaN(); - msg.pose.pose.orientation.y = - std::numeric_limits::quiet_NaN(); - msg.pose.pose.orientation.z = - std::numeric_limits::quiet_NaN(); + parsing_utilities::setQuaternionNaN(msg.pose.pose.orientation); } if ((last_insnavgeod_.sb_list & 4) != 0) { // Attitude autocov - msg.pose.covariance[21] = - convertAutoCovariance(last_insnavgeod_.roll_std_dev); - msg.pose.covariance[28] = - convertAutoCovariance(last_insnavgeod_.pitch_std_dev); - msg.pose.covariance[35] = - convertAutoCovariance(last_insnavgeod_.heading_std_dev); + msg.pose.covariance[21] = parsing_utilities::convertAutoCovariance( + last_insnavgeod_.roll_std_dev); + msg.pose.covariance[28] = parsing_utilities::convertAutoCovariance( + last_insnavgeod_.pitch_std_dev); + msg.pose.covariance[35] = parsing_utilities::convertAutoCovariance( + last_insnavgeod_.heading_std_dev); } else { @@ -147,19 +130,19 @@ namespace io { if ((last_insnavgeod_.sb_list & 64) != 0) { // Attitude cov - msg.pose.covariance[22] = - convertCovariance(last_insnavgeod_.pitch_roll_cov); - msg.pose.covariance[23] = - convertCovariance(last_insnavgeod_.heading_roll_cov); - msg.pose.covariance[27] = - convertCovariance(last_insnavgeod_.pitch_roll_cov); - - msg.pose.covariance[29] = - convertCovariance(last_insnavgeod_.heading_pitch_cov); - msg.pose.covariance[33] = - convertCovariance(last_insnavgeod_.heading_roll_cov); - msg.pose.covariance[34] = - convertCovariance(last_insnavgeod_.heading_pitch_cov); + msg.pose.covariance[22] = parsing_utilities::convertCovariance( + last_insnavgeod_.pitch_roll_cov); + msg.pose.covariance[23] = parsing_utilities::convertCovariance( + last_insnavgeod_.heading_roll_cov); + msg.pose.covariance[27] = parsing_utilities::convertCovariance( + last_insnavgeod_.pitch_roll_cov); + + msg.pose.covariance[29] = parsing_utilities::convertCovariance( + last_insnavgeod_.heading_pitch_cov); + msg.pose.covariance[33] = parsing_utilities::convertCovariance( + last_insnavgeod_.heading_roll_cov); + msg.pose.covariance[34] = parsing_utilities::convertCovariance( + last_insnavgeod_.heading_pitch_cov); } } else { @@ -197,24 +180,24 @@ namespace io { msg.pose.covariance[12] = last_poscovgeodetic_.cov_lonhgt; msg.pose.covariance[13] = last_poscovgeodetic_.cov_lathgt; msg.pose.covariance[14] = last_poscovgeodetic_.cov_hgthgt; - msg.pose.covariance[21] = - convertAutoCovariance(last_attcoveuler_.cov_rollroll); - msg.pose.covariance[22] = - convertCovariance(last_attcoveuler_.cov_pitchroll); + msg.pose.covariance[21] = parsing_utilities::convertAutoCovariance( + last_attcoveuler_.cov_rollroll); + msg.pose.covariance[22] = parsing_utilities::convertCovariance( + last_attcoveuler_.cov_pitchroll); msg.pose.covariance[23] = - convertCovariance(last_attcoveuler_.cov_headroll); - msg.pose.covariance[27] = - convertCovariance(last_attcoveuler_.cov_pitchroll); - msg.pose.covariance[28] = - convertAutoCovariance(last_attcoveuler_.cov_pitchpitch); - msg.pose.covariance[29] = - convertCovariance(last_attcoveuler_.cov_headpitch); + parsing_utilities::convertCovariance(last_attcoveuler_.cov_headroll); + msg.pose.covariance[27] = parsing_utilities::convertCovariance( + last_attcoveuler_.cov_pitchroll); + msg.pose.covariance[28] = parsing_utilities::convertAutoCovariance( + last_attcoveuler_.cov_pitchpitch); + msg.pose.covariance[29] = parsing_utilities::convertCovariance( + last_attcoveuler_.cov_headpitch); msg.pose.covariance[33] = - convertCovariance(last_attcoveuler_.cov_headroll); - msg.pose.covariance[34] = - convertCovariance(last_attcoveuler_.cov_headpitch); - msg.pose.covariance[35] = - convertAutoCovariance(last_attcoveuler_.cov_headhead); + parsing_utilities::convertCovariance(last_attcoveuler_.cov_headroll); + msg.pose.covariance[34] = parsing_utilities::convertCovariance( + last_attcoveuler_.cov_headpitch); + msg.pose.covariance[35] = parsing_utilities::convertAutoCovariance( + last_attcoveuler_.cov_headhead); } publish("pose", msg); }; @@ -635,28 +618,37 @@ namespace io { validValue(last_insnavgeod_.heading_std_dev)) { msg.orientation_covariance[0] = - convertAutoCovariance(last_insnavgeod_.roll_std_dev); + parsing_utilities::convertAutoCovariance( + last_insnavgeod_.roll_std_dev); msg.orientation_covariance[4] = - convertAutoCovariance(last_insnavgeod_.pitch_std_dev); + parsing_utilities::convertAutoCovariance( + last_insnavgeod_.pitch_std_dev); msg.orientation_covariance[8] = - convertAutoCovariance(last_insnavgeod_.heading_std_dev); + parsing_utilities::convertAutoCovariance( + last_insnavgeod_.heading_std_dev); if ((last_insnavgeod_.sb_list & 64) != 0) { // Attitude cov msg.orientation_covariance[1] = - convertCovariance(last_insnavgeod_.pitch_roll_cov); + parsing_utilities::convertCovariance( + last_insnavgeod_.pitch_roll_cov); msg.orientation_covariance[2] = - convertCovariance(last_insnavgeod_.heading_roll_cov); + parsing_utilities::convertCovariance( + last_insnavgeod_.heading_roll_cov); msg.orientation_covariance[3] = - convertCovariance(last_insnavgeod_.pitch_roll_cov); + parsing_utilities::convertCovariance( + last_insnavgeod_.pitch_roll_cov); - msg.orientation_covariance[5] = convertCovariance( - last_insnavgeod_.heading_pitch_cov); + msg.orientation_covariance[5] = + parsing_utilities::convertCovariance( + last_insnavgeod_.heading_pitch_cov); msg.orientation_covariance[6] = - convertCovariance(last_insnavgeod_.heading_roll_cov); - msg.orientation_covariance[7] = convertCovariance( - last_insnavgeod_.heading_pitch_cov); + parsing_utilities::convertCovariance( + last_insnavgeod_.heading_roll_cov); + msg.orientation_covariance[7] = + parsing_utilities::convertCovariance( + last_insnavgeod_.heading_pitch_cov); } } else { @@ -670,10 +662,7 @@ namespace io { if (!valid_orientation) { - msg.orientation.w = std::numeric_limits::quiet_NaN(); - msg.orientation.x = std::numeric_limits::quiet_NaN(); - msg.orientation.y = std::numeric_limits::quiet_NaN(); - msg.orientation.z = std::numeric_limits::quiet_NaN(); + parsing_utilities::setQuaternionNaN(msg.orientation); } publish("imu", msg); @@ -690,9 +679,7 @@ namespace io { msg.twist.covariance[28] = -1.0; msg.twist.covariance[35] = -1.0; // Set angular velocities to NaN - msg.twist.twist.angular.x = std::numeric_limits::quiet_NaN(); - msg.twist.twist.angular.y = std::numeric_limits::quiet_NaN(); - msg.twist.twist.angular.z = std::numeric_limits::quiet_NaN(); + parsing_utilities::setVector3NaN(msg.twist.twist.angular); if (fromIns) { @@ -720,9 +707,7 @@ namespace io { msg.twist.twist.linear.z = vel(2); } else { - msg.twist.twist.linear.x = std::numeric_limits::quiet_NaN(); - msg.twist.twist.linear.y = std::numeric_limits::quiet_NaN(); - msg.twist.twist.linear.z = std::numeric_limits::quiet_NaN(); + parsing_utilities::setVector3NaN(msg.twist.twist.linear); } if (((last_insnavgeod_.sb_list & 16) != 0) && @@ -826,9 +811,7 @@ namespace io { msg.twist.twist.linear.z = vel(2); } else { - msg.twist.twist.linear.x = std::numeric_limits::quiet_NaN(); - msg.twist.twist.linear.y = std::numeric_limits::quiet_NaN(); - msg.twist.twist.linear.z = std::numeric_limits::quiet_NaN(); + parsing_utilities::setVector3NaN(msg.twist.twist.linear); } if (last_velcovgeodetic_.error == 0) @@ -998,20 +981,17 @@ namespace io { convertEulerToQuaternionMsg(roll, pitch, yaw); } else { - msg.pose.pose.orientation.w = std::numeric_limits::quiet_NaN(); - msg.pose.pose.orientation.x = std::numeric_limits::quiet_NaN(); - msg.pose.pose.orientation.y = std::numeric_limits::quiet_NaN(); - msg.pose.pose.orientation.z = std::numeric_limits::quiet_NaN(); + parsing_utilities::setQuaternionNaN(msg.pose.pose.orientation); } if ((last_insnavgeod_.sb_list & 4) != 0) { // Attitude autocovariance - msg.pose.covariance[21] = - convertAutoCovariance(last_insnavgeod_.roll_std_dev); - msg.pose.covariance[28] = - convertAutoCovariance(last_insnavgeod_.pitch_std_dev); - msg.pose.covariance[35] = - convertAutoCovariance(last_insnavgeod_.heading_std_dev); + msg.pose.covariance[21] = parsing_utilities::convertAutoCovariance( + last_insnavgeod_.roll_std_dev); + msg.pose.covariance[28] = parsing_utilities::convertAutoCovariance( + last_insnavgeod_.pitch_std_dev); + msg.pose.covariance[35] = parsing_utilities::convertAutoCovariance( + last_insnavgeod_.heading_std_dev); } else { msg.pose.covariance[21] = -1.0; @@ -1067,19 +1047,19 @@ namespace io { if ((last_insnavgeod_.sb_list & 64) != 0) { // Attitude covariancae - msg.pose.covariance[22] = - convertCovariance(last_insnavgeod_.pitch_roll_cov); - msg.pose.covariance[23] = - convertCovariance(last_insnavgeod_.heading_roll_cov); - msg.pose.covariance[27] = - convertCovariance(last_insnavgeod_.pitch_roll_cov); + msg.pose.covariance[22] = parsing_utilities::convertCovariance( + last_insnavgeod_.pitch_roll_cov); + msg.pose.covariance[23] = parsing_utilities::convertCovariance( + last_insnavgeod_.heading_roll_cov); + msg.pose.covariance[27] = parsing_utilities::convertCovariance( + last_insnavgeod_.pitch_roll_cov); - msg.pose.covariance[29] = - convertCovariance(last_insnavgeod_.heading_pitch_cov); - msg.pose.covariance[33] = - convertCovariance(last_insnavgeod_.heading_roll_cov); - msg.pose.covariance[34] = - convertCovariance(last_insnavgeod_.heading_pitch_cov); + msg.pose.covariance[29] = parsing_utilities::convertCovariance( + last_insnavgeod_.heading_pitch_cov); + msg.pose.covariance[33] = parsing_utilities::convertCovariance( + last_insnavgeod_.heading_roll_cov); + msg.pose.covariance[34] = parsing_utilities::convertCovariance( + last_insnavgeod_.heading_pitch_cov); } assembleLocalizationMsgTwist(roll, pitch, yaw, msg); @@ -1165,22 +1145,19 @@ namespace io { parsing_utilities::quaternionToQuaternionMsg(q_b_ecef); } else { - msg.pose.pose.orientation.w = std::numeric_limits::quiet_NaN(); - msg.pose.pose.orientation.x = std::numeric_limits::quiet_NaN(); - msg.pose.pose.orientation.y = std::numeric_limits::quiet_NaN(); - msg.pose.pose.orientation.z = std::numeric_limits::quiet_NaN(); + parsing_utilities::setQuaternionNaN(msg.pose.pose.orientation); } Eigen::Matrix3d covAtt_local = Eigen::Matrix3d::Zero(); bool covAttValid = true; if ((last_insnavgeod_.sb_list & 4) != 0) { // Attitude autocovariance - covAtt_local(0, 0) = - convertAutoCovariance(last_insnavgeod_.roll_std_dev); - covAtt_local(1, 1) = - convertAutoCovariance(last_insnavgeod_.pitch_std_dev); - covAtt_local(2, 2) = - convertAutoCovariance(last_insnavgeod_.heading_std_dev); + covAtt_local(0, 0) = parsing_utilities::convertAutoCovariance( + last_insnavgeod_.roll_std_dev); + covAtt_local(1, 1) = parsing_utilities::convertAutoCovariance( + last_insnavgeod_.pitch_std_dev); + covAtt_local(2, 2) = parsing_utilities::convertAutoCovariance( + last_insnavgeod_.heading_std_dev); covAttValid = !std::isnan(last_insnavgeod_.roll_std_dev) && !std::isnan(last_insnavgeod_.pitch_std_dev) && !std::isnan(last_insnavgeod_.heading_std_dev); @@ -1197,18 +1174,18 @@ namespace io { if ((last_insnavcart_.sb_list & 64) != 0) { // Attitude covariancae - covAtt_local(0, 1) = - convertCovariance(last_insnavcart_.pitch_roll_cov); - covAtt_local(0, 2) = - convertCovariance(last_insnavcart_.heading_roll_cov); - covAtt_local(1, 0) = - convertCovariance(last_insnavcart_.pitch_roll_cov); - covAtt_local(2, 1) = - convertCovariance(last_insnavcart_.heading_pitch_cov); - covAtt_local(2, 0) = - convertCovariance(last_insnavcart_.heading_roll_cov); - covAtt_local(1, 2) = - convertCovariance(last_insnavcart_.heading_pitch_cov); + covAtt_local(0, 1) = parsing_utilities::convertCovariance( + last_insnavcart_.pitch_roll_cov); + covAtt_local(0, 2) = parsing_utilities::convertCovariance( + last_insnavcart_.heading_roll_cov); + covAtt_local(1, 0) = parsing_utilities::convertCovariance( + last_insnavcart_.pitch_roll_cov); + covAtt_local(2, 1) = parsing_utilities::convertCovariance( + last_insnavcart_.heading_pitch_cov); + covAtt_local(2, 0) = parsing_utilities::convertCovariance( + last_insnavcart_.heading_roll_cov); + covAtt_local(1, 2) = parsing_utilities::convertCovariance( + last_insnavcart_.heading_pitch_cov); } Eigen::Matrix3d R_local_ecef; @@ -1250,6 +1227,8 @@ namespace io { double yaw, LocalizationMsg& msg) const { + + parsing_utilities::setVector3NaN(msg.twist.twist.angular); Eigen::Matrix3d R_local_body = parsing_utilities::rpyToRot(roll, pitch, yaw).inverse(); if ((last_insnavgeod_.sb_list & 8) != 0) @@ -1275,9 +1254,7 @@ namespace io { msg.twist.twist.linear.z = vel_body(2); } else { - msg.twist.twist.linear.x = std::numeric_limits::quiet_NaN(); - msg.twist.twist.linear.y = std::numeric_limits::quiet_NaN(); - msg.twist.twist.linear.z = std::numeric_limits::quiet_NaN(); + parsing_utilities::setVector3NaN(msg.twist.twist.linear); } Eigen::Matrix3d covVel_local = Eigen::Matrix3d::Zero(); if ((last_insnavgeod_.sb_list & 16) != 0) diff --git a/src/septentrio_gnss_driver/node/rosaic_node.cpp b/src/septentrio_gnss_driver/node/rosaic_node.cpp index 84a122f1..a63d0afa 100644 --- a/src/septentrio_gnss_driver/node/rosaic_node.cpp +++ b/src/septentrio_gnss_driver/node/rosaic_node.cpp @@ -57,7 +57,7 @@ rosaic_node::ROSaicNode::ROSaicNode(const rclcpp::NodeOptions& options) : this->log(log_level::DEBUG, "Called ROSaicNode() constructor.."); - tfListener_.reset(new tf2_ros::TransformListener(tfBuffer_)); + tfListener_ = std::make_unique(tfBuffer_); // Parameters must be set before initializing IO if (!getROSParams()) diff --git a/src/septentrio_gnss_driver/node/rosaic_node_ros1.cpp b/src/septentrio_gnss_driver/node/rosaic_node_ros1.cpp index 2c67a46f..c7305836 100644 --- a/src/septentrio_gnss_driver/node/rosaic_node_ros1.cpp +++ b/src/septentrio_gnss_driver/node/rosaic_node_ros1.cpp @@ -53,7 +53,7 @@ rosaic_node::ROSaicNode::ROSaicNode() : IO_(this) this->log(log_level::DEBUG, "Called ROSaicNode() constructor.."); - tfListener_.reset(new tf2_ros::TransformListener(tfBuffer_)); + tfListener_ = std::make_unique(tfBuffer_); // Parameters must be set before initializing IO if (!getROSParams()) diff --git a/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp b/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp index 30b9eeca..64021e1f 100644 --- a/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp +++ b/src/septentrio_gnss_driver/parsers/parsing_utilities.cpp @@ -44,15 +44,8 @@ namespace parsing_utilities { - const double pihalf = boost::math::constants::pi() / 2.0; - namespace qi = boost::spirit::qi; - [[nodiscard]] double wrapAngle180to180(double angle) - { - return std::remainder(angle, 360.0); - } - [[nodiscard]] double parseDouble(const uint8_t* buffer) { double val; @@ -295,108 +288,6 @@ namespace parsing_utilities { return timegm(timeinfo); } - //! The rotational sequence convention we adopt here (and Septentrio receivers' - //! pitch, roll, yaw definition too) is the yaw-pitch-roll sequence, i.e. the - //! 3-2-1 sequence: The body first does yaw around the z-axis, then pitches - //! around the new y-axis and finally rolls around the new x-axis. - [[nodiscard]] Eigen::Quaterniond - convertEulerToQuaternion(double roll, double pitch, double yaw) - { - double cy = std::cos(yaw * 0.5); - double sy = std::sin(yaw * 0.5); - double cp = std::cos(pitch * 0.5); - double sp = std::sin(pitch * 0.5); - double cr = std::cos(roll * 0.5); - double sr = std::sin(roll * 0.5); - - return Eigen::Quaterniond( - cr * cp * cy + sr * sp * sy, sr * cp * cy - cr * sp * sy, - cr * sp * cy + sr * cp * sy, cr * cp * sy - sr * sp * cy); - } - - [[nodiscard]] QuaternionMsg - quaternionToQuaternionMsg(const Eigen::Quaterniond& q) - { - QuaternionMsg qm; - - qm.w = q.w(); - qm.x = q.x(); - qm.y = q.y(); - qm.z = q.z(); - - return qm; - } - - [[nodiscard]] QuaternionMsg convertEulerToQuaternionMsg(double roll, - double pitch, double yaw) - { - return quaternionToQuaternionMsg(convertEulerToQuaternion(roll, pitch, yaw)); - } - - [[nodiscard]] Eigen::Quaterniond q_enu_ecef(double lat, double lon) - { - double sr = sin((pihalf - lat) / 2.0); - double cr = cos((pihalf - lat) / 2.0); - double sy = sin((lon + pihalf) / 2.0); - double cy = cos((lon + pihalf) / 2.0); - - return Eigen::Quaterniond(cr * cy, sr * cy, sr * sy, cr * sy); - } - - [[nodiscard]] Eigen::Quaterniond q_ned_ecef(double lat, double lon) - { - double sp = sin((-lat - pihalf) / 2.0); - double cp = cos((-lat - pihalf) / 2.0); - double sy = sin(lon / 2.0); - double cy = cos(lon / 2.0); - - return Eigen::Quaterniond(cp * cy, -sp * sy, sp * cy, cp * sy); - } - - [[nodiscard]] Eigen::Matrix3d R_enu_ecef(double lat, double lon) - { - Eigen::Matrix3d R; - - double sin_lat = sin(lat); - double cos_lat = cos(lat); - double sin_lon = sin(lon); - double cos_lon = cos(lon); - - R(0, 0) = -sin_lon; - R(0, 1) = -cos_lon * sin_lat; - R(0, 2) = cos_lon * cos_lat; - R(1, 0) = cos_lon; - R(1, 1) = -sin_lon * sin_lat; - R(1, 2) = sin_lon * cos_lat; - R(2, 0) = 0.0; - R(2, 1) = cos_lat; - R(2, 2) = sin_lat; - - return R; - } - - [[nodiscard]] Eigen::Matrix3d R_ned_ecef(double lat, double lon) - { - Eigen::Matrix3d R; - - double sin_lat = sin(lat); - double cos_lat = cos(lat); - double sin_lon = sin(lon); - double cos_lon = cos(lon); - - R(0, 0) = -cos_lon * sin_lat; - R(0, 1) = -sin_lon; - R(0, 2) = -cos_lon * cos_lat; - R(1, 0) = -sin_lon * sin_lat; - R(1, 1) = cos_lon; - R(1, 2) = -sin_lon * cos_lat; - R(2, 0) = cos_lat; - R(2, 1) = 0.0; - R(2, 2) = -sin_lat; - - return R; - } - [[nodiscard]] std::string convertUserPeriodToRxCommand(uint32_t period_user) { std::string cmd;