diff --git a/create_ros_ws.sh b/create_ros_ws.sh index 8ea2e55..c7f354f 100755 --- a/create_ros_ws.sh +++ b/create_ros_ws.sh @@ -121,10 +121,8 @@ function main ln -s ${srcpath}/fixposition_driver_msgs ${abspath}/src ln -s ${srcpath}/rtcm_msgs ${abspath}/src if [ ${rosver} -eq 1 ]; then - ln -s ${srcpath}/fixposition-sdk/fpsdk_ros1 ${abspath}/src ln -s ${srcpath}/fixposition_driver_ros1 ${abspath}/src else - ln -s ${srcpath}/fixposition-sdk/fpsdk_ros2 ${abspath}/src ln -s ${srcpath}/fixposition_driver_ros2 ${abspath}/src fi @@ -133,9 +131,9 @@ function main if [ ${rosver} -eq 1 ]; then catkin init if [ ${dev} -gt 0 ]; then - catkin config --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DCMAKE_BUILD_TYPE=Debug + catkin config --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DCMAKE_BUILD_TYPE=Debug -DFPSDK_BUILD_TESTING=OFF else - catkin config --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DCMAKE_BUILD_TYPE=Release + catkin config --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DCMAKE_BUILD_TYPE=Release -DFPSDK_BUILD_TESTING=OFF fi else echo 'build:' > ${abspath}/colcon_defaults.yaml diff --git a/fixposition-sdk b/fixposition-sdk index 4d935a0..bd16faa 160000 --- a/fixposition-sdk +++ b/fixposition-sdk @@ -1 +1 @@ -Subproject commit 4d935a0f2d44b02b9e46f193a21b3c11f3a6d874 +Subproject commit bd16faa4198cabca87b9087fc8630ee9521aaa5e diff --git a/fixposition_driver.code-workspace b/fixposition_driver.code-workspace index 7dc75d4..b489e6d 100644 --- a/fixposition_driver.code-workspace +++ b/fixposition_driver.code-workspace @@ -57,15 +57,29 @@ // https://code.visualstudio.com/docs/cpp/c-cpp-properties-schema-reference) "C_Cpp.default.includePath": [ "${workspaceFolder}/fixposition-sdk/fpsdk_common/**", - "${workspaceFolder}/fixposition-sdk/fpsdk_ros1/**", - "${workspaceFolder}/fixposition-sdk/fpsdk_ros2/**", "${workspaceFolder}/fixposition-sdk/fpsdk_apps/**", "${workspaceFolder}/fixposition_driver_lib/**", "${workspaceFolder}/fixposition_driver_ros1/**", "${workspaceFolder}/fixposition_driver_ros2/**", "/opt/ros/noetic/include/**", "/opt/ros/humble/include/**", "/opt/ros/jazzy/include/**", "/opt/ros/lyrical/include/**" ], - "C_Cpp.default.defines": [ "FP_USE_ROS1" ], - //"C_Cpp.default.compileCommands": "", + "C_Cpp.default.defines": [ ], + "C_Cpp.default.compileCommands": [ + // "${workspaceFolder}/ros2_ws/build/fixposition_driver_msgs/compile_commands.json", + // "${workspaceFolder}/ros2_ws/build/fpsdk_apps/compile_commands.json", + // "${workspaceFolder}/ros2_ws/build/fixposition_driver_lib/compile_commands.json", + // "${workspaceFolder}/ros2_ws/build/fpsdk_common/compile_commands.json", + // "${workspaceFolder}/ros2_ws/build/rtcm_msgs/compile_commands.json", + // "${workspaceFolder}/ros2_ws/build/fixposition_driver_ros2/compile_commands.json", + "${workspaceFolder}/ros2_ws/build/compile_commands.json", + // + "${workspaceFolder}/ros1_ws/build/fixposition_driver_msgs/compile_commands.json", + "${workspaceFolder}/ros1_ws/build/fpsdk_apps/compile_commands.json", + "${workspaceFolder}/ros1_ws/build/fixposition_driver_lib/compile_commands.json", + "${workspaceFolder}/ros1_ws/build/fpsdk_common/compile_commands.json", + "${workspaceFolder}/ros1_ws/build/rtcm_msgs/compile_commands.json", + "${workspaceFolder}/ros1_ws/build/fixposition_driver_ros1/compile_commands.json", + "${workspaceFolder}/ros1_ws/build/catkin_tools_prebuild/compile_commands.json" + ], //"C_Cpp.default.forcedIncludes": [ ], "C_Cpp.default.intelliSenseMode": "gcc-x64", "C_Cpp.default.compilerPath": "/usr/bin/gcc", diff --git a/fixposition_driver_lib/CMakeLists.txt b/fixposition_driver_lib/CMakeLists.txt index ea98f90..8f4c7a9 100644 --- a/fixposition_driver_lib/CMakeLists.txt +++ b/fixposition_driver_lib/CMakeLists.txt @@ -24,6 +24,7 @@ set(_unused "${CMAKE_C_COMPILER}") # suppress warning find_package(Boost 1.65.0 REQUIRED) find_package(Eigen3 REQUIRED) find_package(fpsdk_common REQUIRED) +find_package(Threads REQUIRED) include_directories(include ${EIGEN3_INCLUDE_DIR} diff --git a/fixposition_driver_lib/package.xml b/fixposition_driver_lib/package.xml index 6a1308f..4483477 100644 --- a/fixposition_driver_lib/package.xml +++ b/fixposition_driver_lib/package.xml @@ -18,9 +18,6 @@ message_generation message_runtime fpsdk_common - fpsdk_ros1 - fpsdk_ros2 - cmake diff --git a/fixposition_driver_msgs/CMakeLists.txt b/fixposition_driver_msgs/CMakeLists.txt index 24537c6..1d4cfa7 100644 --- a/fixposition_driver_msgs/CMakeLists.txt +++ b/fixposition_driver_msgs/CMakeLists.txt @@ -32,6 +32,7 @@ find_package(std_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(Threads REQUIRED) # BUILD, INSTALL ======================================================================================================= diff --git a/fixposition_driver_msgs/package.xml b/fixposition_driver_msgs/package.xml index cac8723..ceef92c 100644 --- a/fixposition_driver_msgs/package.xml +++ b/fixposition_driver_msgs/package.xml @@ -20,8 +20,6 @@ sensor_msgs fixposition_driver_lib fpsdk_common - fpsdk_ros1 - fpsdk_ros2 message_runtime rosidl_default_runtime diff --git a/fixposition_driver_ros1/CMakeLists.txt b/fixposition_driver_ros1/CMakeLists.txt index 53c9935..977a133 100644 --- a/fixposition_driver_ros1/CMakeLists.txt +++ b/fixposition_driver_ros1/CMakeLists.txt @@ -25,7 +25,7 @@ find_package(Eigen3 REQUIRED) find_package(fixposition_driver_lib REQUIRED) find_package(fixposition_driver_msgs REQUIRED) find_package(fpsdk_common REQUIRED) -find_package(fpsdk_ros1 REQUIRED) +find_package(Threads REQUIRED) find_package(catkin REQUIRED COMPONENTS roscpp tf @@ -54,7 +54,7 @@ include_directories( ${fixposition_driver_msgs_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIR} - ${fpsdk_common_INLCUDE_DIRS} ${fpsdk_ros1_INLCUDE_DIRS} + ${fpsdk_common_INLCUDE_DIRS} ) @@ -72,7 +72,7 @@ target_link_libraries( ${catkin_LIBRARIES} ${fixposition_driver_lib_LIBRARIES} ${Boost_LIBRARIES} - ${fpsdk_common_LIBRARIES} ${fpsdk_ros1_LIBRARIES} + ${fpsdk_common_LIBRARIES} pthread ) diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp index 4b8ae8b..6ef1e55 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp @@ -23,7 +23,8 @@ #include #include #include -#include + +#include "fixposition_driver_ros1/ext/ros.hpp" /* PACKAGE */ #include "ros1_msgs.hpp" diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/ext/eigen_conversions.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/ext/eigen_conversions.hpp new file mode 100644 index 0000000..248b1b4 --- /dev/null +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/ext/eigen_conversions.hpp @@ -0,0 +1,20 @@ +// Wrapper to suppress warnings from ROS headers +#ifndef __FIXPOSITION_DRIVER_ROS1_EXT_EIGEN_CONVERSIONS_HPP__ +#define __FIXPOSITION_DRIVER_ROS1_EXT_EIGEN_CONVERSIONS_HPP__ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wall" +#pragma GCC diagnostic ignored "-Wextra" +#pragma GCC diagnostic ignored "-Wpedantic" +#pragma GCC diagnostic ignored "-Wunused-parameter" +#pragma GCC diagnostic ignored "-Wshadow" +#pragma GCC diagnostic ignored "-Wmaybe-uninitialized" +#if defined(__GNUC__) && (__GNUC__ >= 9) +#pragma GCC diagnostic ignored "-Wdeprecated-copy" +#endif +#include +#pragma GCC diagnostic pop +// See commentes in fp_common/ext/eigen_core.hpp +#if !EIGEN_VERSION_AT_LEAST(3, 4, 0) && defined(__GNUC__) && (__GNUC__ >= 9) +#pragma GCC diagnostic ignored "-Wdeprecated-copy" +#endif +#endif // __FIXPOSITION_DRIVER_ROS1_EXT_EIGEN_CONVERSIONS_HPP__ diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/ext/ros.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/ext/ros.hpp new file mode 100644 index 0000000..dd1d3df --- /dev/null +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/ext/ros.hpp @@ -0,0 +1,10 @@ +// Wrapper to suppress warnings from ROS headers +#ifndef __FIXPOSITION_DRIVER_ROS1_EXT_ROS_HPP__ +#define __FIXPOSITION_DRIVER_ROS1_EXT_ROS_HPP__ +#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 +#endif // __FIXPOSITION_DRIVER_ROS1_EXT_ROS_HPP__ diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp index d9ec591..3581ade 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp @@ -20,7 +20,8 @@ /* EXTERNAL */ #include -#include + +#include "fixposition_driver_ros1/ext/ros.hpp" /* PACKAGE */ #include "data_to_ros1.hpp" diff --git a/fixposition_driver_ros1/package.xml b/fixposition_driver_ros1/package.xml index 929d32f..5939e35 100644 --- a/fixposition_driver_ros1/package.xml +++ b/fixposition_driver_ros1/package.xml @@ -20,5 +20,4 @@ tf2_ros tf2_geometry_msgs fpsdk_common - fpsdk_ros1 diff --git a/fixposition_driver_ros1/src/data_to_ros1.cpp b/fixposition_driver_ros1/src/data_to_ros1.cpp index 0731d3a..a82cbdc 100644 --- a/fixposition_driver_ros1/src/data_to_ros1.cpp +++ b/fixposition_driver_ros1/src/data_to_ros1.cpp @@ -17,10 +17,11 @@ /* EXTERNAL */ #include #include +#include #include #include -#include -#include + +#include "fixposition_driver_ros1/ext/eigen_conversions.hpp" /* PACKAGE */ #include "fixposition_driver_ros1/data_to_ros1.hpp" @@ -49,7 +50,7 @@ static void TwistWithCovDataToMsg(const TwistWithCovData& data, geometry_msgs::T } void TfDataToTransformStamped(const TfData& data, geometry_msgs::TransformStamped& msg) { - msg.header.stamp = ros1::utils::ConvTime(data.stamp); + msg.header.stamp = ros1::ConvTime(data.stamp); msg.header.frame_id = data.frame_id; msg.child_frame_id = data.child_frame_id; tf::quaternionEigenToMsg(data.rotation, msg.transform.rotation); @@ -57,7 +58,7 @@ void TfDataToTransformStamped(const TfData& data, geometry_msgs::TransformStampe } void OdometryDataToTransformStamped(const OdometryData& data, geometry_msgs::TransformStamped& msg) { - msg.header.stamp = ros1::utils::ConvTime(data.stamp); + msg.header.stamp = ros1::ConvTime(data.stamp); msg.header.frame_id = data.frame_id; msg.child_frame_id = data.child_frame_id; tf::quaternionEigenToMsg(data.pose.orientation, msg.transform.rotation); @@ -68,7 +69,7 @@ void OdometryDataToTransformStamped(const OdometryData& data, geometry_msgs::Tra template static void FpaOdomToRos(const SomeFpaOdoPayload& payload, SomeOdoMsg& msg) { - msg.header.stamp = ros1::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time)); + msg.header.stamp = ros1::ConvTime(FpaGpsTimeToTime(payload.gps_time)); msg.fusion_status = FpaFusionStatusLegacyToMsg(msg, payload.fusion_status); msg.imu_bias_status = FpaImuStatusLegacyToMsg(msg, payload.imu_bias_status); @@ -138,7 +139,7 @@ void PublishFpaOdomsh(const fpa::FpaOdomshPayload& payload, ros::Publisher& pub) void PublishFpaOdometryDataImu(const fpa::FpaOdometryPayload& payload, bool nav2_mode_, ros::Publisher& pub) { if (pub.getNumSubscribers() > 0) { sensor_msgs::Imu msg; - msg.header.stamp = ros1::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time)); + msg.header.stamp = ros1::ConvTime(FpaGpsTimeToTime(payload.gps_time)); if (nav2_mode_) { msg.header.frame_id = "vrtk_link"; } else { @@ -155,7 +156,7 @@ void PublishFpaOdometryDataImu(const fpa::FpaOdometryPayload& payload, bool nav2 void PublishFpaOdometryDataNavSatFix(const fpa::FpaOdometryPayload& payload, bool nav2_mode_, ros::Publisher& pub) { if (pub.getNumSubscribers() > 0) { sensor_msgs::NavSatFix msg; - msg.header.stamp = ros1::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time)); + msg.header.stamp = ros1::ConvTime(FpaGpsTimeToTime(payload.gps_time)); if (nav2_mode_) { msg.header.frame_id = "vrtk_link"; } else { @@ -209,7 +210,7 @@ void PublishFpaOdomenuVector3Stamped(const fpa::FpaOdomenuPayload& payload, ros: if (pub.getNumSubscribers() > 0) { geometry_msgs::Vector3Stamped msg; - msg.header.stamp = ros1::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time)); + msg.header.stamp = ros1::ConvTime(FpaGpsTimeToTime(payload.gps_time)); msg.header.frame_id = ODOMENU_FRAME_ID; const Eigen::Quaterniond quat = {payload.orientation.values[0], payload.orientation.values[1], @@ -229,7 +230,7 @@ void PublishFpaOdomenuVector3Stamped(const fpa::FpaOdomenuPayload& payload, ros: static void FpaOdomstatusToMsg(const fpa::FpaOdomstatusPayload& payload, fixposition_driver_msgs::FpaOdomstatus& msg) { // clang-format off - msg.header.stamp = ros1::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time)); + msg.header.stamp = ros1::ConvTime(FpaGpsTimeToTime(payload.gps_time)); msg.init_status = FpaInitStatusToMsg(msg, payload.init_status); msg.fusion_imu = FpaMeasStatusToMsg(msg, payload.fusion_imu); msg.fusion_gnss1 = FpaMeasStatusToMsg(msg, payload.fusion_gnss1); @@ -268,7 +269,7 @@ void PublishFpaOdomstatus(const fpa::FpaOdomstatusPayload& payload, ros::Publish void PublishFpaLlh(const fpa::FpaLlhPayload& payload, ros::Publisher& pub) { if (pub.getNumSubscribers() > 0) { fixposition_driver_msgs::FpaLlh msg; - msg.header.stamp = ros1::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time)); + msg.header.stamp = ros1::ConvTime(FpaGpsTimeToTime(payload.gps_time)); msg.header.frame_id = ODOMETRY_CHILD_FRAME_ID; FpaFloat3ToVector3(payload.llh, msg.position); if (payload.cov_enu.valid) { @@ -285,7 +286,7 @@ void PublishFpaLlh(const fpa::FpaLlhPayload& payload, ros::Publisher& pub) { void PublishFpaEoe(const fpa::FpaEoePayload& payload, ros::Publisher& pub) { if (pub.getNumSubscribers() > 0) { fixposition_driver_msgs::FpaEoe msg; - msg.header.stamp = ros1::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time)); + msg.header.stamp = ros1::ConvTime(FpaGpsTimeToTime(payload.gps_time)); msg.epoch = FpaEpochToMsg(msg, payload.epoch); pub.publish(msg); } @@ -294,7 +295,7 @@ void PublishFpaEoe(const fpa::FpaEoePayload& payload, ros::Publisher& pub) { // --------------------------------------------------------------------------------------------------------------------- static void FpaImubiasToMsg(const fpa::FpaImubiasPayload& payload, fixposition_driver_msgs::FpaImubias& msg) { - msg.header.stamp = ros1::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time)); + msg.header.stamp = ros1::ConvTime(FpaGpsTimeToTime(payload.gps_time)); msg.header.frame_id = IMU_FRAME_ID; msg.fusion_imu = FpaMeasStatusToMsg(msg, payload.fusion_imu); msg.imu_status = FpaImuStatusToMsg(msg, payload.imu_status); @@ -319,7 +320,7 @@ void PublishFpaImubias(const fpa::FpaImubiasPayload& payload, ros::Publisher& pu void PublishFpaGnssant(const fpa::FpaGnssantPayload& payload, ros::Publisher& pub) { if (pub.getNumSubscribers() > 0) { fixposition_driver_msgs::FpaGnssant msg; - msg.header.stamp = ros1::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time)); + msg.header.stamp = ros1::ConvTime(FpaGpsTimeToTime(payload.gps_time)); msg.gnss1_state = FpaAntStateToMsg(msg, payload.gnss1_state); msg.gnss1_power = FpaAntPowerToMsg(msg, payload.gnss1_power); msg.gnss1_age = (payload.gnss1_age.valid ? payload.gnss1_age.value : -1); @@ -335,7 +336,7 @@ void PublishFpaGnssant(const fpa::FpaGnssantPayload& payload, ros::Publisher& pu void PublishFpaGnsscorr(const fpa::FpaGnsscorrPayload& payload, ros::Publisher& pub) { if (pub.getNumSubscribers() > 0) { fixposition_driver_msgs::FpaGnsscorr msg; - msg.header.stamp = ros1::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time)); + msg.header.stamp = ros1::ConvTime(FpaGpsTimeToTime(payload.gps_time)); msg.gnss1_fix = FpaGnssFixToMsg(msg, payload.gnss1_fix); msg.gnss1_nsig_l1 = (payload.gnss1_nsig_l1.valid ? payload.gnss1_nsig_l1.value : -1); msg.gnss1_nsig_l2 = (payload.gnss1_nsig_l2.valid ? payload.gnss1_nsig_l2.value : -1); @@ -383,7 +384,7 @@ void PublishFpaText(const fpa::FpaTextPayload& payload, ros::Publisher& pub) { template static void FpaImuPayloadToRos(const SomeFpaImuPayload& payload, sensor_msgs::Imu& msg) { - msg.header.stamp = ros1::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time)); + msg.header.stamp = ros1::ConvTime(FpaGpsTimeToTime(payload.gps_time)); msg.header.frame_id = IMU_FRAME_ID; if (payload.acc.valid) { msg.linear_acceleration.x = payload.acc.values[0]; @@ -433,7 +434,7 @@ bool PublishNovbBestgnsspos(const novb::NovbHeader* header, const novb::NovbBest time::Time stamp; if (stamp.SetWnoTow({header->long_header.gps_week, (double)header->long_header.gps_milliseconds * 1e-3, time::WnoTow::Sys::GPS})) { - msg.header.stamp = ros1::utils::ConvTime(stamp); + msg.header.stamp = ros1::ConvTime(stamp); } msg.header.frame_id = (header->Source() == novb::NovbMsgTypeSource::PRIMARY ? GNSS1_FRAME_ID : GNSS2_FRAME_ID); @@ -468,7 +469,7 @@ static void NovbInspvaxToMsg(const novb::NovbHeader* header, const novb::NovbIns time::Time stamp; if (stamp.SetWnoTow({header->long_header.gps_week, (double)header->long_header.gps_milliseconds * 1e-3, time::WnoTow::Sys::GPS})) { - msg.header.stamp = ros1::utils::ConvTime(stamp); + msg.header.stamp = ros1::ConvTime(stamp); } msg.ins_status = payload->ins_status; @@ -514,7 +515,7 @@ static void NovbHeading2ToMsg(const novb::NovbHeader* header, const novb::NovbHe time::Time stamp; if (stamp.SetWnoTow({header->long_header.gps_week, (double)header->long_header.gps_milliseconds * 1e-3, time::WnoTow::Sys::GPS})) { - msg.header.stamp = ros1::utils::ConvTime(stamp); + msg.header.stamp = ros1::ConvTime(stamp); } msg.sol_status = payload->sol_status; @@ -763,7 +764,7 @@ void PublishParserMsg(const fpsdk::common::parser::ParserMsg& msg, ros::Publishe void PublishNmeaEpochData(const NmeaEpochData& data, ros::Publisher& pub) { if (pub.getNumSubscribers() > 0) { fixposition_driver_msgs::NmeaEpoch msg; - msg.header.stamp = ros1::utils::ConvTime(data.stamp_); + msg.header.stamp = ros1::ConvTime(data.stamp_); msg.header.frame_id = data.frame_id_; if (data.date_.valid) { msg.date_valid = true; @@ -837,7 +838,7 @@ void PublishNmeaEpochData(const NmeaEpochData& data, ros::Publisher& pub) { void PublishOdometryData(const OdometryData& data, ros::Publisher& pub) { if (pub.getNumSubscribers() > 0) { nav_msgs::Odometry msg; - msg.header.stamp = ros1::utils::ConvTime(data.stamp); + msg.header.stamp = ros1::ConvTime(data.stamp); msg.header.frame_id = data.frame_id; msg.child_frame_id = data.child_frame_id; PoseWithCovDataToMsg(data.pose, msg.pose); @@ -851,7 +852,7 @@ void PublishOdometryData(const OdometryData& data, ros::Publisher& pub) { void PublishJumpWarning(const JumpDetector& jump_detector, ros::Publisher& pub) { if (pub.getNumSubscribers() > 0) { fixposition_driver_msgs::CovWarn msg; - msg.header.stamp = ros1::utils::ConvTime(jump_detector.curr_stamp_); + msg.header.stamp = ros1::ConvTime(jump_detector.curr_stamp_); tf::vectorEigenToMsg(jump_detector.pos_diff_, msg.jump); msg.covariance.x = jump_detector.prev_cov_(0, 0); msg.covariance.y = jump_detector.prev_cov_(1, 1); @@ -890,7 +891,7 @@ void PublishDatum(const geometry_msgs::Vector3& payload, const ros::Time& stamp, void PublishFusionEpochData(const FusionEpochData& data, ros::Publisher& pub) { if (pub.getNumSubscribers() > 0) { fixposition_driver_msgs::FusionEpoch msg; - msg.header.stamp = ros1::utils::ConvTime(FpaGpsTimeToTime(data.fpa_eoe_.gps_time)); + msg.header.stamp = ros1::ConvTime(FpaGpsTimeToTime(data.fpa_eoe_.gps_time)); if (data.fpa_odometry_avail_) { msg.fpa_odometry_avail = true; FpaOdometryToMsg(data.fpa_odometry_, msg.fpa_odometry); diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp index 8233852..d5e8de6 100644 --- a/fixposition_driver_ros1/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros1/src/fixposition_driver_node.cpp @@ -25,10 +25,11 @@ #include #include #include +#include #include #include -#include -#include + +#include "fixposition_driver_ros1/ext/eigen_conversions.hpp" /* PACKAGE */ #include "fixposition_driver_ros1/fixposition_driver_node.hpp" @@ -581,7 +582,7 @@ void FixpositionDriverNode::ProcessOdometryData(const OdometryData& odometry_dat // This message computes the difference between the message time and the local system time. // Thus, if the local time is off, the message might be triggered or not triggered when it should. if (params_.delay_warning_ > 0.0) { - const double delay = (ros::Time::now() - fpsdk::ros1::utils::ConvTime(odometry_data.stamp)).toSec(); + const double delay = (ros::Time::now() - ros1::ConvTime(odometry_data.stamp)).toSec(); if (delay > params_.delay_warning_) { ROS_WARN_THROTTLE(1.0, "The system is experiencing significant delays! (estimated delay: %.3f seconds)", delay); @@ -599,7 +600,7 @@ void FixpositionDriverNode::ProcessOdometryData(const OdometryData& odometry_dat // Output jump warning if (params_.cov_warning_ && odometry_data.valid && jump_detector_.Check(odometry_data)) { - ROS_WARN(jump_detector_.warning_.c_str()); + ROS_WARN("%s", jump_detector_.warning_.c_str()); PublishJumpWarning(jump_detector_, jump_pub_); } @@ -722,7 +723,7 @@ int main(int argc, char** argv) { ros::NodeHandle node_handle("~"); // Redirect Fixposition SDK logging to ROS console - fpsdk::ros1::utils::RedirectLoggingToRosConsole(); + fpsdk::common::ros1::RedirectLoggingToRosConsole(); // Say hello HelloWorld(); diff --git a/fixposition_driver_ros1/src/params.cpp b/fixposition_driver_ros1/src/params.cpp index 25bcd35..14a90d7 100644 --- a/fixposition_driver_ros1/src/params.cpp +++ b/fixposition_driver_ros1/src/params.cpp @@ -15,9 +15,7 @@ #include /* EXTERNAL */ -#include -#include -#include +#include "fixposition_driver_ros1/ext/ros.hpp" /* PACKAGE */ #include "fixposition_driver_ros1/params.hpp" @@ -25,34 +23,46 @@ namespace fixposition { /* ****************************************************************************************************************** */ -using namespace fpsdk::ros1; +template +static bool LoadRosParam(const std::string& name, T& value) { + try { + if (ros::param::has(name)) { + ros::param::get(name, value); + } else { + return false; + } + } catch (ros::InvalidNameException& e) { + return false; + } + return true; +} bool LoadParamsFromRos1(const std::string& ns, DriverParams& params) { bool ok = true; ROS_INFO("DriverParams: loading from %s", ns.c_str()); - if (!utils::LoadRosParam(ns + "/stream", params.stream_)) { + if (!LoadRosParam(ns + "/stream", params.stream_)) { ROS_WARN("Failed loading %s/stream param", ns.c_str()); ok = false; } - if (!utils::LoadRosParam(ns + "/reconnect_delay", params.reconnect_delay_)) { + if (!LoadRosParam(ns + "/reconnect_delay", params.reconnect_delay_)) { ROS_WARN("Failed loading %s/reconnect_delay param", ns.c_str()); ok = false; } - if (!utils::LoadRosParam(ns + "/delay_warning", params.delay_warning_)) { + if (!LoadRosParam(ns + "/delay_warning", params.delay_warning_)) { ROS_WARN("Failed loading %s/delay_warning param", ns.c_str()); ok = false; } - if (!utils::LoadRosParam(ns + "/messages", params.messages_) || params.messages_.empty()) { + if (!LoadRosParam(ns + "/messages", params.messages_) || params.messages_.empty()) { ROS_WARN("Failed loading %s/messages param", ns.c_str()); ok = false; } - if (!utils::LoadRosParam(ns + "/fusion_epoch", params.fusion_epoch_)) { + if (!LoadRosParam(ns + "/fusion_epoch", params.fusion_epoch_)) { ROS_WARN("Failed loading %s/fusion_epoch param", ns.c_str()); ok = false; } std::string epoch_str; - if (!utils::LoadRosParam(ns + "/nmea_epoch", epoch_str)) { + if (!LoadRosParam(ns + "/nmea_epoch", epoch_str)) { ROS_WARN("Failed loading %s/nmea_epoch param", ns.c_str()); ok = false; } @@ -60,44 +70,44 @@ bool LoadParamsFromRos1(const std::string& ns, DriverParams& params) { ROS_WARN("Bad value for %s/nmea_epoch param", ns.c_str()); ok = false; } - if (!utils::LoadRosParam(ns + "/raw_output", params.raw_output_)) { + if (!LoadRosParam(ns + "/raw_output", params.raw_output_)) { ROS_WARN("Failed loading %s/raw_output param", ns.c_str()); ok = false; } - if (!utils::LoadRosParam(ns + "/cov_warning", params.cov_warning_)) { + if (!LoadRosParam(ns + "/cov_warning", params.cov_warning_)) { ROS_WARN("Failed loading %s/cov_warning param", ns.c_str()); ok = false; } - if (!utils::LoadRosParam(ns + "/nav2_mode", params.nav2_mode_)) { + if (!LoadRosParam(ns + "/nav2_mode", params.nav2_mode_)) { ROS_WARN("Failed loading %s/nav2_mode param", ns.c_str()); ok = false; } - if (!utils::LoadRosParam(ns + "/converter/enabled", params.converter_enabled_)) { + if (!LoadRosParam(ns + "/converter/enabled", params.converter_enabled_)) { ROS_WARN("Failed loading %s/converter/enabled param", ns.c_str()); ok = false; } - if (!utils::LoadRosParam(ns + "/converter/input_topic", params.converter_input_topic_)) { + if (!LoadRosParam(ns + "/converter/input_topic", params.converter_input_topic_)) { ROS_WARN("Failed loading %s/converter/input_topic param", ns.c_str()); ok = false; } - if (!utils::LoadRosParam(ns + "/converter/scale_factor", params.converter_scale_factor_)) { + if (!LoadRosParam(ns + "/converter/scale_factor", params.converter_scale_factor_)) { ROS_WARN("Failed loading %s/converter/scale_factor param", ns.c_str()); ok = false; } - if (!utils::LoadRosParam(ns + "/converter/use_x", params.converter_use_x_)) { + if (!LoadRosParam(ns + "/converter/use_x", params.converter_use_x_)) { ROS_WARN("Failed loading %s/converter/use_x param", ns.c_str()); ok = false; } - if (!utils::LoadRosParam(ns + "/converter/use_y", params.converter_use_y_)) { + if (!LoadRosParam(ns + "/converter/use_y", params.converter_use_y_)) { ROS_WARN("Failed loading %s/converter/use_y param", ns.c_str()); ok = false; } - if (!utils::LoadRosParam(ns + "/converter/use_z", params.converter_use_z_)) { + if (!LoadRosParam(ns + "/converter/use_z", params.converter_use_z_)) { ROS_WARN("Failed loading %s/converter/use_z param", ns.c_str()); ok = false; } std::string topic_type_string_; - if (!utils::LoadRosParam(ns + "/converter/topic_type", topic_type_string_)) { + if (!LoadRosParam(ns + "/converter/topic_type", topic_type_string_)) { ROS_WARN("Failed loading %s/converter/topic_type param", ns.c_str()); ok = false; } else { @@ -112,15 +122,15 @@ bool LoadParamsFromRos1(const std::string& ns, DriverParams& params) { } } - if (!utils::LoadRosParam(ns + "/output_ns", params.output_ns_)) { + if (!LoadRosParam(ns + "/output_ns", params.output_ns_)) { ROS_WARN("Failed loading %s/output_ns param", ns.c_str()); ok = false; } - if (!utils::LoadRosParam(ns + "/speed_topic", params.speed_topic_)) { + if (!LoadRosParam(ns + "/speed_topic", params.speed_topic_)) { ROS_WARN("Failed loading %s/speed_topic param", ns.c_str()); ok = false; } - if (!utils::LoadRosParam(ns + "/corr_topic", params.corr_topic_)) { + if (!LoadRosParam(ns + "/corr_topic", params.corr_topic_)) { ROS_WARN("Failed loading %s/corr_topic param", ns.c_str()); ok = false; } diff --git a/fixposition_driver_ros2/CMakeLists.txt b/fixposition_driver_ros2/CMakeLists.txt index f6a297f..b16d81f 100644 --- a/fixposition_driver_ros2/CMakeLists.txt +++ b/fixposition_driver_ros2/CMakeLists.txt @@ -28,8 +28,8 @@ find_package(fixposition_driver_lib REQUIRED) find_package(fixposition_driver_msgs REQUIRED) find_package(rtcm_msgs REQUIRED) find_package(fpsdk_common REQUIRED) -find_package(fpsdk_ros2 REQUIRED) find_package(rosbag2_cpp REQUIRED) +find_package(Threads REQUIRED) include_directories( include @@ -39,7 +39,7 @@ include_directories( ${rtcm_msgs_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIR} - ${fpsdk_common_INLCUDE_DIRS} ${fpsdk_ros2_INLCUDE_DIRS} + ${fpsdk_common_INLCUDE_DIRS} ) add_executable( @@ -54,7 +54,7 @@ target_link_libraries(${PROJECT_NAME}_exec ${rtcm_msgs_LIBRARIES} ${Boost_LIBRARIES} ${EIGEN3_LIBRARIES} - ${fpsdk_common_LIBRARIES} ${fpsdk_ros2_LIBRARIES} + ${fpsdk_common_LIBRARIES} ${rosbag2_cpp_TARGETS} ${rclcpp_TARGETS} ${std_msgs_TARGETS} 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 7730559..b3f9ab3 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 @@ -23,7 +23,8 @@ #include #include #include -#include + +#include "fixposition_driver_ros2/ext/rclcpp.hpp" /* PACKAGE */ #include "ros2_msgs.hpp" diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/ext/rclcpp.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/ext/rclcpp.hpp new file mode 100644 index 0000000..84815ec --- /dev/null +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/ext/rclcpp.hpp @@ -0,0 +1,10 @@ +// Wrapper to suppress warnings from ROS headers +#ifndef __FIXPOSITION_DRIVER_ROS2_EXT_RCLCPP_HPP__ +#define __FIXPOSITION_DRIVER_ROS2_EXT_RCLCPP_HPP__ +#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 +#endif // __FIXPOSITION_DRIVER_ROS2_EXT_RCLCPP_HPP__ 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 be87495..44451d6 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 @@ -20,7 +20,8 @@ /* EXTERNAL */ #include -#include + +#include "fixposition_driver_ros2/ext/rclcpp.hpp" /* PACKAGE */ #include "data_to_ros2.hpp" diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/params.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/params.hpp index 070e260..a4562f0 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/params.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/params.hpp @@ -18,7 +18,8 @@ /* EXTERNAL */ #include -#include + +#include "fixposition_driver_ros2/ext/rclcpp.hpp" /* PACKAGE */ diff --git a/fixposition_driver_ros2/package.xml b/fixposition_driver_ros2/package.xml index a3388d6..bcb49b1 100644 --- a/fixposition_driver_ros2/package.xml +++ b/fixposition_driver_ros2/package.xml @@ -25,7 +25,6 @@ tf2_geometry_msgs rtcm_msgs fpsdk_common - fpsdk_ros2 rclcpp rosidl_default_runtime diff --git a/fixposition_driver_ros2/src/data_to_ros2.cpp b/fixposition_driver_ros2/src/data_to_ros2.cpp index 5af16d2..4ecaa87 100644 --- a/fixposition_driver_ros2/src/data_to_ros2.cpp +++ b/fixposition_driver_ros2/src/data_to_ros2.cpp @@ -16,9 +16,9 @@ /* EXTERNAL */ #include #include +#include #include #include -#include /* PACKAGE */ #include "fixposition_driver_ros2/data_to_ros2.hpp" @@ -47,7 +47,7 @@ static void TwistWithCovDataToMsg(const TwistWithCovData& data, geometry_msgs::m } void TfDataToTransformStamped(const TfData& data, geometry_msgs::msg::TransformStamped& msg) { - msg.header.stamp = ros2::utils::ConvTime(data.stamp); + msg.header.stamp = ros2::ConvTime(data.stamp); msg.header.frame_id = data.frame_id; msg.child_frame_id = data.child_frame_id; msg.transform.rotation = tf2::toMsg(data.rotation); @@ -55,7 +55,7 @@ void TfDataToTransformStamped(const TfData& data, geometry_msgs::msg::TransformS } void OdometryDataToTransformStamped(const OdometryData& data, geometry_msgs::msg::TransformStamped& msg) { - msg.header.stamp = ros2::utils::ConvTime(data.stamp); + msg.header.stamp = ros2::ConvTime(data.stamp); msg.header.frame_id = data.frame_id; msg.child_frame_id = data.child_frame_id; msg.transform.rotation = tf2::toMsg(data.pose.orientation); @@ -66,7 +66,7 @@ void OdometryDataToTransformStamped(const OdometryData& data, geometry_msgs::msg template static void FpaOdomToRos(const SomeFpaOdoPayload& payload, SomeOdoMsg& msg) { - msg.header.stamp = ros2::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time)); + msg.header.stamp = ros2::ConvTime(FpaGpsTimeToTime(payload.gps_time)); msg.fusion_status = FpaFusionStatusLegacyToMsg(msg, payload.fusion_status); msg.imu_bias_status = FpaImuStatusLegacyToMsg(msg, payload.imu_bias_status); @@ -139,7 +139,7 @@ void PublishFpaOdometryDataImu(const fpa::FpaOdometryPayload& payload, bool nav2 rclcpp::Publisher::SharedPtr& pub) { if (pub->get_subscription_count() > 0) { sensor_msgs::msg::Imu msg; - msg.header.stamp = ros2::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time)); + msg.header.stamp = ros2::ConvTime(FpaGpsTimeToTime(payload.gps_time)); if (nav2_mode_) { msg.header.frame_id = "vrtk_link"; } else { @@ -157,7 +157,7 @@ void PublishFpaOdometryDataNavSatFix(const fpa::FpaOdometryPayload& payload, boo rclcpp::Publisher::SharedPtr& pub) { if (pub->get_subscription_count() > 0) { sensor_msgs::msg::NavSatFix msg; - msg.header.stamp = ros2::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time)); + msg.header.stamp = ros2::ConvTime(FpaGpsTimeToTime(payload.gps_time)); if (nav2_mode_) { msg.header.frame_id = "vrtk_link"; } else { @@ -212,7 +212,7 @@ void PublishFpaOdomenuVector3Stamped(const fpa::FpaOdomenuPayload& payload, if (pub->get_subscription_count() > 0) { geometry_msgs::msg::Vector3Stamped msg; - msg.header.stamp = ros2::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time)); + msg.header.stamp = ros2::ConvTime(FpaGpsTimeToTime(payload.gps_time)); msg.header.frame_id = ODOMENU_FRAME_ID; const Eigen::Quaterniond quat = {payload.orientation.values[0], payload.orientation.values[1], @@ -234,7 +234,7 @@ void PublishFpaOdomenuVector3Stamped(const fpa::FpaOdomenuPayload& payload, static void FpaOdomstatusToMsg(const fpa::FpaOdomstatusPayload& payload, fpmsgs::FpaOdomstatus& msg) { // clang-format off - msg.header.stamp = ros2::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time)); + msg.header.stamp = ros2::ConvTime(FpaGpsTimeToTime(payload.gps_time)); msg.init_status = FpaInitStatusToMsg(msg, payload.init_status); msg.fusion_imu = FpaMeasStatusToMsg(msg, payload.fusion_imu); msg.fusion_gnss1 = FpaMeasStatusToMsg(msg, payload.fusion_gnss1); @@ -274,7 +274,7 @@ void PublishFpaOdomstatus(const fpa::FpaOdomstatusPayload& payload, void PublishFpaLlh(const fpa::FpaLlhPayload& payload, rclcpp::Publisher::SharedPtr& pub) { if (pub->get_subscription_count() > 0) { fpmsgs::FpaLlh msg; - msg.header.stamp = ros2::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time)); + msg.header.stamp = ros2::ConvTime(FpaGpsTimeToTime(payload.gps_time)); msg.header.frame_id = ODOMETRY_CHILD_FRAME_ID; FpaFloat3ToVector3(payload.llh, msg.position); if (payload.cov_enu.valid) { @@ -291,7 +291,7 @@ void PublishFpaLlh(const fpa::FpaLlhPayload& payload, rclcpp::Publisher::SharedPtr& pub) { if (pub->get_subscription_count() > 0) { fpmsgs::FpaEoe msg; - msg.header.stamp = ros2::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time)); + msg.header.stamp = ros2::ConvTime(FpaGpsTimeToTime(payload.gps_time)); msg.epoch = FpaEpochToMsg(msg, payload.epoch); pub->publish(msg); } @@ -300,7 +300,7 @@ void PublishFpaEoe(const fpa::FpaEoePayload& payload, rclcpp::Publisher::SharedPtr& pub) { if (pub->get_subscription_count() > 0) { fpmsgs::FpaGnssant msg; - msg.header.stamp = ros2::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time)); + msg.header.stamp = ros2::ConvTime(FpaGpsTimeToTime(payload.gps_time)); msg.gnss1_state = FpaAntStateToMsg(msg, payload.gnss1_state); msg.gnss1_power = FpaAntPowerToMsg(msg, payload.gnss1_power); msg.gnss1_age = (payload.gnss1_age.valid ? payload.gnss1_age.value : -1); @@ -342,7 +342,7 @@ void PublishFpaGnsscorr(const fpa::FpaGnsscorrPayload& payload, rclcpp::Publisher::SharedPtr& pub) { if (pub->get_subscription_count() > 0) { fpmsgs::FpaGnsscorr msg; - msg.header.stamp = ros2::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time)); + msg.header.stamp = ros2::ConvTime(FpaGpsTimeToTime(payload.gps_time)); msg.gnss1_fix = FpaGnssFixToMsg(msg, payload.gnss1_fix); msg.gnss1_nsig_l1 = (payload.gnss1_nsig_l1.valid ? payload.gnss1_nsig_l1.value : -1); msg.gnss1_nsig_l2 = (payload.gnss1_nsig_l2.valid ? payload.gnss1_nsig_l2.value : -1); @@ -391,7 +391,7 @@ void PublishFpaText(const fpa::FpaTextPayload& payload, rclcpp::Publisher static void FpaImuPayloadToRos(const SomeFpaImuPayload& payload, sensor_msgs::msg::Imu& msg) { - msg.header.stamp = ros2::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time)); + msg.header.stamp = ros2::ConvTime(FpaGpsTimeToTime(payload.gps_time)); msg.header.frame_id = IMU_FRAME_ID; if (payload.acc.valid) { msg.linear_acceleration.x = payload.acc.values[0]; @@ -442,7 +442,7 @@ bool PublishNovbBestgnsspos(const novb::NovbHeader* header, const novb::NovbBest time::Time stamp; if (stamp.SetWnoTow({header->long_header.gps_week, (double)header->long_header.gps_milliseconds * 1e-3, time::WnoTow::Sys::GPS})) { - msg.header.stamp = ros2::utils::ConvTime(stamp); + msg.header.stamp = ros2::ConvTime(stamp); } msg.header.frame_id = (header->Source() == novb::NovbMsgTypeSource::PRIMARY ? GNSS1_FRAME_ID : GNSS2_FRAME_ID); @@ -477,7 +477,7 @@ static void NovbInspvaxToMsg(const novb::NovbHeader* header, const novb::NovbIns time::Time stamp; if (stamp.SetWnoTow({header->long_header.gps_week, (double)header->long_header.gps_milliseconds * 1e-3, time::WnoTow::Sys::GPS})) { - msg.header.stamp = ros2::utils::ConvTime(stamp); + msg.header.stamp = ros2::ConvTime(stamp); } msg.ins_status = payload->ins_status; @@ -524,7 +524,7 @@ static void NovbHeading2ToMsg(const novb::NovbHeader* header, const novb::NovbHe time::Time stamp; if (stamp.SetWnoTow({header->long_header.gps_week, (double)header->long_header.gps_milliseconds * 1e-3, time::WnoTow::Sys::GPS})) { - msg.header.stamp = ros2::utils::ConvTime(stamp); + msg.header.stamp = ros2::ConvTime(stamp); } msg.sol_status = payload->sol_status; @@ -784,7 +784,7 @@ void PublishParserMsg(const fpsdk::common::parser::ParserMsg& msg, void PublishNmeaEpochData(const NmeaEpochData& data, rclcpp::Publisher::SharedPtr& pub) { if (pub->get_subscription_count() > 0) { fpmsgs::NmeaEpoch msg; - msg.header.stamp = ros2::utils::ConvTime(data.stamp_); + msg.header.stamp = ros2::ConvTime(data.stamp_); msg.header.frame_id = data.frame_id_; if (data.date_.valid) { msg.date_valid = true; @@ -858,7 +858,7 @@ void PublishNmeaEpochData(const NmeaEpochData& data, rclcpp::Publisher::SharedPtr& pub) { if (pub->get_subscription_count() > 0) { nav_msgs::msg::Odometry msg; - msg.header.stamp = ros2::utils::ConvTime(data.stamp); + msg.header.stamp = ros2::ConvTime(data.stamp); msg.header.frame_id = data.frame_id; msg.child_frame_id = data.child_frame_id; PoseWithCovDataToMsg(data.pose, msg.pose); @@ -872,7 +872,7 @@ void PublishOdometryData(const OdometryData& data, rclcpp::Publisher::SharedPtr& pub) { if (pub->get_subscription_count() > 0) { fpmsgs::CovWarn msg; - msg.header.stamp = ros2::utils::ConvTime(jump_detector.curr_stamp_); + msg.header.stamp = ros2::ConvTime(jump_detector.curr_stamp_); tf2::toMsg(jump_detector.pos_diff_, msg.jump); msg.covariance.x = jump_detector.prev_cov_(0, 0); msg.covariance.y = jump_detector.prev_cov_(1, 1); @@ -912,7 +912,7 @@ void PublishDatum(const geometry_msgs::msg::Vector3& payload, const builtin_inte void PublishFusionEpochData(const FusionEpochData& data, rclcpp::Publisher::SharedPtr& pub) { if (pub->get_subscription_count() > 0) { fpmsgs::FusionEpoch msg; - msg.header.stamp = ros2::utils::ConvTime(FpaGpsTimeToTime(data.fpa_eoe_.gps_time)); + msg.header.stamp = ros2::ConvTime(FpaGpsTimeToTime(data.fpa_eoe_.gps_time)); if (data.fpa_odometry_avail_) { msg.fpa_odometry_avail = true; FpaOdometryToMsg(data.fpa_odometry_, msg.fpa_odometry); diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp index 89f15e9..5352e3c 100644 --- a/fixposition_driver_ros2/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros2/src/fixposition_driver_node.cpp @@ -26,9 +26,9 @@ #include #include #include +#include #include #include -#include /* PACKAGE */ #include "fixposition_driver_ros2/fixposition_driver_node.hpp" @@ -618,7 +618,7 @@ void FixpositionDriverNode::ProcessOdometryData(const OdometryData& odometry_dat // This message computes the difference between the message time and the local system time. // Thus, if the local time is off, the message might be triggered or not triggered when it should. if (params_.delay_warning_ > 0.0) { - const double delay = (nh_->now() - fpsdk::ros2::utils::ConvTime(odometry_data.stamp)).seconds(); + const double delay = (nh_->now() - ros2::ConvTime(odometry_data.stamp)).seconds(); if (delay > params_.delay_warning_) { RCLCPP_WARN_THROTTLE(logger_, *nh_->get_clock(), 1e3, "The system is experiencing significant delays! (estimated delay: %.3f seconds)", @@ -759,7 +759,7 @@ int main(int argc, char** argv) { auto logger = nh->get_logger(); // Redirect Fixposition SDK logging to ROS console - fpsdk::ros2::utils::RedirectLoggingToRosConsole(logger.get_name()); + fpsdk::common::ros2::RedirectLoggingToRosConsole(logger.get_name()); // Say hello HelloWorld(); diff --git a/fixposition_driver_ros2/src/params.cpp b/fixposition_driver_ros2/src/params.cpp index 3a8a5f4..1e1298d 100644 --- a/fixposition_driver_ros2/src/params.cpp +++ b/fixposition_driver_ros2/src/params.cpp @@ -15,7 +15,7 @@ #include /* EXTERNAL */ -#include +#include "fixposition_driver_ros2/ext/rclcpp.hpp" /* PACKAGE */ #include "fixposition_driver_ros2/params.hpp" diff --git a/setup_ros_ws.sh b/setup_ros_ws.sh index 8224faf..ae1e322 100755 --- a/setup_ros_ws.sh +++ b/setup_ros_ws.sh @@ -85,14 +85,12 @@ function main notice "Setup fixposition_driver for ROS${rosver}" if [ ${rosver} -eq 1 ]; then - for path in fixposition_driver_ros2 \ - fixposition-sdk/fpsdk_ros2 fixposition-sdk/examples; do + for path in fixposition_driver_ros2 fixposition-sdk/examples; do info "- ${path}/CATKIN_IGNORE"; touch ${SCRIPTDIR}/${path}/CATKIN_IGNORE done else - for path in fixposition_driver_ros1 \ - fixposition-sdk/fpsdk_ros1 fixposition-sdk/examples; do + for path in fixposition_driver_ros1 fixposition-sdk/examples; do info "- ${path}/COLCON_IGNORE"; touch ${SCRIPTDIR}/${path}/COLCON_IGNORE done