diff --git a/CMakeLists.txt b/CMakeLists.txt index b520657..64ae0f9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,15 +9,36 @@ find_package(PkgConfig REQUIRED) find_package(Boost REQUIRED COMPONENTS system) find_package(Threads REQUIRED) +# Find libbson - prefer static library for deployment if(PkgConfig_FOUND) - pkg_search_module(LIBBSON REQUIRED libbson-1.0) + pkg_search_module(LIBBSON libbson-1.0) +endif() + +# Try to find static libbson library (required for deployment without installing libraries on target) +find_library(LIBBSON_STATIC_LIB + NAMES bson-static-1.0 libbson-static-1.0 + PATHS ${LIBBSON_LIBRARY_DIRS} /usr/lib /usr/lib/x86_64-linux-gnu /usr/local/lib +) + +if(LIBBSON_STATIC_LIB) + message(STATUS "Found static libbson: ${LIBBSON_STATIC_LIB}") + set(LIBBSON_LIBRARIES ${LIBBSON_STATIC_LIB}) + # Ensure we have include dirs + if(NOT LIBBSON_INCLUDE_DIRS AND PkgConfig_FOUND) + pkg_search_module(LIBBSON libbson-1.0) + endif() + if(NOT LIBBSON_INCLUDE_DIRS) + find_path(LIBBSON_INCLUDE_DIRS + NAMES bson.h + PATHS /usr/include /usr/local/include + PATH_SUFFIXES libbson-1.0 + ) + endif() else() - message(STATUS "Didn't found PkgConfig, please specify the path yourself") - set(LIBBSON_DIR "" CACHE PATH "Install directory of libbson. Should contain /include and /lib") - set(LIBBSON_INCLUDE_DIRS "${LIBBSON_DIR}/include") - set(LIBBSON_LIBRARY_DIRS "${LIBBSON_DIR}/lib") - set(LIBBSON_CFLAGS_OTHER "-I${LIBBSON_INCLUDE_DIRS}/libbson-1.0") - set(LIBBSON_LIBRARIES "bson-static-1.0.lib" CACHE STRING "Lib file. Different between platforms") + # Static library not found - fail the build + message(FATAL_ERROR + "Static libbson library (libbson-static-1.0.a) not found!\n" + ) endif() if(NOT WIN32) diff --git a/include/ros_service.h b/include/ros_service.h index cb34806..250c4aa 100644 --- a/include/ros_service.h +++ b/include/ros_service.h @@ -50,6 +50,12 @@ namespace rosbridge2cpp { return service_name_; } + // Check if the service is actually advertised on the ROS side via rosapi + // This queries the ROS system to verify the service is available + // @param timeout_ms Timeout in milliseconds (default: 2000ms) + // @return true if service is found in ROS, false if not found or timeout + bool isAdvertised(int timeout_ms = 2000) const; + private: ROSBridge &ros_; std::string service_name_; diff --git a/include/ros_tf_broadcaster.h b/include/ros_tf_broadcaster.h index b8127ca..726ff6c 100644 --- a/include/ros_tf_broadcaster.h +++ b/include/ros_tf_broadcaster.h @@ -27,6 +27,10 @@ namespace rosbridge2cpp { // Send transforms to /tf_static (for static transforms) void SendStaticTransforms(json &geometry_msgs_transformstamped_array_msg); + // Advertise /tf_static topic (unadvertises first to handle reconnection) + // @return true if topic was successfully advertised, false otherwise + bool AdvertiseStaticTopic(); + ~ROSTFBroadcaster() = default; private: diff --git a/src/ros_service.cpp b/src/ros_service.cpp index 4787419..45c47c6 100644 --- a/src/ros_service.cpp +++ b/src/ros_service.cpp @@ -48,10 +48,10 @@ namespace rosbridge2cpp{ } bool ROSService::Advertise(FunVrROSCallServiceMsgrROSServiceResponseMsgrAllocator callback) { - if (is_advertised_) - return true; - - // Register on ROSBridge + // Always try to advertise - if already advertised and connection is good, + // the send will succeed and we'll update the callback. If connection was lost, + // the send will fail and is_advertised_ will be set to false, allowing retry. + // Register on ROSBridge (this will update the callback even if already registered) ros_.RegisterServiceRequestCallback(service_name_, callback); ROSBridgeAdvertiseServiceMsg cmd(true); @@ -63,10 +63,10 @@ namespace rosbridge2cpp{ } bool ROSService::Advertise(FunVrROSCallServiceMsgrROSServiceResponseMsg callback) { - if (is_advertised_) - return true; - - // Register on ROSBridge + // Always try to advertise - if already advertised and connection is good, + // the send will succeed and we'll update the callback. If connection was lost, + // the send will fail and is_advertised_ will be set to false, allowing retry. + // Register on ROSBridge (this will update the callback even if already registered) ros_.RegisterServiceRequestCallback(service_name_, callback); ROSBridgeAdvertiseServiceMsg cmd(true); diff --git a/src/ros_tf_broadcaster.cpp b/src/ros_tf_broadcaster.cpp index 0f8f45c..401bcb5 100644 --- a/src/ros_tf_broadcaster.cpp +++ b/src/ros_tf_broadcaster.cpp @@ -1,4 +1,6 @@ #include "ros_tf_broadcaster.h" +#include +#include namespace rosbridge2cpp { void ROSTFBroadcaster::SendTransform(json &geometry_msgs_transformstamped_msg) @@ -40,4 +42,17 @@ namespace rosbridge2cpp { tf_static_topic_.Publish(tf_message); } + + bool ROSTFBroadcaster::AdvertiseStaticTopic() + { + // Always ensure /tf_static is advertised before publishing + // Unadvertise first to force re-advertisement (handles stale state after reconnection) + tf_static_topic_.Unadvertise(); + bool success = tf_static_topic_.Advertise(); + // Small delay to allow rosbridge to process the advertise message + if (success) { + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + return success; + } }