import roslibpy
import time

def println(ros_list):
    if len(ros_list) == 0:
        print('Empty')
    else:
        for i in ros_list:
            print(i)
    return 0

def subscribe_callback(message):
    if message['data'] == 0:
        #print("it's 0")
    else:
        print(message)

host = input("input host IP: ")

client = roslibpy.Ros(host = host, port = 9090)
client.run()
print('Is ROS connected?', client.is_connected)

input host IP: 192.168.1.12
Is ROS connected? True

while True:
    command = input('please input command(topic, node, service, subscriber quit): ')
    if command == 'topic':
        print('topic')
        topic = client.get_topics()
        println(topic)

        
    elif command == 'node':
        print('node')
        node = client.get_nodes()
        println(node)
        
#     elif command == 'service':
#         print('service')
#         service = client.get_services()
#         println(service)
    
#     elif command == 'para':
#         print('parameter')
#         para = client.get_params()
#         println(para)
#     elif command == 'publisher':
#         message_data = input('what message you want to publish')
#         talker = roslibpy.Topic(client, '/chatter', 'std_msgs/String')
#         for i in range(1,100):
#             talker.publish(roslibpy.Message({'data': message_data}))
#             print('Sending message...')
#             time.sleep(1)
#         talker.unadvertise()
        
    elif command == 'subscriber':
        topic = client.get_topics()
        println(topic)
        topic_name = input('what topic you want to listen')
        topic_type = client.get_topic_type(topic_name)
        print('type_is ' + topic_type)
        listener = roslibpy.Topic(client, topic_name, topic_type, throttle_rate=1000)
        listener.subscribe(subscribe_callback)

    elif command == 'close':
        listener.unsubscribe(topic_name)
         
    elif command == 'quit':
        client.terminate()
        break

please input command(topic, node, service, subscriber quit): subscriber
/client_count
/anchor01/ranges
/rosout
/rosout_agg
/connected_clients
/joy/set_feedback
/joy
/diagnostics
/anchor01/distance
what topic you want to listen/anchor01/distance
type_is std_msgs/Float64
{'data': 3694.0}
{'data': 0.0}
{'data': 3661.0}
{'data': 0.0}
{'data': 3684.0}
{'data': 0.0}
{'data': 3708.0}
{'data': 3708.0}
{'data': 0.0}
{'data': 0.0}
{'data': 3642.0}
{'data': 3698.0}
{'data': 3680.0}
{'data': 3694.0}
{'data': 0.0}
{'data': 0.0}
{'data': 3694.0}
{'data': 3698.0}
{'data': 3703.0}
{'data': 3694.0}
{'data': 0.0}
{'data': 0.0}
{'data': 0.0}
{'data': 3698.0}
{'data': 0.0}
{'data': 0.0}
{'data': 3703.0}
{'data': 3694.0}
{'data': 3712.0}
{'data': 0.0}
{'data': 3741.0}
{'data': 3698.0}
{'data': 3694.0}
{'data': 3717.0}
{'data': 3698.0}
{'data': 3670.0}
{'data': 3661.0}
{'data': 3750.0}
{'data': 3703.0}
{'data': 3661.0}
{'data': 0.0}
{'data': 0.0}
{'data': 0.0}
{'data': 0.0}
{'data': 3694.0}
{'data': 3694.0}
{'data': 3684.0}
{'data': 3689.0}
{'data': 0.0}
{'data': 3694.0}
{'data': 3689.0}
{'data': 3694.0}
{'data': 3712.0}
{'data': 3680.0}
{'data': 3680.0}
{'data': 3712.0}
{'data': 3708.0}

---------------------------------------------------------------------------
KeyboardInterrupt                         Traceback (most recent call last)
Input In [4], in <cell line: 2>()
      1 while True:
----> 2     command = input('please input command(topic, node, service, subscriber quit): ')
      3     if command == 'topic':
      4         print('topic')

File /usr/local/lib/python3.8/dist-packages/ipykernel/kernelbase.py:1165, in Kernel.raw_input(self, prompt)
   1161 if not self._allow_stdin:
   1162     raise StdinNotImplementedError(
   1163         "raw_input was called, but this frontend does not support input requests."
   1164     )
-> 1165 return self._input_request(
   1166     str(prompt),
   1167     self._parent_ident["shell"],
   1168     self.get_parent("shell"),
   1169     password=False,
   1170 )

File /usr/local/lib/python3.8/dist-packages/ipykernel/kernelbase.py:1207, in Kernel._input_request(self, prompt, ident, parent, password)
   1204             break
   1205 except KeyboardInterrupt:
   1206     # re-raise KeyboardInterrupt, to truncate traceback
-> 1207     raise KeyboardInterrupt("Interrupted by user") from None
   1208 except Exception:
   1209     self.log.warning("Invalid Message:", exc_info=True)

KeyboardInterrupt: Interrupted by user