Adding Back TCP Handler to ROS2

rosbridge_library/src/rosbridge_library/capabilities/advertise_service.py

@@ -6,7 +6,6 @@
 from rosbridge_library.internal import message_conversion
 from rosbridge_library.internal.ros_loader import get_service_class
 
-
 class AdvertisedServiceHandler:
 
     id_counter = 1

rosbridge_library/src/rosbridge_library/capabilities/unadvertise_service.py

@@ -50,9 +50,9 @@ def unadvertise_service(self, message):
         # unregister service in ROS
         if service_name in self.protocol.external_service_list.keys():
             self.protocol.external_service_list[service_name].graceful_shutdown()
-            self.protocol.external_service_list[service_name].service_handle.shutdown(
-                "Unadvertise request."
-            )
+            # self.protocol.external_service_list[service_name].service_handle.shutdown(
+            #     "Unadvertise request."
+            # )
             del self.protocol.external_service_list[service_name]
             self.protocol.log("info", "Unadvertised service %s." % service_name)
         else:

rosbridge_library/src/rosbridge_library/internal/publishers.py

@@ -31,6 +31,7 @@
 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 # POSSIBILITY OF SUCH DAMAGE.
 
+from multiprocessing.spawn import is_forking
 from threading import Timer
 
 from rclpy.duration import Duration

rosbridge_library/src/rosbridge_library/protocol.py

@@ -31,6 +31,8 @@
 # POSSIBILITY OF SUCH DAMAGE.
 
 import time
+import traceback
+import datetime
 
 from rosbridge_library.capabilities.fragmentation import Fragmentation
 from rosbridge_library.util import bson, json
@@ -134,7 +136,7 @@ def incoming(self, message_string=""):
                 # that receives exactly one full BSON message.
                 # This will then be passed to self.deserialize and shouldn't cause any
                 # exceptions because of fragmented messages (broken or invalid messages might still be sent tough)
-                self.log("error", "Exception in deserialization of BSON")
+                self.log("error", f"Exception in deserialization of BSON: {traceback.format_exc()}")
 
             else:
                 # TODO: handling of partial/multiple/broken json data in incoming buffer
@@ -387,9 +389,9 @@ def log(self, level, message, lid=None):
         """
         stdout_formatted_msg = None
         if lid is not None:
-            stdout_formatted_msg = f"[Client {self.client_id}] [id: {lid}] {message}"
+            stdout_formatted_msg = f"[{datetime.datetime.now()}] [Client {self.client_id}] [id: {lid}] {message}"
         else:
-            stdout_formatted_msg = f"[Client {self.client_id}] {message}"
+            stdout_formatted_msg = f"[{datetime.datetime.now()}] [Client {self.client_id}] {message}"
 
         if level == "error" or level == "err":
             self.node_handle.get_logger().error(stdout_formatted_msg)

rosbridge_server/CMakeLists.txt

@@ -13,12 +13,12 @@ ament_package()
 install(PROGRAMS
   scripts/rosbridge_websocket.py
   scripts/rosbridge_websocket
+  scripts/rosbridge_tcp
   DESTINATION lib/${PROJECT_NAME}
 )
 
-install(FILES
-  launch/rosbridge_websocket_launch.xml
-  DESTINATION share/${PROJECT_NAME}/launch
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
 )
 
 if(BUILD_TESTING)

rosbridge_server/launch/rosbridge_tcp_launch.xml

@@ -0,0 +1,45 @@
+<launch>
+  <arg name="port" default="9090" />
+  <arg name="host" default="" />
+
+  <arg name="incoming_buffer" default="65536" />
+  <arg name="socket_timeout" default="10" />
+  <arg name="retry_startup_delay" default="5.0" />
+
+  <arg name="fragment_timeout" default="600" />
+  <arg name="delay_between_messages" default="0" />
+  <arg name="max_message_size" default="None" />
+  <arg name="unregister_timeout" default="10.0" />
+
+  <arg name="call_services_in_new_thread" default="false" />
+
+  <arg name="topics_glob" default="" />
+  <arg name="services_glob" default="" />
+  <arg name="params_glob" default="" />
+  <arg name="bson_only_mode" default="true" />
+
+  <node name="rosbridge_tcp" pkg="rosbridge_server" exec="rosbridge_tcp" output="screen">
+    <param name="port" value="$(var port)"/>
+    <param name="host" value="$(var host)"/>
+    <param name="incoming_buffer" value="$(var incoming_buffer)"/>
+    <param name="socket_timeout" value="$(var socket_timeout)"/>
+    <param name="retry_startup_delay" value="$(var retry_startup_delay)"/>
+    <param name="fragment_timeout" value="$(var fragment_timeout)"/>
+    <param name="delay_between_messages" value="$(var delay_between_messages)"/>
+    <param name="max_message_size" value="$(var max_message_size)"/>
+    <param name="unregister_timeout" value="$(var unregister_timeout)"/>
+    <param name="call_services_in_new_thread" value="$(var call_services_in_new_thread)"/>
+
+    <param name="topics_glob" value="$(var topics_glob)"/>
+    <param name="services_glob" value="$(var services_glob)"/>
+    <param name="params_glob" value="$(var params_glob)"/>
+
+    <param name="bson_only_mode" value="$(var bson_only_mode)"/>
+  </node>
+
+  <node name="rosapi" pkg="rosapi" exec="rosapi_node">
+    <param name="topics_glob" value="$(var topics_glob)"/>
+    <param name="services_glob" value="$(var services_glob)"/>
+    <param name="params_glob" value="$(var params_glob)"/>
+  </node>
+</launch>