def run_topic_pubsub():
context = {'counter': 0}
ros_client = Ros('127.0.0.1', 9090)
listener = Topic(ros_client, '/chatter', 'std_msgs/String')
publisher = Topic(ros_client, '/chatter', 'std_msgs/String')
def receive_message(message):
context['counter'] += 1
assert message['data'] == 'hello world', 'Unexpected message content'
if context['counter'] == 3:
listener.unsubscribe()
# Give it a bit of time, just to make sure that unsubscribe
# really unsubscribed and counter stays at the asserted value
ros_client.call_later(2, ros_client.terminate)
def start_sending():
while True:
if not ros_client.is_connected:
break
publisher.publish(Message({'data': 'hello world'}))
time.sleep(0.1)
publisher.unadvertise()
def start_receiving():
listener.subscribe(receive_message)
ros_client.on_ready(start_receiving, run_in_thread=True)
ros_client.on_ready(start_sending, run_in_thread=True)
ros_client.run_forever()
assert context[
'counter'] >= 3, 'Expected at least 3 messages but got ' + str(
context['counter'])
def run_param_manipulation():
ros_client = Ros('127.0.0.1', 9090)
def delete_param_done(_):
ros_client._test_param_del = True
def get_param_done(value):
ros_client._test_param_value = value
delete_param = Param(ros_client, 'test_param')
delete_param.delete(delete_param_done)
def set_param_done(_):
ros_client._test_param_set = True
check_param = Param(ros_client, 'test_param')
check_param.get(get_param_done)
param = Param(ros_client, 'test_param')
param.set('test_value', set_param_done)
def verify():
assert(ros_client._test_param_set is True)
assert(ros_client._test_param_value == 'test_value')
assert(ros_client._test_param_del is True)
ros_client.terminate()
ros_client.call_later(1, verify)
ros_client.run_forever()
def run_add_two_ints_service():
ros_client = Ros('127.0.0.1', 9090)
service = Service(ros_client, '/test_server',
'rospy_tutorials/AddTwoInts')
def dispose_server():
service.unadvertise()
service.ros.call_later(1, service.ros.terminate)
def add_two_ints(request, response):
response['sum'] = request['a'] + request['b']
if response['sum'] == 42:
service.ros.call_later(2, dispose_server)
return True
def check_sum(result):
assert(result['sum'] == 42)
def invoke_service():
client = Service(ros_client, '/test_server',
'rospy_tutorials/AddTwoInts')
client.call(ServiceRequest({'a': 2, 'b': 40}), check_sum, print)
service.advertise(add_two_ints)
ros_client.call_later(1, invoke_service)
ros_client.run_forever()
def run_topic_pubsub():
context = {'counter': 0}
ros_client = Ros('192.168.1.106', 9090)
listener = Topic(ros_client, '/chatter', 'std_msgs/String')
publisher = Topic(ros_client, '/chatter', 'std_msgs/String')
def receive_message(message):
print("============================")
context['counter'] += 1
assert message['data'] == 'hello world', 'Unexpected message content'
if context['counter'] == 3:
listener.unsubscribe()
# Give it a bit of time, just to make sure that unsubscribe
# really unsubscribed and counter stays at the asserted value
ros_client.call_later(2, ros_client.terminate)
# def callback(topic_list):
# print(topic_list)
# assert('/rosout' in topic_list['topics'])
# time.sleep(1)
# ros_client.terminate()
def start_sending():
while True:
print("---------------------")
if not ros_client.is_connected:
break
publisher.publish(Message({'data': 'hello world'}))
print(listener.is_subscribed)
time.sleep(0.1)
ros_client.terminate
break
# ros_client.get_topics(callback)
publisher.unadvertise()
def start_receiving():
listener.subscribe(receive_message)
ros_client.on_ready(start_receiving, run_in_thread=True)
ros_client.on_ready(start_sending, run_in_thread=True)
ros_client.run_forever()
assert context[
'counter'] >= 3, 'Expected at least 3 messages but got ' + str(
context['counter'])
def run_rosapi_topics(*args, **kwargs):
ros_client = Ros(*args, **kwargs)
def callback(topic_list):
print(topic_list)
assert('/rosout' in topic_list['topics'])
time.sleep(1)
ros_client.terminate()
def get_topics():
ros_client.get_topics(callback)
ros_client.on_ready(get_topics)
ros_client.run_forever()
def run_tf_test():
ros_client = Ros('127.0.0.1', 9090)
tf_client = TFClient(ros_client, fixed_frame='world')
def callback(message):
assert message['translation'] == dict(
x=0.0, y=0.0, z=0.0), 'Unexpected translation received'
assert message['rotation'] == dict(
x=0.0, y=0.0, z=0.0, w=1.0), 'Unexpected rotation received'
ros_client.terminate()
tf_client.subscribe(frame_id='/world', callback=callback)
ros_client.call_later(2, ros_client.terminate)
ros_client.run_forever()
class rosbridge_client:
def __init__(self):
self.ros_client = Ros('127.0.0.1', 9090)
print("wait for server")
self.publisher = Topic(self.ros_client, '/cmd_vel',
'geometry_msgs/Twist')
self.listener = Topic(self.ros_client, '/odom', 'nav_msgs/Odometry')
self.listener.subscribe(self.callback)
self.ros_client.on_ready(self.start_thread, run_in_thread=True)
self.ros_client.run_forever()
def callback(self, message):
x = message["pose"]["pose"]["position"]["x"]
y = message["pose"]["pose"]["position"]["y"]
print(x, y)
def start_thread(self):
while True:
if self.ros_client.is_connected:
self.publisher.publish(
Message({
'linear': {
'x': 0.5,
'y': 0,
'z': 0
},
'angular': {
'x': 0,
'y': 0,
'z': 0.5
}
}))
else:
print("Disconnect")
break
time.sleep(1.0)
"""Unsubscribe and unadvertise all topics associated with this instance."""
if self.current_topic:
self.current_topic.unsubscribe()
if __name__ == '__main__':
from roslibpy import Ros
FORMAT = '%(asctime)-15s [%(levelname)s] %(message)s'
logging.basicConfig(level=logging.DEBUG, format=FORMAT)
ros_client = Ros('127.0.0.1', 9090)
def run_tf_example():
tfclient = TFClient(ros_client,
fixed_frame='world',
angular_threshold=0.01,
translation_threshold=0.01)
tfclient.subscribe('turtle2', print)
def dispose_server():
tfclient.dispose()
ros_client.call_later(10, dispose_server)
ros_client.call_later(11, ros_client.close)
ros_client.call_later(12, ros_client.terminate)
run_tf_example()
ros_client.run_forever()
class Muse_app:
def __init__(self):
self.client = mqtt.Client()
self.client2 = mqtt.Client()
self.ros_client = Ros(host='10.205.240.34', port=9090)
self.publisher = Topic(self.ros_client, '/chatter', 'std_msgs/String')
self.muse_left = "DC11"
self.muse_right = "C00E"
self.status_left = False
self.status_right = False
self.action_mode = 0
self.client.on_connect = self.on_connect
self.client.on_message = self.on_message
self.client.connect("localhost", 1883, 60)
t = threading.Thread(target=self.worker)
t.start()
self.client2.on_connect = self.on_connect2
self.client2.on_message = self.on_message
self.client2.connect("localhost", 1883, 60)
t2 = threading.Thread(target=self.worker2)
t2.start()
self.ros_client.on_ready(self.subscribing, run_in_thread=True)
t3 = threading.Thread(target=self.worker3)
t3.start()
while True:
pass
def on_connect(self, client, userdata, flags, rc):
print("Connected with result code "+str(rc))
# Subscribing in on_connect() means that if we lose the connection and
# reconnect then subscriptions will be renewed.
self.client.subscribe("topic/" + self.muse_left)
#self.client.subscribe("topic/test")
def on_connect2(self, client, userdata, flags, rc):
print("Connected with result code "+str(rc))
# Subscribing in on_connect() means that if we lose the connection and
# reconnect then subscriptions will be renewed.
self.client2.subscribe("topic/" + self.muse_right)
def worker(self):
self.client.loop_forever()
return
def worker2(self):
self.client2.loop_forever()
return
def on_message(self, client, userdata, message):
print("message received " ,str(message.payload.decode("utf-8")))
print("message topic=",message.topic)
if message.topic == "topic/" + self.muse_left:
if(message.payload.decode("utf-8") == '0'):
self.status_left = False
elif(message.payload.decode("utf-8") == '1'):
self.status_left = True
print("left = " , self.status_left)
if message.topic == "topic/" + self.muse_right:
if(message.payload.decode("utf-8") == '0'):
self.status_right = False
elif(message.payload.decode("utf-8") == '1'):
self.status_right = True
print("right = " , self.status_right)
# print("message qos=",message.qos)
# print("message retain flag=",message.retain)
def subscribing(self):
print("subscribe to Ros..")
self.publisher.subscribe(self.receive_message)
def uninitialize(self):
# nop
return
def receive_message(self, message):
# context['counter'] += 1
print('Receive data from Ros, ' , message['data'])
#assert message['data'] == 'hello world', 'Unexpected message content'
if(message['data'] == "action"):
self.client.publish("topic/parameters", 'action')
time.sleep(3)
print("lastest status from left = " , self.status_left)
print("lastest status from right = " , self.status_right)
if self.status_left and not self.status_right: ##======================TURN LEFT
self.action_mode = 1
elif not self.status_left and self.status_right: ##======================TURN RIGHT
self.action_mode = 2
elif not self.status_left and not self.status_right: ##======================MOVE FORWARD
self.action_mode = 3
elif self.status_left and self.status_right: ##======================MOVE BACKWARD
self.action_mode = 4
print("summary action mode = ", str(self.action_mode))
self.client.publish("topic/parameters", 'wait')
isConnected = self.ros_client.is_connected
print("isConnected = ", isConnected)
if(isConnected):
self.publisher.publish(Message({'data': str(self.action_mode)}))
def worker3(self):
self.ros_client.run_forever()
class MyOVBox(OVBox):
def __init__(self):
OVBox.__init__(self)
self.signalHeader = None
self.ros_client = Ros(host='10.204.231.147', port=9090)
#self.listener = Topic(self.ros_client, '/chatter', 'std_msgs/String')
self.publisher = Topic(self.ros_client, '/chatter', 'std_msgs/String')
self.ros_client.on_ready(self.subscribing, run_in_thread=True)
self.phase = 0
self.parameters = {
"count": "4",
"scenario": "4",
"gender": "Male",
"age": "25",
"subjectno": "55",
"phase": 5,
"type": 2
}
# ros_client.on_ready(start_receiving, run_in_thread=True)
def initialize(self):
self.counter = 0
t = threading.Thread(target=self.worker)
t.start()
s = threading.Thread(target=self.workerMQTT)
s.start()
# self.ros_client.run_forever()
return
def on_connect(self, client, userdata, flags, rc):
print("Connected with result code " + str(rc))
# Subscribing in on_connect() means that if we lose the connection and
# reconnect then subscriptions will be renewed.
self.client.subscribe("topic/parameters")
# The callback for when a PUBLISH message is received from the server.
def on_message(self, client, userdata, msg):
#print(msg.topic+" "+str(msg.payload))
parameters = msg.payload
print("parameters = ", parameters)
self.parameters = json.loads(parameters)
self.phase = int(self.parameters.get('phase'))
print("phase = ", self.phase)
def worker(self):
self.ros_client.run_forever()
def workerMQTT(self):
self.client = mqtt.Client()
self.client.on_connect = self.on_connect
self.client.on_message = self.on_message
self.client.connect("localhost", 1883, 60)
# Blocking call that processes network traffic, dispatches callbacks and
# handles reconnecting.
# Other loop*() functions are available that give a threaded interface and a
# manual interface.
self.client.loop_forever()
def process(self):
self.counter += 1
for chunkIndex in range(len(self.input[0])):
if (type(self.input[0][chunkIndex]) == OVSignalHeader):
self.signalHeader = self.input[0].pop()
outputHeader = OVSignalHeader(
self.signalHeader.startTime, self.signalHeader.endTime,
[1, self.signalHeader.dimensionSizes[1]],
['Mean'] + self.signalHeader.dimensionSizes[1] * [''],
self.signalHeader.samplingRate)
self.output[0].append(outputHeader)
elif (type(self.input[0][chunkIndex]) == OVSignalBuffer):
#Check if Condition != 0 , Meaning user has been clicked start scenario
#if(self.phase != 0):
chunk = self.input[0].pop()
numpyBuffer = np.array(chunk).reshape(
tuple(self.signalHeader.dimensionSizes))
#with open("C:/Users/h4wk/Desktop/Temp/foo.csv",'ab') as f:
#path = os.getcwd() + datetime.datetime.now().strftime("%B %d, %Y") + ".csv"
# if(self.phase != 0):
path = self.setting[
'OutputPath'] + '/BCI_GTEC_' + datetime.datetime.now(
).strftime("%Y%m%d_" + self.setting['subject_no']) + ".csv"
with open(path, 'ab') as f:
#t = time.localtime()
#print("Current path:" + path)
x = datetime.datetime.now()
#a = np.asarray([[time.mktime(x.timetuple()),
#Header Pattern: Timestamp, phase, scenario, subjectno, gender, age, type, count
a = np.asarray(int(round(time.time() * 1000)),
dtype=object)
# print(a)
# np.savetxt(f, a, delimiter=",", fmt='%5s')
for channel in numpyBuffer:
channel = np.insert(channel, 0, a, axis=0)
# print(channel.shape)
np.savetxt(f, [channel], delimiter=",")
#==================Sending to ROS Server==========================#
#print(self.counter)
# if(self.counter >= 20):
# self.counter = 0
# #self.ros_client.on_ready(self.publisher.publish(Message({'data': numpyBuffer})))
# isConnected = self.ros_client.is_connected
# #print("isConnected = ", isConnected)
# if(isConnected):
# print("sending data... ", numpyBuffer.tolist())
# #self.publisher.publish(Message({'data': numpyBuffer.tolist()}))
# a = " ".join(str(i) for i in numpyBuffer.tolist())
# self.publisher.publish(Message({'data': a}))
# #self.publisher.publish(Message({'data': 'hello test'}))
# #self.publisher.unadvertise()
# ==================End Sending to ROS Server==========================#
numpyBuffer = numpyBuffer.mean(axis=0)
chunk = OVSignalBuffer(chunk.startTime, chunk.endTime,
numpyBuffer.tolist())
self.output[0].append(chunk)
elif (type(self.input[0][chunkIndex]) == OVSignalEnd):
self.output[0].append(self.input[0].pop())
def subscribing(self):
self.publisher.subscribe(receive_message)
def uninitialize(self):
# nop
return
