Skip to content

Commit

Permalink
Laurie/topic list (#41)
Browse files Browse the repository at this point in the history
  • Loading branch information
LaurieCheers-unity authored and sdiao committed May 27, 2021
1 parent b7f51cc commit 6db3ee4
Show file tree
Hide file tree
Showing 5 changed files with 53 additions and 27 deletions.
1 change: 1 addition & 0 deletions config/params.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
ROS_IP: 127.0.0.1
4 changes: 4 additions & 0 deletions launch/endpoint.launch
Original file line number Diff line number Diff line change
@@ -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>
69 changes: 42 additions & 27 deletions src/ros_tcp_endpoint/client.py
Original file line number Diff line number Diff line change
Expand Up @@ -40,23 +40,25 @@ def __init__(self, conn, tcp_server, incoming_ip, incoming_port):
self.incoming_ip = incoming_ip
self.incoming_port = incoming_port

def read_int32(self):
@staticmethod
def read_int32(conn):
"""
Reads four bytes from socket connection and unpacks them to an int
Returns: int
"""
try:
raw_bytes = self.conn.recv(4)
raw_bytes = conn.recv(4)
num = struct.unpack('<I', raw_bytes)[0]
return num
except Exception as e:
print("Unable to read integer from connection. {}".format(e))

return None

def read_string(self):
@staticmethod
def read_string(conn):
"""
Reads int32 from socket connection to determine how many bytes to
read to get the string that follows. Read that number of bytes and
Expand All @@ -66,9 +68,9 @@ def read_string(self):
"""
try:
str_len = self.read_int32()
str_len = ClientThread.read_int32(conn)

str_bytes = self.conn.recv(str_len)
str_bytes = conn.recv(str_len)
decoded_str = str_bytes.decode('utf-8')

return decoded_str
Expand All @@ -78,6 +80,34 @@ def read_string(self):

return None

@staticmethod
def read_message(conn):
"""
Decode destination and full message size from socket connection.
Grab bytes in chunks until full message has been read.
"""
data = b''

destination = ClientThread.read_string(conn)
full_message_size = ClientThread.read_int32(conn)

while len(data) < full_message_size:
# Only grabs max of 1024 bytes TODO: change to TCPServer's buffer_size
grab = 1024 if full_message_size - len(data) > 1024 else full_message_size - len(data)
packet = conn.recv(grab)

if not packet:
print("No packets...")
break

data += packet

if full_message_size > 0 and not data:
print("No data for a message size of {}, breaking!".format(full_message_size))
return

return destination, data

@staticmethod
def serialize_message(destination, message):
"""
Expand Down Expand Up @@ -111,9 +141,7 @@ def serialize_message(destination, message):

def run(self):
"""
Decode destination and full message size from socket connection.
Grab bytes in chunks until full message has been read.
Determine where to send the message based on the source_destination_dict
Read a message and determine where to send it based on the source_destination_dict
and destination string. Then send the read message.
If there is a response after sending the serialized data, assume it is a
Expand All @@ -126,25 +154,7 @@ def run(self):
msg: the ROS msg type as bytes
"""
data = b''

destination = self.read_string()
full_message_size = self.read_int32()

while len(data) < full_message_size:
# Only grabs max of 1024 bytes TODO: change to TCPServer's buffer_size
grab = 1024 if full_message_size - len(data) > 1024 else full_message_size - len(data)
packet = self.conn.recv(grab)

if not packet:
print("No packets...")
break

data += packet

if not data:
print("No data for a message size of {}, breaking!".format(full_message_size))
return
destination, data = self.read_message(self.conn)

if destination == '__syscommand':
self.tcp_server.handle_syscommand(data)
Expand All @@ -154,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())
Expand Down
4 changes: 4 additions & 0 deletions src/ros_tcp_endpoint/server.py
Original file line number Diff line number Diff line change
Expand Up @@ -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:
"""
Expand Down Expand Up @@ -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)
Expand Down
2 changes: 2 additions & 0 deletions srv/RosUnityTopicList.srv
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
---
string[] topics

0 comments on commit 6db3ee4

Please sign in to comment.