From 97bb44e9e99596d1af54cce116eb0d91e697ad87 Mon Sep 17 00:00:00 2001
From: LaurieCheers-unity
 <73140792+LaurieCheers-unity@users.noreply.github.com>
Date: Fri, 12 Mar 2021 11:25:24 -0800
Subject: [PATCH] Laurie/topic list (#41)

---
 config/params.yaml             | 1 +
 launch/endpoint.launch         | 4 ++++
 src/ros_tcp_endpoint/client.py | 7 ++++++-
 src/ros_tcp_endpoint/server.py | 4 ++++
 srv/RosUnityTopicList.srv      | 2 ++
 5 files changed, 17 insertions(+), 1 deletion(-)
 create mode 100644 config/params.yaml
 create mode 100644 launch/endpoint.launch
 create mode 100644 srv/RosUnityTopicList.srv

diff --git a/config/params.yaml b/config/params.yaml
new file mode 100644
index 0000000..50aaa22
--- /dev/null
+++ b/config/params.yaml
@@ -0,0 +1 @@
+ROS_IP: 127.0.0.1
\ No newline at end of file
diff --git a/launch/endpoint.launch b/launch/endpoint.launch
new file mode 100644
index 0000000..f5d7788
--- /dev/null
+++ b/launch/endpoint.launch
@@ -0,0 +1,4 @@
+<launch>
+    <rosparam file="$(find ros_tcp_endpoint)/config/params.yaml" command="load"/>
+    <node name="server_endpoint" pkg="ros_tcp_endpoint" type="default_server_endpoint.py" args="--wait" output="screen" respawn="true" />
+</launch>
\ No newline at end of file
diff --git a/src/ros_tcp_endpoint/client.py b/src/ros_tcp_endpoint/client.py
index 7d19048..c54acb0 100644
--- a/src/ros_tcp_endpoint/client.py
+++ b/src/ros_tcp_endpoint/client.py
@@ -102,7 +102,7 @@ def read_message(conn):
 
             data += packet
 
-        if not data:
+        if full_message_size > 0 and not data:
             print("No data for a message size of {}, breaking!".format(full_message_size))
             return
 
@@ -164,6 +164,11 @@ def run(self):
             response_message = self.serialize_message(destination, response)
             self.conn.send(response_message)
             return
+        elif destination == '__topic_list':
+            response = self.tcp_server.topic_list(data)
+            response_message = self.serialize_message(destination, response)
+            self.conn.send(response_message)
+            return
         elif destination not in self.tcp_server.source_destination_dict.keys():
             error_msg = "Topic/service destination '{}' is not defined! Known topics are: {} "\
                 .format(destination, self.tcp_server.source_destination_dict.keys())
diff --git a/src/ros_tcp_endpoint/server.py b/src/ros_tcp_endpoint/server.py
index b1b7d2a..fdb3802 100644
--- a/src/ros_tcp_endpoint/server.py
+++ b/src/ros_tcp_endpoint/server.py
@@ -25,6 +25,7 @@
 from .subscriber import RosSubscriber
 from .publisher import RosPublisher
 from ros_tcp_endpoint.msg import RosUnitySysCommand
+from ros_tcp_endpoint.srv import RosUnityTopicListResponse
 
 class TcpServer:
     """
@@ -96,6 +97,9 @@ def send_unity_message(self, topic, message):
     def send_unity_service(self, topic, service_class, request):
         return self.unity_tcp_sender.send_unity_service(topic, service_class, request)
 
+    def topic_list(self, data):
+        return RosUnityTopicListResponse(self.source_destination_dict.keys())
+
     def handle_syscommand(self, data):
         message = RosUnitySysCommand().deserialize(data)
         function = getattr(self.syscommands, message.command)
diff --git a/srv/RosUnityTopicList.srv b/srv/RosUnityTopicList.srv
new file mode 100644
index 0000000..895f4a6
--- /dev/null
+++ b/srv/RosUnityTopicList.srv
@@ -0,0 +1,2 @@
+---
+string[] topics
\ No newline at end of file