diff --git a/docker/Dockerfile.jazzy-base b/docker/Dockerfile.jazzy-base index 68d4955..d2fd40f 100644 --- a/docker/Dockerfile.jazzy-base +++ b/docker/Dockerfile.jazzy-base @@ -24,10 +24,6 @@ RUN /tmp/install_clangformat.sh && rm -f /tmp/install_clangformat.sh COPY docker/scripts/install_capnp.sh /tmp RUN /tmp/install_capnp.sh && rm -f /tmp/install_capnp.sh -# Download, build and install PROJ -COPY docker/scripts/install_proj.sh /tmp -RUN /tmp/install_proj.sh && rm -f /tmp/install_proj.sh - # Download, build and install Doxygen COPY docker/scripts/install_doxygen.sh /tmp RUN /tmp/install_doxygen.sh && rm -f /tmp/install_doxygen.sh @@ -46,10 +42,6 @@ Pin-Priority: -1 Package: capnproto libcapnp-* libcapnp-dev Pin: origin * Pin-Priority: -1 - -Package: proj proj-ps-doc proj-data proj-rdnap proj-bin libproj15 libproj-dev -Pin: origin * -Pin-Priority: -1 EOF # For debugging docker builds... (last layer!) diff --git a/docker/scripts/install_aptbase.sh b/docker/scripts/install_aptbase.sh index 4888e9b..cdc4dca 100755 --- a/docker/scripts/install_aptbase.sh +++ b/docker/scripts/install_aptbase.sh @@ -21,11 +21,15 @@ packages=$(awk -v filt=${FPSDK_IMAGE%-*} '$1 ~ filt { print $2 }' <, >=, <= operators on this! */ enum class NmeaTalkerId : int { // clang-format off - UNSPECIFIED = '?', //!< Unspecified + UNSPECIFIED = '!', //!< Unspecified PROPRIETARY = 'x', //!< Proprietary GPS_SBAS = 'P', //!< GPS and/or SBAS GLO = 'L', //!< GLONASS @@ -195,12 +193,10 @@ enum class NmeaTalkerId : int /** * @brief NMEA-Gx-GGA quality indicator - * - * @note Do not use <, >, >=, <= operators on this! */ enum class NmeaQualityGga : int { // clang-format off - UNSPECIFIED = '?', //!< Unspecified + UNSPECIFIED = '!', //!< Unspecified NOFIX = '0', //!< No fix SPP = '1', //!< Autonomous GNSS fix DGNSS = '2', //!< Differential GPS fix (e.g. with SBAS) @@ -219,7 +215,7 @@ enum class NmeaQualityGga : int */ enum class NmeaStatusGllRmc : int { // clang-format off - UNSPECIFIED = '?', //!< Unspecified + UNSPECIFIED = '!', //!< Unspecified INVALID = 'V', //!< Data invalid VALID = 'A', //!< Data valid // DIFFERENTIAL = 'D', // @todo another possible value? @@ -232,7 +228,7 @@ enum class NmeaStatusGllRmc : int */ enum class NmeaModeGllVtg : int { // clang-format off - UNSPECIFIED = '?', //!< Unspecified + UNSPECIFIED = '!', //!< Unspecified INVALID = 'N', //!< Invalid (no fix) AUTONOMOUS = 'A', //!< Autonomous mode (SPP) DGNSS = 'D', //!< Differential GNSS fix @@ -248,7 +244,7 @@ enum class NmeaModeGllVtg : int */ enum class NmeaModeRmcGns : int { // clang-format off - UNSPECIFIED = '?', //!< Unspecified + UNSPECIFIED = '!', //!< Unspecified INVALID = 'N', //!< Invalid (no fix) AUTONOMOUS = 'A', //!< Autonomous mode (SPP) DGNSS = 'D', //!< Differential GNSS fix @@ -267,7 +263,7 @@ enum class NmeaModeRmcGns : int */ enum class NmeaNavStatusRmc : int { // clang-format off - UNSPECIFIED = '?', //!< Unspecified + UNSPECIFIED = '!', //!< Unspecified SAFE = 'S', //!< Safe CAUTION = 'C', //!< Caution UNSAFE = 'U', //!< Unsafe @@ -281,19 +277,17 @@ enum class NmeaNavStatusRmc : int */ enum class NmeaOpModeGsa : int { // clang-format off - UNSPECIFIED = '?', //!< Unspecified + UNSPECIFIED = '!', //!< Unspecified MANUAL = 'M', //!< Manual AUTO = 'A', //!< Automatic }; // clang-format on /** * @brief NMEA-Gx-GNS nav mode - * - * @note Do not use <, >, >=, <= operators on this! */ enum class NmeaNavModeGsa : int { // clang-format off - UNSPECIFIED = '?', //!< Unspecified + UNSPECIFIED = '!', //!< Unspecified NOFIX = '1', //!< No fix FIX2D = '2', //!< 2D fix FIX3D = '3', //!< 3D fix @@ -301,10 +295,12 @@ enum class NmeaNavModeGsa : int /** * @brief NMEA system IDs + * + * @note Do not use <, >, >=, <= operators on this! */ enum class NmeaSystemId : int { // clang-format off - UNSPECIFIED = '?', //!< Unspecified + UNSPECIFIED = '!', //!< Unspecified GPS_SBAS = '1', //!< GPS and/or SBAS GLO = '2', //!< GLONASS GAL = '3', //!< Galileo @@ -315,10 +311,12 @@ enum class NmeaSystemId : int /** * @brief NMEA signal IDs + * + * @note Do not use <, >, >=, <= operators on this! */ enum class NmeaSignalId : int { // clang-format off - UNSPECIFIED = '?', //!< Unspecified + UNSPECIFIED = '!', //!< Unspecified GPS_L1CA = '1', //!< GPS L1 C/A GPS_L2CL = '6', //!< GPS L2 CL GPS_L2CM = '5', //!< GPS L2 CM diff --git a/fpsdk_common/include/fpsdk_common/parser/novb.hpp b/fpsdk_common/include/fpsdk_common/parser/novb.hpp index ae1add3..686fede 100644 --- a/fpsdk_common/include/fpsdk_common/parser/novb.hpp +++ b/fpsdk_common/include/fpsdk_common/parser/novb.hpp @@ -26,6 +26,7 @@ #define __FPSDK_COMMON_PARSER_NOVB_HPP__ /* LIBC/STL */ +#include #include /* EXTERNAL */ @@ -228,6 +229,34 @@ struct NovbShortHeader static_assert(sizeof(NovbShortHeader) == NOV_B_HEAD_SIZE_SHORT, ""); +enum class NovbMsgTypeSource : uint8_t; // forward declaration + +/** + * @brief Union of NOV_B long and short header + */ +struct NovbHeader +{ + union + { + NovbLongHeader long_header; //!< Long header + NovbShortHeader short_header; //!< Short header + }; + /** + * @brief Check for long vs short header + * + * @returns true if long_header is valid, false if short_header is valid + */ + bool IsLongHeader() const; + /** + * @brief Get message type source + * + * @returns the message type source (NovbMsgTypeSource::_MASK if unknown) + */ + NovbMsgTypeSource Source() const; +}; + +static_assert(sizeof(NovbHeader) == sizeof(NovbLongHeader), ""); + /** * @brief Message type measurement source (bits 4..0 of NovbLongHeader.message_type) */ diff --git a/fpsdk_common/include/fpsdk_common/time.hpp b/fpsdk_common/include/fpsdk_common/time.hpp index 5a75ec7..14fc15f 100644 --- a/fpsdk_common/include/fpsdk_common/time.hpp +++ b/fpsdk_common/include/fpsdk_common/time.hpp @@ -25,6 +25,7 @@ #include #include #include +#include /* EXTERNAL */ diff --git a/fpsdk_common/src/parser/fpa.cpp b/fpsdk_common/src/parser/fpa.cpp index d44f582..852648e 100644 --- a/fpsdk_common/src/parser/fpa.cpp +++ b/fpsdk_common/src/parser/fpa.cpp @@ -899,7 +899,7 @@ bool FpaRawimuPayload::SetFromMsg(const uint8_t* msg, const std::size_t msg_size GetFloatArr(rot, m.fields_, 5, false)); } if (ok) { - which = Which::FP_A_RAWIMU; + which = Which::RAWIMU; } FPA_TRACE("FpaRawimuPayload %s", ok ? "true" : "false"); return ok; @@ -918,7 +918,7 @@ bool FpaCorrimuPayload::SetFromMsg(const uint8_t* msg, const std::size_t msg_siz GetFloatArr(rot, m.fields_, 5, false)); } if (ok) { - which = Which::FP_A_CORRIMU; + which = Which::CORRIMU; } FPA_TRACE("FpaCorrimuPayload %s", ok ? "true" : "false"); return ok; @@ -977,10 +977,10 @@ bool FpaOdometryPayload::SetFromMsg(const uint8_t* msg, const std::size_t msg_si GetImuStatusLegacy(imu_bias_status, m.fields_[19]) && GetGnssFix(gnss1_fix, m.fields_[20]) && GetGnssFix(gnss2_fix, m.fields_[21]) && GetWsStatusLegacy(wheelspeed_status, m.fields_[22]) && GetFloatArr(pos_cov, m.fields_, 23, false) && GetFloatArr(orientation_cov, m.fields_, 29, false) && - GetFloatArr(vel_cov, m.fields_, 35, false)); + GetFloatArr(vel_cov, m.fields_, 35, false) && GetText(version, sizeof(version), m.fields_[41])); } if (ok) { - which = Which::FP_A_ODOMETRY; + which = Which::ODOMETRY; } FPA_TRACE("FpaOdometryPayload %s", ok ? "true" : "false"); return ok; @@ -1003,7 +1003,7 @@ bool FpaOdomenuPayload::SetFromMsg(const uint8_t* msg, const std::size_t msg_siz GetFloatArr(vel_cov, m.fields_, 35, false)); } if (ok) { - which = Which::FP_A_ODOMENU; + which = Which::ODOMENU; } FPA_TRACE("FpaOdomenuPayload %s", ok ? "true" : "false"); return ok; @@ -1026,7 +1026,7 @@ bool FpaOdomshPayload::SetFromMsg(const uint8_t* msg, const std::size_t msg_size GetFloatArr(vel_cov, m.fields_, 35, false)); } if (ok) { - which = Which::FP_A_ODOMSH; + which = Which::ODOMSH; } FPA_TRACE("FpaOdomshPayload %s", ok ? "true" : "false"); return ok; diff --git a/fpsdk_common/src/parser/novb.cpp b/fpsdk_common/src/parser/novb.cpp index 6a1e858..a2fa2f8 100644 --- a/fpsdk_common/src/parser/novb.cpp +++ b/fpsdk_common/src/parser/novb.cpp @@ -26,6 +26,7 @@ /* PACKAGE */ #include "fpsdk_common/parser/novb.hpp" +#include "fpsdk_common/types.hpp" namespace fpsdk { namespace common { @@ -94,6 +95,24 @@ bool NovbGetMessageName(char* name, const std::size_t size, const uint8_t* msg, // --------------------------------------------------------------------------------------------------------------------- +bool NovbHeader::IsLongHeader() const +{ + return long_header.sync3 == NOV_B_SYNC_3_LONG; +} + +NovbMsgTypeSource NovbHeader::Source() const +{ + if (long_header.sync3 == NOV_B_SYNC_3_LONG) { + return static_cast( + long_header.message_type & fpsdk::common::types::EnumToVal(NovbMsgTypeSource::_MASK)); + + } else { + return NovbMsgTypeSource::_MASK; + } +} + +// --------------------------------------------------------------------------------------------------------------------- + const char* NovbMsgTypeSourceStr(const uint8_t message_type) { // clang-format off diff --git a/fpsdk_common/test/parser_fpa_test.cpp b/fpsdk_common/test/parser_fpa_test.cpp index 622df79..f281443 100644 --- a/fpsdk_common/test/parser_fpa_test.cpp +++ b/fpsdk_common/test/parser_fpa_test.cpp @@ -344,7 +344,7 @@ TEST(FpaTest, FpaRawimuPayload) const char* msg = // clang-format off "$FP,RAWIMU,1,2197,126191.777855,-0.199914,0.472851,9.917973,0.023436,0.007723,0.002131*34\r\n"; // clang-format on EXPECT_TRUE(payload.SetFromMsg((const uint8_t*)msg, strlen(msg))); - EXPECT_EQ(payload.which, FpaImuPayload::Which::FP_A_RAWIMU); + EXPECT_EQ(payload.which, FpaImuPayload::Which::RAWIMU); EXPECT_TRUE(payload.gps_time.week.valid); EXPECT_EQ(payload.gps_time.week.value, 2197); EXPECT_TRUE(payload.gps_time.tow.valid); @@ -363,7 +363,7 @@ TEST(FpaTest, FpaRawimuPayload) const char* msg = // clang-format off "$FP,RAWIMU,1,,,,,,,,*XX\r\n"; // clang-format on EXPECT_TRUE(payload.SetFromMsg((const uint8_t*)msg, strlen(msg))); - EXPECT_EQ(payload.which, FpaImuPayload::Which::FP_A_RAWIMU); + EXPECT_EQ(payload.which, FpaImuPayload::Which::RAWIMU); EXPECT_FALSE(payload.gps_time.week.valid); EXPECT_EQ(payload.gps_time.week.value, 0); EXPECT_FALSE(payload.gps_time.tow.valid); @@ -388,7 +388,7 @@ TEST(FpaTest, FpaCorrimuPayload) const char* msg = // clang-format off "$FP,CORRIMU,1,2197,126191.777855,-0.195224,0.393969,9.869998,0.013342,-0.004620,-0.000728*7D\r\n"; // clang-format on EXPECT_TRUE(payload.SetFromMsg((const uint8_t*)msg, strlen(msg))); - EXPECT_EQ(payload.which, FpaImuPayload::Which::FP_A_CORRIMU); + EXPECT_EQ(payload.which, FpaImuPayload::Which::CORRIMU); EXPECT_TRUE(payload.gps_time.week.valid); EXPECT_EQ(payload.gps_time.week.value, 2197); EXPECT_TRUE(payload.gps_time.tow.valid); @@ -407,7 +407,7 @@ TEST(FpaTest, FpaCorrimuPayload) const char* msg = // clang-format off "$FP,CORRIMU,1,,,,,,,,*XX\r\n"; // clang-format on EXPECT_TRUE(payload.SetFromMsg((const uint8_t*)msg, strlen(msg))); - EXPECT_EQ(payload.which, FpaImuPayload::Which::FP_A_CORRIMU); + EXPECT_EQ(payload.which, FpaImuPayload::Which::CORRIMU); EXPECT_FALSE(payload.gps_time.week.valid); EXPECT_EQ(payload.gps_time.week.value, 0); EXPECT_FALSE(payload.gps_time.tow.valid); @@ -547,7 +547,7 @@ TEST(FpaTest, FpaOdometryPayload) "0.01761,0.02274,0.01713,-0.00818,0.00235,0.00129,0.00013,0.00015,0.00014,-0.00001,0.00001,0.00002," "0.03482,0.06244,0.05480,0.00096,0.00509,0.00054,fp_release_vr2_2.54.0_160*4F\r\n"; // clang-format on EXPECT_TRUE(payload.SetFromMsg((const uint8_t*)msg, strlen(msg))); - EXPECT_EQ(payload.which, FpaOdomPayload::Which::FP_A_ODOMETRY); + EXPECT_EQ(payload.which, FpaOdomPayload::Which::ODOMETRY); EXPECT_TRUE(payload.gps_time.week.valid); EXPECT_EQ(payload.gps_time.week.value, 2231); EXPECT_TRUE(payload.gps_time.tow.valid); @@ -599,6 +599,7 @@ TEST(FpaTest, FpaOdometryPayload) EXPECT_NEAR(payload.vel_cov.values[3], 0.00096, 1e-9); EXPECT_NEAR(payload.vel_cov.values[4], 0.00509, 1e-9); EXPECT_NEAR(payload.vel_cov.values[5], 0.00054, 1e-9); + EXPECT_EQ(std::string(payload.version), "fp_release_vr2_2.54.0_160"); } } @@ -614,11 +615,12 @@ TEST(FpaTest, FpaOdomenuPayload) "-0.00149,-0.00084,0.00025,0.00003,0.00003,0.00012,0.00000,0.00000,0.00000,0.01742,0.01146,0.01612," "-0.00550,-0.00007,-0.00050*74\r\n"; // clang-format on EXPECT_TRUE(payload.SetFromMsg((const uint8_t*)msg, strlen(msg))); - EXPECT_EQ(payload.which, FpaOdomPayload::Which::FP_A_ODOMENU); + EXPECT_EQ(payload.which, FpaOdomPayload::Which::ODOMENU); EXPECT_TRUE(payload.gps_time.week.valid); EXPECT_EQ(payload.gps_time.week.value, 2180); EXPECT_TRUE(payload.gps_time.tow.valid); EXPECT_NEAR(payload.gps_time.tow.value, 298591.500000, 1e-9); + EXPECT_TRUE(std::string(payload.version).empty()); } } @@ -634,11 +636,12 @@ TEST(FpaTest, FpaOdomshPayload) "-0.00149,-0.00084,0.00025,0.00003,0.00003,0.00012,0.00000,0.00000,0.00000,0.01742,0.01146,0.01612," "-0.00550,-0.00007,-0.00050*74\r\n"; // clang-format on EXPECT_TRUE(payload.SetFromMsg((const uint8_t*)msg, strlen(msg))); - EXPECT_EQ(payload.which, FpaOdomPayload::Which::FP_A_ODOMSH); + EXPECT_EQ(payload.which, FpaOdomPayload::Which::ODOMSH); EXPECT_TRUE(payload.gps_time.week.valid); EXPECT_EQ(payload.gps_time.week.value, 2180); EXPECT_TRUE(payload.gps_time.tow.valid); EXPECT_NEAR(payload.gps_time.tow.value, 298591.500000, 1e-9); + EXPECT_TRUE(std::string(payload.version).empty()); } } diff --git a/fpsdk_doc/README.md b/fpsdk_doc/README.md index 34e16ef..266f9f1 100644 --- a/fpsdk_doc/README.md +++ b/fpsdk_doc/README.md @@ -24,7 +24,7 @@ For building the libraries and apps: - Eigen3 (≥ 3.3.7, tested with 3.3.7, 3.4.0) - yaml-cpp (≥ 0.6.2, tested with 0.6.2, 0.7.0, 0.8.0) - zlib1g (≥ 1.2.11, tested with 1.2.11, 1.2.13, 1.3) -- PROJ (*) (≥ 9.?.?, tested with 9.4.1) +- PROJ (*) (≥ 9.4.x, tested with 9.4.0, 9.4.1) - ROS1 (*) (Noetic), or - ROS2 (*) (Humble or Jazzy) diff --git a/fpsdk_ros1/include/fpsdk_ros1/utils.hpp b/fpsdk_ros1/include/fpsdk_ros1/utils.hpp index 7ecf874..fb0923d 100644 --- a/fpsdk_ros1/include/fpsdk_ros1/utils.hpp +++ b/fpsdk_ros1/include/fpsdk_ros1/utils.hpp @@ -24,6 +24,7 @@ #include /* EXTERNAL */ +#include #include /* Fixposition SDK */ @@ -52,8 +53,12 @@ namespace utils { * - WARNING --> WARN * - ERROR --> ERROR * - FATAL --> FATAL + * + * @param[in] logger_name The name of the logger. The default value should give the caller package's + * ROSCONSOLE_DEFAULT_NAME, for example, "ros1_fpsdk_demo". That is, typically this argument + * should be left empty (the default value). */ -void RedirectLoggingToRosConsole(); +void RedirectLoggingToRosConsole(const char* logger_name = ROSCONSOLE_DEFAULT_NAME /* = caller's package name */); /** * @brief Loads a parameter from the ROS parameter server (int) diff --git a/fpsdk_ros1/src/utils.cpp b/fpsdk_ros1/src/utils.cpp index 58d5b17..48e8336 100644 --- a/fpsdk_ros1/src/utils.cpp +++ b/fpsdk_ros1/src/utils.cpp @@ -12,6 +12,7 @@ */ /* LIBC/STL */ +#include /* EXTERNAL */ #include "fpsdk_ros1/ext/ros.hpp" @@ -31,26 +32,31 @@ using namespace fpsdk::common::logging; // --------------------------------------------------------------------------------------------------------------------- +static char g_logger_name[100]; + static void sLoggingFn(const LoggingParams& /*params*/, const LoggingLevel level, const char* str) { - // These will appear under the "ros.fpsdk_ros1" logger. - // @todo Can we make this log to "ros1.fpsdk_common"? switch (level) { // clang-format off - case LoggingLevel::TRACE: ROS_DEBUG("%s", str); break; - case LoggingLevel::DEBUG: ROS_DEBUG("%s", str); break; - case LoggingLevel::INFO: ROS_INFO( "%s", str); break; - case LoggingLevel::NOTICE: ROS_INFO( "%s", str); break; - case LoggingLevel::WARNING: ROS_WARN( "%s", str); break; - case LoggingLevel::ERROR: ROS_ERROR("%s", str); break; - case LoggingLevel::FATAL: ROS_FATAL("%s", str); break; + case LoggingLevel::TRACE: ROS_LOG(ros::console::levels::Debug, g_logger_name, "%s", str); break; + case LoggingLevel::DEBUG: ROS_LOG(ros::console::levels::Debug, g_logger_name, "%s", str); break; + case LoggingLevel::INFO: ROS_LOG(ros::console::levels::Info, g_logger_name, "%s", str); break; + case LoggingLevel::NOTICE: ROS_LOG(ros::console::levels::Info, g_logger_name, "%s", str); break; + case LoggingLevel::WARNING: ROS_LOG(ros::console::levels::Warn, g_logger_name, "%s", str); break; + case LoggingLevel::ERROR: ROS_LOG(ros::console::levels::Error, g_logger_name, "%s", str); break; + case LoggingLevel::FATAL: ROS_LOG(ros::console::levels::Fatal, g_logger_name, "%s", str); break; } // clang-format on } -void RedirectLoggingToRosConsole() +void RedirectLoggingToRosConsole(const char* logger_name) { LoggingParams params = LoggingGetParams(); params.fn_ = sLoggingFn; params.level_ = LoggingLevel::TRACE; // We leave it up to ROS to decide what to print + // Set logger name. Note that ROSCONSOLE_DEFAULT_NAME here is the "ros.fpsdk_ros1" package name. However, when + // called from the app (node) then the default argument to logger_name is that package's ROSCONSOLE_DEFAULT_NAME, + // e.g. "ros.ros1_fpsdk_demo" + std::snprintf( + g_logger_name, sizeof(g_logger_name), "%s", logger_name != NULL ? logger_name : ROSCONSOLE_DEFAULT_NAME); LoggingSetParams(params); } diff --git a/fpsdk_ros2/include/fpsdk_ros2/utils.hpp b/fpsdk_ros2/include/fpsdk_ros2/utils.hpp index 371bfaf..6dcca65 100644 --- a/fpsdk_ros2/include/fpsdk_ros2/utils.hpp +++ b/fpsdk_ros2/include/fpsdk_ros2/utils.hpp @@ -42,15 +42,17 @@ namespace utils { * This configures the fpsdk::common::logging facility to output via the ROS console. This does *not* configure the ROS * console (logger level, logger name, etc.). * - * The mapping of fpsdk::common::logging::LoggingLevel to ros::console::levels is as follows: + * The mapping of fpsdk::common::logging::LoggingLevel to rclcpp levels is as follows: * * - TRACE and DEBUG --> DEBUG * - INFO and NOTICE --> INFO * - WARNING --> WARN * - ERROR --> ERROR * - FATAL --> FATAL + * + * @param[in] logger_name The name of the logger. The recommended value is node->get_logger().get_name() */ -void RedirectLoggingToRosConsole(); +void RedirectLoggingToRosConsole(const char* logger_name = "fpsdk_ros2"); /** * @brief Convert to ROS time (atomic -> POSIX) diff --git a/fpsdk_ros2/src/utils.cpp b/fpsdk_ros2/src/utils.cpp index c6dfad1..00ca196 100644 --- a/fpsdk_ros2/src/utils.cpp +++ b/fpsdk_ros2/src/utils.cpp @@ -47,9 +47,9 @@ static void sLoggingFn(const LoggingParams& /*params*/, const LoggingLevel level } // clang-format on } -void RedirectLoggingToRosConsole() +void RedirectLoggingToRosConsole(const char* logger_name) { - g_logger = std::make_unique(rclcpp::get_logger("fpsdk_ros2")); + g_logger = std::make_unique(rclcpp::get_logger(logger_name)); LoggingParams params = LoggingGetParams(); params.fn_ = sLoggingFn; params.level_ = LoggingLevel::TRACE; // We leave it up to ROS to decide what to print diff --git a/ros1_fpsdk_demo/launch/console.conf b/ros1_fpsdk_demo/launch/console.conf index 503d9d5..f8251b1 100644 --- a/ros1_fpsdk_demo/launch/console.conf +++ b/ros1_fpsdk_demo/launch/console.conf @@ -1,3 +1,2 @@ log4j.logger.ros=INFO -log4j.logger.ros.fpsdk_ros1=DEBUG log4j.logger.ros.ros1_fpsdk_demo=DEBUG diff --git a/ros1_fpsdk_demo/src/node_main.cpp b/ros1_fpsdk_demo/src/node_main.cpp index aa3a37f..2ea4cf6 100644 --- a/ros1_fpsdk_demo/src/node_main.cpp +++ b/ros1_fpsdk_demo/src/node_main.cpp @@ -35,6 +35,7 @@ int main(int argc, char** argv) { #ifndef NDEBUG fpsdk::common::app::StacktraceHelper stacktrace; + WARNING("***** Running debug build *****"); #endif ROS_INFO("main() 0x%" PRIxMAX, fpsdk::common::thread::ThisThreadId()); @@ -44,10 +45,10 @@ int main(int argc, char** argv) ros::init(argc, argv, "ros1_fpsdk_demo_node"); ros::NodeHandle nh("~"); - // Redirect Fixposition SDK logging to ROS console + // Redirect Fixposition SDK logging to ROS console, all should use the "ros.ros1_fpsdk_demo" logger fpsdk::ros1::utils::RedirectLoggingToRosConsole(); - DEBUG("This is a message from fpsdk_common's logging, redirected to the ROS console"); // logger: "ros1.fpsdk_ros1" - ROS_DEBUG("This is a proper ROS console message"); // logger: "ros.ros1_fpsdk_demo" + DEBUG("This is a message from fpsdk_common's logging, redirected to the ROS console"); + ROS_DEBUG("This is a proper ROS console message"); // Handle CTRL-C / SIGINT ourselves fpsdk::common::app::SigIntHelper sigint; diff --git a/ros2_fpsdk_demo/include/ros2_fpsdk_demo/node.hpp b/ros2_fpsdk_demo/include/ros2_fpsdk_demo/node.hpp index 653d171..4f9c6b4 100644 --- a/ros2_fpsdk_demo/include/ros2_fpsdk_demo/node.hpp +++ b/ros2_fpsdk_demo/include/ros2_fpsdk_demo/node.hpp @@ -14,9 +14,10 @@ #define __ROS2_FPSDK_DEMO_NODE_HPP__ /* LIBC/STL */ +#include /* EXTERNAL */ -#include +#include #include /* Fixposition SDK */ @@ -31,7 +32,7 @@ class DemoParams { public: DemoParams(); - bool LoadFromRos(std::shared_ptr node); + bool LoadFromRos(std::shared_ptr nh, const std::string& ns = ""); double worker1_interval_; double worker2_interval_; @@ -42,17 +43,18 @@ class DemoParams static constexpr double INTERVAL_MAX = 10.0; }; -class DemoNode : public rclcpp::Node +class DemoNode { public: - DemoNode(); + DemoNode(std::shared_ptr nh, const DemoParams& params); ~DemoNode(); - bool LoadParams(); bool Start(); void Stop(); private: + std::shared_ptr nh_; DemoParams params_; + rclcpp::Logger logger_; fpsdk::common::thread::Thread worker1_; fpsdk::common::thread::Thread worker2_; rclcpp::TimerBase::SharedPtr timer1_; diff --git a/ros2_fpsdk_demo/launch/console.conf b/ros2_fpsdk_demo/launch/console.conf deleted file mode 100644 index 503d9d5..0000000 --- a/ros2_fpsdk_demo/launch/console.conf +++ /dev/null @@ -1,3 +0,0 @@ -log4j.logger.ros=INFO -log4j.logger.ros.fpsdk_ros1=DEBUG -log4j.logger.ros.ros1_fpsdk_demo=DEBUG diff --git a/ros2_fpsdk_demo/src/node.cpp b/ros2_fpsdk_demo/src/node.cpp index 32e9a8e..cc2c7ac 100644 --- a/ros2_fpsdk_demo/src/node.cpp +++ b/ros2_fpsdk_demo/src/node.cpp @@ -37,15 +37,15 @@ DemoParams::DemoParams() /* clang-format off */ : // --------------------------------------------------------------------------------------------------------------------- -bool DemoParams::LoadFromRos(std::shared_ptr node) +bool DemoParams::LoadFromRos(std::shared_ptr node, const std::string& ns) { bool ok = true; // Declare used params, use as default whatever we the object has currently - const std::string WORKER1_INTERVAL_NAME = "worker1_interval"; - const std::string WORKER2_INTERVAL_NAME = "worker2_interval"; - const std::string TIMER1_INTERVAL_NAME = "timer1_interval"; - const std::string TIMER2_INTERVAL_NAME = "timer2_interval"; + const std::string WORKER1_INTERVAL_NAME = ns + ".worker1_interval"; + const std::string WORKER2_INTERVAL_NAME = ns + ".worker2_interval"; + const std::string TIMER1_INTERVAL_NAME = ns + ".timer1_interval"; + const std::string TIMER2_INTERVAL_NAME = ns + ".timer2_interval"; node->declare_parameter(WORKER1_INTERVAL_NAME, worker1_interval_); node->declare_parameter(WORKER2_INTERVAL_NAME, worker2_interval_); node->declare_parameter(TIMER1_INTERVAL_NAME, timer1_interval_); @@ -96,16 +96,18 @@ bool DemoParams::LoadFromRos(std::shared_ptr node) /* ****************************************************************************************************************** */ -DemoNode::DemoNode() /* clang-format off */ : - Node("demo_node"), +DemoNode::DemoNode(std::shared_ptr nh, const DemoParams& params) /* clang-format off */ : + nh_ { nh }, + params_ { params }, + logger_ { nh_->get_logger() }, worker1_ { "worker1", std::bind(&DemoNode::Worker1, this, std::placeholders::_1) }, worker2_ { "worker2", std::bind(&DemoNode::Worker2, this, std::placeholders::_1) } // clang-format on { - RCLCPP_DEBUG(this->get_logger(), "i am debug"); - RCLCPP_INFO(this->get_logger(), "i am info"); - RCLCPP_WARN(this->get_logger(), "i am warn"); - RCLCPP_ERROR(this->get_logger(), "i am error"); - RCLCPP_FATAL(this->get_logger(), "i am fatal"); + RCLCPP_DEBUG(logger_, "i am debug"); + RCLCPP_INFO(logger_, "i am info"); + RCLCPP_WARN(logger_, "i am warn"); + RCLCPP_ERROR(logger_, "i am error"); + RCLCPP_FATAL(logger_, "i am fatal"); } // --------------------------------------------------------------------------------------------------------------------- @@ -117,25 +119,19 @@ DemoNode::~DemoNode() // --------------------------------------------------------------------------------------------------------------------- -bool DemoNode::LoadParams() -{ - return params_.LoadFromRos(shared_from_this()); -} - -// --------------------------------------------------------------------------------------------------------------------- - bool DemoNode::Start() { - RCLCPP_DEBUG(this->get_logger(), "DemoNode::Start() %s %s", get_name(), get_namespace()); - timer1_ = this->create_wall_timer( + RCLCPP_DEBUG(logger_, "DemoNode::Start() namespace=%s name=%s", nh_->get_namespace(), nh_->get_name()); + timer1_ = nh_->create_wall_timer( rclcpp::Duration::from_seconds(params_.timer1_interval_).to_chrono(), std::bind(&DemoNode::Timer1, this)); - timer2_ = this->create_wall_timer( + timer2_ = nh_->create_wall_timer( rclcpp::Duration::from_seconds(params_.timer2_interval_).to_chrono(), std::bind(&DemoNode::Timer2, this)); - const std::string topic_name = std::string(get_namespace()) + std::string(get_name()) + "/string"; - publisher_ = this->create_publisher(topic_name, 5); + const std::string topic_name = std::string(nh_->get_namespace()) + std::string(nh_->get_name()) + "/string"; + RCLCPP_DEBUG(logger_, "topic_name: %s", topic_name.c_str()); + publisher_ = nh_->create_publisher(topic_name, 5); return worker1_.Start() && worker2_.Start(); } @@ -144,7 +140,7 @@ bool DemoNode::Start() void DemoNode::Stop() { - RCLCPP_DEBUG(this->get_logger(), "DemoNode::Stop()"); + RCLCPP_DEBUG(logger_, "DemoNode::Stop()"); // timer1_.stop(); // timer2_.stop(); worker1_.Stop(); @@ -156,39 +152,39 @@ void DemoNode::Stop() void DemoNode::Worker1(void* /*arg*/) { - RCLCPP_DEBUG(this->get_logger(), "DemoNode::Worker1() start 0x%" PRIxMAX, fpsdk::common::thread::ThisThreadId()); + RCLCPP_DEBUG(logger_, "DemoNode::Worker1() start 0x%" PRIxMAX, fpsdk::common::thread::ThisThreadId()); while (!worker1_.ShouldAbort()) { - RCLCPP_DEBUG(this->get_logger(), "DemoNode::Worker1() ..."); + RCLCPP_DEBUG(logger_, "DemoNode::Worker1() 0x%" PRIxMAX, fpsdk::common::thread::ThisThreadId()); auto msg = std_msgs::msg::String(); msg.data = "worker1..."; publisher_->publish(msg); rclcpp::sleep_for( rclcpp::Duration::from_seconds(params_.worker1_interval_).to_chrono()); } - RCLCPP_DEBUG(this->get_logger(), "DemoNode::Worker1() done"); + RCLCPP_DEBUG(logger_, "DemoNode::Worker1() done"); } // --------------------------------------------------------------------------------------------------------------------- void DemoNode::Worker2(void* /*arg*/) { - RCLCPP_DEBUG(this->get_logger(), "DemoNode::Worker2() start 0x%" PRIxMAX, fpsdk::common::thread::ThisThreadId()); + RCLCPP_DEBUG(logger_, "DemoNode::Worker2() start 0x%" PRIxMAX, fpsdk::common::thread::ThisThreadId()); while (!worker2_.ShouldAbort()) { - RCLCPP_DEBUG(this->get_logger(), "DemoNode::Worker2() ..."); + RCLCPP_DEBUG(logger_, "DemoNode::Worker2() 0x%" PRIxMAX, fpsdk::common::thread::ThisThreadId()); auto msg = std_msgs::msg::String(); msg.data = "worker2..."; publisher_->publish(msg); rclcpp::sleep_for( rclcpp::Duration::from_seconds(params_.worker2_interval_).to_chrono()); } - RCLCPP_DEBUG(this->get_logger(), "DemoNode::Worker() done"); + RCLCPP_DEBUG(logger_, "DemoNode::Worker() done"); } // --------------------------------------------------------------------------------------------------------------------- void DemoNode::Timer1() { - RCLCPP_DEBUG(this->get_logger(), "DemoNode::Timer1() 0x%" PRIxMAX, fpsdk::common::thread::ThisThreadId()); + RCLCPP_DEBUG(logger_, "DemoNode::Timer1() 0x%" PRIxMAX, fpsdk::common::thread::ThisThreadId()); auto msg = std_msgs::msg::String(); msg.data = "timer1..."; publisher_->publish(msg); @@ -198,7 +194,7 @@ void DemoNode::Timer1() void DemoNode::Timer2() { - RCLCPP_DEBUG(this->get_logger(), "DemoNode::Timer2() 0x%" PRIxMAX, fpsdk::common::thread::ThisThreadId()); + RCLCPP_DEBUG(logger_, "DemoNode::Timer2() 0x%" PRIxMAX, fpsdk::common::thread::ThisThreadId()); auto msg = std_msgs::msg::String(); msg.data = "timer2..."; publisher_->publish(msg); diff --git a/ros2_fpsdk_demo/src/node_main.cpp b/ros2_fpsdk_demo/src/node_main.cpp index 9e3f4cf..6618677 100644 --- a/ros2_fpsdk_demo/src/node_main.cpp +++ b/ros2_fpsdk_demo/src/node_main.cpp @@ -30,47 +30,66 @@ /* ****************************************************************************************************************** */ +using namespace Ros2FpsdkDemo; + int main(int argc, char* argv[]) { #ifndef NDEBUG fpsdk::common::app::StacktraceHelper stacktrace; + WARNING("***** Running debug build *****"); #endif - auto logger = rclcpp::get_logger("node_main"); - RCLCPP_INFO(logger, "main() 0x%" PRIxMAX, fpsdk::common::thread::ThisThreadId()); + INFO("main() 0x%" PRIxMAX, fpsdk::common::thread::ThisThreadId()); bool ok = true; - // Initialise + // Initialise ROS, create node handle rclcpp::init(argc, argv); - - // Redirect Fixposition SDK logging to ROS console - fpsdk::ros2::utils::RedirectLoggingToRosConsole(); - DEBUG("This is a message from fpsdk_common's logging, redirected to the ROS console"); // logger: "fpsdk_ros2" - RCLCPP_DEBUG(logger, "This is a proper ROS console message"); // logger: "node_main" + auto nh = std::make_shared("ros2_fpsdk_demo"); + auto logger = nh->get_logger(); + + // Redirect Fixposition SDK logging to ROS console, all should use the "ros2_fpsdk_demo" logger + fpsdk::ros2::utils::RedirectLoggingToRosConsole(logger.get_name()); + DEBUG("This is a message from fpsdk_common's logging, redirected to the ROS console"); + RCLCPP_DEBUG(logger, "This is a proper ROS console message"); + + // Load parameters + RCLCPP_INFO(logger, "Loading parameters..."); + DemoParams params; + if (!params.LoadFromRos(nh)) { + RCLCPP_ERROR(logger, "Failed loading parameters"); + ok = false; + } // Handle CTRL-C / SIGINT ourselves fpsdk::common::app::SigIntHelper sigint; - // Create node. This doesn't do much yet. - auto node = std::make_shared(); - - // Load params - if (node->LoadParams()) { - // Start node + // Start node + std::unique_ptr node; + if (ok) { + try { + node = std::make_unique(nh, params); + } catch (const std::exception& ex) { + RCLCPP_ERROR(logger, "Failed creating node: %s", ex.what()); + ok = false; + } + } + if (ok) { + RCLCPP_INFO(logger, "Starting node..."); if (node->Start()) { RCLCPP_INFO(logger, "main() spinning..."); #if 1 - // Do the same as rclpp::spin(), but also handle CTRL-C / SIGINT nicely + // Do the same as rclpp::spin(nh), but also handle CTRL-C / SIGINT nicely // Callbacks execute in main thread while (rclcpp::ok() && !sigint.ShouldAbort()) { rclcpp::spin_until_future_complete( - node, std::promise().get_future(), std::chrono::milliseconds(345)); + nh, std::promise().get_future(), std::chrono::milliseconds(345)); } #else + // Use multiple spinner threads. Callback execute in one of them. - // TODO: this doesn't seem to work + // TODO: this (executing in those threads) doesn't seem to work rclcpp::executors::MultiThreadedExecutor executor{ rclcpp::ExecutorOptions(), 4 }; - executor.add_node(node); + executor.add_node(nh); while (rclcpp::ok() && !sigint.ShouldAbort()) { executor.spin_once(std::chrono::milliseconds(345)); } @@ -80,13 +99,11 @@ int main(int argc, char* argv[]) RCLCPP_ERROR(logger, "Failed starting node"); ok = false; } - } else { - RCLCPP_ERROR(logger, "Failed loading parameters"); - ok = false; + node->Stop(); + node.reset(); + nh.reset(); } - node->Stop(); - // Are we happy? if (ok) { RCLCPP_INFO(logger, "Done");