Improve advertise services after reconnection.

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)

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_;

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:

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);

src/ros_tf_broadcaster.cpp

@@ -1,4 +1,6 @@
 #include "ros_tf_broadcaster.h"
+#include <thread>
+#include <chrono>
 
 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;
+	}
 }