diff --git a/docker/ci.sh b/docker/ci.sh index facb015d..505b7d6d 100755 --- a/docker/ci.sh +++ b/docker/ci.sh @@ -69,11 +69,13 @@ function do_step echo "----- $func: ${TITLES[$func]} -----" ((NSTEPS=${NSTEPS} + 1)) + set -x if ! ${func}; then res=1 ((ERROR_COUNT=${ERROR_COUNT} + 1)) ERROR_NAMES="${ERROR_NAMES} ${func}" fi + set +x echo "::endgroup::" if [ ${res} -ne 0 ]; then diff --git a/examples/ros2_fpsdk_demo/CMakeLists.txt b/examples/ros2_fpsdk_demo/CMakeLists.txt index 632e4433..865206c2 100644 --- a/examples/ros2_fpsdk_demo/CMakeLists.txt +++ b/examples/ros2_fpsdk_demo/CMakeLists.txt @@ -34,6 +34,11 @@ find_package(std_msgs REQUIRED) find_package(fpsdk_common REQUIRED) find_package(fpsdk_ros2 REQUIRED) find_package(rosbag2_cpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2_msgs REQUIRED) +find_package(nav_msgs REQUIRED) + # include_directories(include ${rclcpp_INCLUDE_DIRS} ${rmw_INCLUDE_DIRS} ${fpsdk_common_INCLUDE_DIRS}) diff --git a/fpsdk_apps/CMakeLists.txt b/fpsdk_apps/CMakeLists.txt index 71566bfc..65613a23 100644 --- a/fpsdk_apps/CMakeLists.txt +++ b/fpsdk_apps/CMakeLists.txt @@ -44,7 +44,13 @@ if(FPSDK_USE_ROS1) add_compile_definitions(FPSDK_USE_ROS1) elseif(FPSDK_USE_ROS2) add_compile_definitions(FPSDK_USE_ROS2) + # Why necessary, what are we doing wrong? See fpsdk_ros2/CMakeLists.txt find_package(rosbag2_cpp REQUIRED) + find_package(std_msgs REQUIRED) + find_package(sensor_msgs REQUIRED) + find_package(geometry_msgs REQUIRED) + find_package(tf2_msgs REQUIRED) + find_package(nav_msgs REQUIRED) if(NOT TARGET fpsdk_ros2) find_package(fpsdk_ros2 REQUIRED) endif() diff --git a/fpsdk_apps/fpltool/fpltool_extract.cpp b/fpsdk_apps/fpltool/fpltool_extract.cpp index c8bfb66a..e88f151d 100644 --- a/fpsdk_apps/fpltool/fpltool_extract.cpp +++ b/fpsdk_apps/fpltool/fpltool_extract.cpp @@ -218,10 +218,10 @@ bool DoExtract(const FplToolOptions& opts) !output.WriteStreamMsg(JSONL_NAME, streammsg, parsermsg_helper.GetParserMsg(true))) { ok = false; } -#if defined(FPSDK_USE_ROS1) // || defined(FPSDK_USE_ROS2) // @todo implement for ROS2, s.a. fpltools_utils.hpp +#if defined(FPSDK_USE_ROS1) || defined(FPSDK_USE_ROS2) if (doRos) { bag.WriteMessage(parsermsg_helper.GetRosMsg(), "/" + streammsg.stream_name_ + "/raw", - streammsg.rec_time_); + parsermsg_helper.GetRosStamp()); } #endif } else { diff --git a/fpsdk_apps/fpltool/fpltool_utils.cpp b/fpsdk_apps/fpltool/fpltool_utils.cpp index 96af156f..e1da7912 100644 --- a/fpsdk_apps/fpltool/fpltool_utils.cpp +++ b/fpsdk_apps/fpltool/fpltool_utils.cpp @@ -34,9 +34,6 @@ #include #include #include -#if defined(FPSDK_USE_ROS1) -# include -#endif /* PACKAGE */ #include "fpltool_utils.hpp" @@ -137,6 +134,9 @@ OutputFile* OutputFileHelper::GetOutputFile(const std::string& name) ParserMsgHelper::ParserMsgHelper() { +#if defined(FPSDK_USE_ROS1) || defined(FPSDK_USE_ROS2) + rosmsg_.layout.dim.resize(1); +#endif } ParserMsgHelper::~ParserMsgHelper() { @@ -158,27 +158,16 @@ void ParserMsgHelper::UpdateParserMsg(const common::fpl::StreamMsg& streammsg) } msg_.seq_ = seq->second++; -#if defined(FPSDK_USE_ROS1) - rosmsg_.stamp = { streammsg.rec_time_.sec_, streammsg.rec_time_.nsec_ }; - rosmsg_.name = msg_.name_; - rosmsg_.seq++; - rosmsg_.data = msg_.data_; - switch (msg_.proto_) { // clang-format off - case Protocol::FP_A: rosmsg_.protocol = fpsdk_ros1::ParserMsg::PROTOCOL_FP_A; break; - case Protocol::FP_B: rosmsg_.protocol = fpsdk_ros1::ParserMsg::PROTOCOL_FP_B; break; - case Protocol::NMEA: rosmsg_.protocol = fpsdk_ros1::ParserMsg::PROTOCOL_NMEA; break; - case Protocol::UBX: rosmsg_.protocol = fpsdk_ros1::ParserMsg::PROTOCOL_UBX; break; - case Protocol::RTCM3: rosmsg_.protocol = fpsdk_ros1::ParserMsg::PROTOCOL_RTCM3; break; - case Protocol::UNI_B: rosmsg_.protocol = fpsdk_ros1::ParserMsg::PROTOCOL_UNI_B; break; - case Protocol::NOV_B: rosmsg_.protocol = fpsdk_ros1::ParserMsg::PROTOCOL_NOV_B; break; - case Protocol::SPARTN: rosmsg_.protocol = fpsdk_ros1::ParserMsg::PROTOCOL_SPARTN; break; - case Protocol::SBF: rosmsg_.protocol = fpsdk_ros1::ParserMsg::PROTOCOL_SBF; break; - case Protocol::QGC: rosmsg_.protocol = fpsdk_ros1::ParserMsg::PROTOCOL_QGC; break; - case Protocol::OTHER: - default: rosmsg_.protocol = fpsdk_ros1::ParserMsg::PROTOCOL_OTHER; break; - } // clang-format on -#elif defined(FPSDK_USE_ROS2) - // @todo implement for ROS2 +#if defined(FPSDK_USE_ROS1) || defined(FPSDK_USE_ROS2) + rosmsg_.layout.dim[0].label = msg_.name_; + rosmsg_.layout.dim[0].size = msg_.data_.size(); + rosmsg_.layout.dim[0].stride = msg_.data_.size(); + rosmsg_.data = { msg_.data_.data(), msg_.data_.data() + msg_.data_.size() }; +# if defined(FPSDK_USE_ROS1) + stamp_ = { streammsg.rec_time_.sec_, streammsg.rec_time_.nsec_ }; +# else + stamp_ = { (int)streammsg.rec_time_.sec_, streammsg.rec_time_.nsec_, RCL_ROS_TIME }; +# endif #endif } @@ -190,17 +179,6 @@ const ParserMsg& ParserMsgHelper::GetParserMsg(const bool make_info) const return msg_; } -#if defined(FPSDK_USE_ROS1) -const fpsdk_ros1::ParserMsg& ParserMsgHelper::GetRosMsg() -{ - msg_.MakeInfo(); - rosmsg_.info = msg_.info_; - return rosmsg_; -} -#elif defined(FPSDK_USE_ROS2) -// @todo implement for ROS2 -#endif - // --------------------------------------------------------------------------------------------------------------------- RosMsgHelper::RosMsgHelper() diff --git a/fpsdk_apps/fpltool/fpltool_utils.hpp b/fpsdk_apps/fpltool/fpltool_utils.hpp index af7d4925..e65927cb 100644 --- a/fpsdk_apps/fpltool/fpltool_utils.hpp +++ b/fpsdk_apps/fpltool/fpltool_utils.hpp @@ -20,15 +20,18 @@ /* EXTERNAL */ #include +#if defined(FPSDK_USE_ROS1) +# include +#elif defined(FPSDK_USE_ROS2) +# include +# include +#endif /* Fixposition SDK */ #include #include #include #include -#if defined(FPSDK_USE_ROS1) -# include -#endif /* PACKAGE */ #include "fpltool_opts.hpp" @@ -66,16 +69,36 @@ class ParserMsgHelper ~ParserMsgHelper(); void UpdateParserMsg(const common::fpl::StreamMsg& streammsg); const common::parser::ParserMsg& GetParserMsg(const bool make_info = false) const; -#if defined(FPSDK_USE_ROS1) // || defined(FPSDK_USE_ROS2) // @todo implement for ROS2 - const fpsdk_ros1::ParserMsg& GetRosMsg(); +#if defined(FPSDK_USE_ROS1) + const std_msgs::ByteMultiArray& GetRosMsg() const + { + return rosmsg_; + } + const ros::Time& GetRosStamp() const + { + return stamp_; + } +#elif defined(FPSDK_USE_ROS2) + const std_msgs::msg::ByteMultiArray& GetRosMsg() const + { + return rosmsg_; + } + const rclcpp::Time& GetRosStamp() const + { + return stamp_; + } #endif private: common::parser::Parser parser_; common::parser::ParserMsg msg_; std::map seq_; -#if defined(FPSDK_USE_ROS1) // || defined(FPSDK_USE_ROS2) // @todo implement for ROS2 - fpsdk_ros1::ParserMsg rosmsg_; +#if defined(FPSDK_USE_ROS1) + std_msgs::ByteMultiArray rosmsg_; + ros::Time stamp_; +#elif defined(FPSDK_USE_ROS2) + std_msgs::msg::ByteMultiArray rosmsg_; + rclcpp::Time stamp_; #endif }; diff --git a/fpsdk_ros1/CMakeLists.txt b/fpsdk_ros1/CMakeLists.txt index dd98d5c5..c8d0ed4f 100644 --- a/fpsdk_ros1/CMakeLists.txt +++ b/fpsdk_ros1/CMakeLists.txt @@ -45,16 +45,6 @@ fpsdk_cmake_find_ros1_package(geometry_msgs) fpsdk_cmake_find_ros1_package(tf2_msgs) fpsdk_cmake_find_ros1_package(nav_msgs) fpsdk_cmake_find_ros1_package(topic_tools) -fpsdk_cmake_find_ros1_package(genmsg) - - -# MESSAGES ============================================================================================================= - -file(GLOB MSG_FILES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} msg/*.msg) - -# /opt/ros/noetic/share/genmsg/cmake/genmsg-extras.cmake -add_message_files(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} FILES ${MSG_FILES}) -generate_messages(LANGS gencpp genpy) # DEPENDENCIES std_msgs sensor_msgs) # SHARED LIBRARY ======================================================================================================= @@ -62,13 +52,10 @@ generate_messages(LANGS gencpp genpy) # DEPENDENCIES std_msgs sensor_msgs) file(GLOB CPP_FILES src/*.cpp) add_library(${PROJECT_NAME} SHARED ${CPP_FILES}) -add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages) - target_include_directories(${PROJECT_NAME} PUBLIC $ $ - $ # generate_message() puts them here ) target_link_libraries(${PROJECT_NAME} @@ -163,7 +150,6 @@ install( include(../fpsdk_common/cmake/testing.cmake) add_gtest(TARGET bagwriter_test SOURCES test/bagwriter_test.cpp LINK_LIBS ${PROJECT_NAME} ros1::rosconsole ros1::rosbag) -add_gtest(TARGET msgs_test SOURCES test/msgs_test.cpp LINK_LIBS ${PROJECT_NAME} ros1::rosconsole) add_gtest(TARGET utils_test SOURCES test/utils_test.cpp LINK_LIBS ${PROJECT_NAME} ros1::rosconsole) diff --git a/fpsdk_ros1/include/fpsdk_ros1/ext/ros_msgs.hpp b/fpsdk_ros1/include/fpsdk_ros1/ext/ros_msgs.hpp index 3600a1ee..42105f0f 100644 --- a/fpsdk_ros1/include/fpsdk_ros1/ext/ros_msgs.hpp +++ b/fpsdk_ros1/include/fpsdk_ros1/ext/ros_msgs.hpp @@ -5,8 +5,7 @@ #pragma GCC diagnostic ignored "-Wpedantic" #pragma GCC diagnostic ignored "-Wunused-parameter" #pragma GCC diagnostic ignored "-Wshadow" -#include +#include #include -#include #pragma GCC diagnostic pop #endif // __FPSDK_ROS1_EXT_ROS_MSGS_HPP__ diff --git a/fpsdk_ros1/include/fpsdk_ros1/msgs.hpp b/fpsdk_ros1/include/fpsdk_ros1/msgs.hpp deleted file mode 100644 index ec3ad6c4..00000000 --- a/fpsdk_ros1/include/fpsdk_ros1/msgs.hpp +++ /dev/null @@ -1,66 +0,0 @@ -/** - * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Copyright (c) Fixposition AG - * / /\ \ License: see the LICENSE file - * /__/ \__\ - * \endverbatim - * - * @file - * @brief Fixposition SDK: ROS1 messages - * - * @page FPSDK_ROS1_MSGS ROS1 messages - * - * **API**: fpsdk_ros1/msgs.hpp and fpsdk::ros1::msgs - * - */ -#ifndef __FPSDK_ROS1_MSGS_HPP__ -#define __FPSDK_ROS1_MSGS_HPP__ - -/* LIBC/STL */ - -/* EXTERNAL */ - -/* Fixposition SDK */ - -/* PACKAGE */ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wpedantic" -#pragma GCC diagnostic ignored "-Wunused-parameter" -#pragma GCC diagnostic ignored "-Wshadow" -#include -#pragma GCC diagnostic pop - -namespace fpsdk { -namespace ros1 { -/** - * @brief ROS1 messages - */ -namespace msgs { -/* ****************************************************************************************************************** */ - -/** - * @name ParserMsg - * - * @include{lineno} ParserMsg.msg - * - * @{ - */ - -//! ROS message (instance) similar to fpsdk::common::parser::ParserMsg -using ParserMsg = fpsdk_ros1::ParserMsg; - -//! ROS message (shared pointer) similar to fpsdk::common::parser::ParserMsg -using ParserMsgPtr = fpsdk_ros1::ParserMsgPtr; - -//! ROS message (shared const pointer) similar to fpsdk::common::parser::ParserMsg -using ParserMsgConstPtr = fpsdk_ros1::ParserMsgConstPtr; - -///@} - -/* ****************************************************************************************************************** */ -} // namespace msgs -} // namespace ros1 -} // namespace fpsdk -#endif // __FPSDK_ROS1_MSGS_HPP__ diff --git a/fpsdk_ros1/msg/ParserMsg.msg b/fpsdk_ros1/msg/ParserMsg.msg deleted file mode 100644 index 9a5fd32a..00000000 --- a/fpsdk_ros1/msg/ParserMsg.msg +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright (c) Fixposition AG (www.fixposition.com) and contributors -# License: see the LICENSE file -# -# Similar to a fpsdk::common::parser::ParserMsg -# -# !!! fpsdk_{ros1,ros2}/msg/ParserMsg.msg should be kept in sync !!! - -time stamp # Timestamp of message reception time -int8 protocol # Protocol -int8 PROTOCOL_UNSPECIFIED = 0 -int8 PROTOCOL_FP_A = 1 -int8 PROTOCOL_FP_B = 2 -int8 PROTOCOL_NMEA = 3 -int8 PROTOCOL_UBX = 4 -int8 PROTOCOL_RTCM3 = 5 -int8 PROTOCOL_UNI_B = 6 -int8 PROTOCOL_NOV_B = 7 -int8 PROTOCOL_SPARTN = 8 -int8 PROTOCOL_OTHER = 9 -int8 PROTOCOL_SBF = 10 -int8 PROTOCOL_QGC = 11 -uint8[] data # Message data -string name # Message name -uint64 seq # Message counter -string info # Message (debug) info diff --git a/fpsdk_ros1/test/msgs_test.cpp b/fpsdk_ros1/test/msgs_test.cpp deleted file mode 100644 index 97d408bb..00000000 --- a/fpsdk_ros1/test/msgs_test.cpp +++ /dev/null @@ -1,54 +0,0 @@ -/** - * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors - * / /\ \ License: see the LICENSE file - * /__/ \__\ - * \endverbatim - * - * @file - * @brief Fixposition SDK: tests for fpsdk_ros1 messages - */ - -/* LIBC/STL */ - -/* EXTERNAL */ -#include - -/* PACKAGE */ -#include -#include - -namespace { -/* ****************************************************************************************************************** */ - -// Checks that some of the messages didn't change. If we change the message definition, we change the MD5 sum of the -// message and make it binary incompatible. I.e. we cannot use the message from old bags (using the old .msg) with the -// changed .msg. In Python this is not a problem (as long no existing field is changed), but in c++ instantiating the -// message is compiled-in and not dynamic (as in Python). Note that changing the comments in a .msg file does not change -// the MD5 sum. Neither does changing the type names ("data type" in ROS speak) if done consistently (e.g. rename all -// fpsdk_ros1/Foo to fpsdk_ros1/Bar in all .msg). However, changing name of the field does change the MD5 sum, and so -// does changing the order, adding or removing fields. -TEST(MsgsTest, MustNeverChange) -{ - // clang-format off - EXPECT_EQ(std::string(ros::message_traits::md5sum()), "74030d9f5f1291a0532d94562740ed9d"); - // clang-format on -} - -/* ****************************************************************************************************************** */ -} // namespace - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - auto level = fpsdk::common::logging::LoggingLevel::WARNING; - for (int ix = 0; ix < argc; ix++) { - if ((argv[ix][0] == '-') && argv[ix][1] == 'v') { - level++; - } - } - fpsdk::common::logging::LoggingSetParams(level); - return RUN_ALL_TESTS(); -} diff --git a/fpsdk_ros2/CMakeLists.txt b/fpsdk_ros2/CMakeLists.txt index 5eba77fa..07e99dff 100644 --- a/fpsdk_ros2/CMakeLists.txt +++ b/fpsdk_ros2/CMakeLists.txt @@ -37,6 +37,7 @@ endif() find_package(rclcpp REQUIRED) find_package(rosbag2_cpp REQUIRED) +find_package(std_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2_msgs REQUIRED) @@ -61,11 +62,18 @@ target_link_libraries(${PROJECT_NAME} fpsdk_common ${rclcpp_TARGETS} ${rosbag2_cpp_TARGETS} - PRIVATE + ${std_msgs_TARGETS} ${sensor_msgs_TARGETS} ${geometry_msgs_TARGETS} ${tf2_msgs_TARGETS} ${nav_msgs_TARGETS} + # Shouldn't this work? (see fpsdk_apps/CMakeLists.txt, examples/ros2_fpsdk_demo/CMakeLists.txt) + # std_msgs::std_msgs + # sensor_msgs::sensor_msgs + # geometry_msgs::geometry_msgs + # tf2_msgs::tf2_msgs + # nav_msgs::nav_msgs + PRIVATE ) set_target_properties(${PROJECT_NAME} @@ -146,7 +154,7 @@ include(../fpsdk_common/cmake/testing.cmake) add_gtest(TARGET bagwriter_test SOURCES test/bagwriter_test.cpp LINK_LIBS ${PROJECT_NAME} ${rclcpp_TARGETS} ${rosbag2_cpp_TARGETS}) add_gtest(TARGET utils_test SOURCES test/utils_test.cpp LINK_LIBS ${PROJECT_NAME} ${rclcpp_TARGETS}) -add_gtest(TARGET ros1_test SOURCES test/ros1_test.cpp LINK_LIBS ${PROJECT_NAME} ${rclcpp_TARGETS} ${nav_msgs_TARGETS} ${sensor_msgs_TARGETS} ${tf2_msgs_TARGETS}) +add_gtest(TARGET ros1_test SOURCES test/ros1_test.cpp LINK_LIBS ${PROJECT_NAME} ${rclcpp_TARGETS} ${nav_msgs_TARGETS} ${sensor_msgs_TARGETS} ${tf2_msgs_TARGETS} ${std_msgs_TARGETS}) # ====================================================================================================================== diff --git a/fpsdk_ros2/include/fpsdk_ros2/ext/msgs.hpp b/fpsdk_ros2/include/fpsdk_ros2/ext/msgs.hpp index 7a90d66b..4dcf4df0 100644 --- a/fpsdk_ros2/include/fpsdk_ros2/ext/msgs.hpp +++ b/fpsdk_ros2/include/fpsdk_ros2/ext/msgs.hpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #pragma GCC diagnostic pop