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_act_debug(self):
if not self.ros_client.is_connected:
QMessageBox.critical(self, "错误", '尚未连接到ROS')
return
if self.host == '127.0.0.1' or self.host == 'localhost':
self.action_debug_client = self.ros_client
cmd = 'rosrun action action_debuger'
else:
cmd = 'roslaunch start start_action_debug_robot.launch'
try:
if self.action_debug_client is None:
print("run action_debug_client at 127.0.0.1:9090")
self.action_debug_client = Ros('127.0.0.1', 9090)
self.action_debug_client.run()
elif not self.action_debug_client.is_connected:
print("connecting action debug client")
self.action_debug_client.connect()
except Exception as e:
QMessageBox.critical(self, "错误", '无法连接到本地调试器 %s' % str(e))
return
act_service = Service(self.action_debug_client, '/debug/action/run',
'common/AddAngles')
act_service.advertise(self.on_recv_action)
os.popen(cmd)
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 __init__(self, ip, port, local_topic_name, remote_topic_name, message_type):
self._ip = ip
self._port = port
self._local_topic_name = local_topic_name
self._remote_topic_name = remote_topic_name
self._message_type = message_type
self._ros_client= Ros(self._ip,self._port)
self._rosPub = None # Should be specified by the automation engine
self._listener = None
self._logger = logging.getLogger(self.__class__.__name__)
self._logger.setLevel(logging.DEBUG)
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_empty_service():
ros_client = Ros('127.0.0.1', 9090)
ros_client.run()
service = Service(ros_client, '/test_empty_service', 'std_srvs/Empty')
service.advertise(lambda req, resp: True)
time.sleep(1)
client = Service(ros_client, '/test_empty_service', 'std_srvs/Empty')
client.call(ServiceRequest())
service.unadvertise()
time.sleep(2)
service.ros.terminate()
class ROSCloudSubBridge(object):
def __init__(self, ip, port, local_topic_name, remote_topic_name,
message_type, file):
self._ip = ip
self._port = port
self._local_topic_name = local_topic_name
self._remote_topic_name = remote_topic_name
self._message_type = message_type
self._ros_client = Ros(self._ip, self._port)
self._rosPub = None # Should be specified by the automation engine
self._listener = None # Should be specified by the automation engine
self._service = None
self._logger = logging.getLogger(self.__class__.__name__)
self._logger.setLevel(logging.DEBUG)
self._file = file
def _run_topic_pubsub(self):
try:
self._listener = Topic(self._ros_client, self._remote_topic_name,
self._message_type)
rospy.init_node('cloudsub', anonymous=True)
self._ros_client.send_on_ready(Message(self._file))
global my
my = rospy.get_time()
self._ros_client.on_ready(self._start_receiving,
run_in_thread=True)
self._ros_client.run_event_loop()
except:
self._logger.exception("Fatal error in constructor")
def _receive_message(self, message):
global my
rospy.loginfo(rospy.get_caller_id() + " Message type %s ",
self._message_type)
rospy.loginfo(
rospy.get_caller_id() + " Time from previous message %s ",
(rospy.get_time() - my))
my = rospy.get_time()
try:
msg = Clock()
msg.clock.secs = message['clock']['secs']
msg.clock.nsecs = message['clock']['nsecs']
self._rosPub = rospy.Publisher(
self._local_topic_name, Clock, queue_size=10
) #message type is String instead of msg_stds/String
self._rosPub.publish(msg)
except:
print('Error')
def _start_receiving(self):
self._listener.subscribe(self._receive_message)
def start(self):
signal.signal(signal.SIGINT, lambda *x: self.stop())
self._run_topic_pubsub()
def stop(self):
if self._ros_client is not None:
self._ros_client.terminate()
def connect(self, host: str, port: int) -> None:
"""Connect to the robot via ROS bridge.
Args:
host (str): Name or IP address of the ROS bridge host, e.g. `127.0.0.1`.
port (int): ROS bridge port, e.g. `9090`.
"""
ros = Ros(host, port)
ros.run()
self._ros = ros
def _set_connecting_flag(*args):
self._connected = True
self._ros.on_ready(_set_connecting_flag)
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
}
def test_rosapi_topics_blocking():
ros = Ros(host, port)
ros.run()
topic_list = ros.get_topics()
print(topic_list)
assert ('/rosout' in topic_list)
ros.close()
def run_rosapi_topics_blocking(*args, **kwargs):
ros_client = Ros(*args, **kwargs)
ros_client.run()
topic_list = ros_client.get_topics()
print(topic_list)
assert ('/rosout' in topic_list)
ros_client.terminate()
def run_reconnect_does_not_trigger_on_client_close():
ros = Ros('127.0.0.1', 9090)
ros.run()
assert ros.is_connected, "ROS initially connected"
time.sleep(0.5)
event = threading.Event()
ros.on('close', lambda m: event.set())
ros.close()
event.wait(5)
assert not ros.is_connected, "Successful disconnect"
assert not ros.is_connecting, "Not trying to re-connect"
class ROSCloudSubBridge(object):
def __init__(self, ip, port, local_topic_name, remote_topic_name, message_type):
self._ip = ip
self._port = port
self._local_topic_name = local_topic_name
self._remote_topic_name = remote_topic_name
self._message_type = message_type
self._ros_client= Ros(self._ip,self._port)
self._rosPub = None # Should be specified by the automation engine
self._listener = None # Should be specified by the automation engine
self._logger = logging.getLogger(self.__class__.__name__)
self._logger.setLevel(logging.DEBUG)
self.br =tf2_ros.StaticTransformBroadcaster()
self.t = []
def _run_topic_pubsub(self):
try:
self._listener = Topic(self._ros_client, self._remote_topic_name, self._message_type)
rospy.init_node('cloudsub', anonymous=True)
global my
my=rospy.get_time()
self._ros_client.on_ready(self._start_receiving, run_in_thread=True)
self._ros_client.run_event_loop()
except:
self._logger.exception("Fatal error in constructor")
def _receive_message(self,message):
global my
rospy.loginfo(rospy.get_caller_id() + " Message type %s ",self._message_type)
rospy.loginfo(rospy.get_caller_id() + " Time from previous message %s ",(rospy.get_time()-my))
my=rospy.get_time()
for i in range(len(message['transforms'])):
self.t.append(TransformStamped())
self.t[-1].header.stamp = rospy.Time.now()
self.t[-1].header.frame_id = message['transforms'][i]['header']['frame_id']
self.t[-1].child_frame_id = message['transforms'][i]['child_frame_id']
self.t[-1].transform.translation.x = message['transforms'][i]['transform']['translation']['x']
self.t[-1].transform.translation.y = message['transforms'][i]['transform']['translation']['y']
self.t[-1].transform.translation.z = message['transforms'][i]['transform']['translation']['z']
self.t[-1].transform.rotation.x = message['transforms'][i]['transform']['rotation']['x']
self.t[-1].transform.rotation.y = message['transforms'][i]['transform']['rotation']['y']
self.t[-1].transform.rotation.z = message['transforms'][i]['transform']['rotation']['z']
self.t[-1].transform.rotation.w = message['transforms'][i]['transform']['rotation']['w']
self.br.sendTransform(self.t)
def _start_receiving(self):
self._listener.subscribe(self._receive_message)
def start(self):
signal.signal(signal.SIGINT, lambda *x: self.stop())
self._run_topic_pubsub()
def stop(self):
if self._ros_client is not None:
self._ros_client.terminate()
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_event_loop()
class ROSCloudSubBridge(object):
def __init__(self, ip, port, local_topic_name, remote_topic_name, message_type):
self._ip = ip
self._port = port
self._local_topic_name = local_topic_name
self._remote_topic_name = remote_topic_name
self._message_type = message_type
self._ros_client= Ros(self._ip,self._port)
self._rosPub = None # Should be specified by the automation engine
self._listener = None
self._logger = logging.getLogger(self.__class__.__name__)
self._logger.setLevel(logging.DEBUG)
def _run_topic_pubsub(self):
try:
self._listener = Topic(self._ros_client, self._remote_topic_name, self._message_type)
self._listener2= Topic(self._ros_client, self._remote_topic_name, self._message_type)
rospy.init_node('cloudsub', anonymous=True)
global my
my=rospy.get_time()
self._ros_client.on_ready(self._start_receiving, run_in_thread=True)
self._ros_client.run_event_loop()
except:
self._logger.exception("Fatal error in constructor")
def _receive_message(self,message):
global my
rospy.loginfo(rospy.get_caller_id() + " I heard a message of %s",self._message_type)
rospy.loginfo(rospy.get_caller_id() + " Time from previous message %s",(rospy.get_time()-my) )#edw mou leei unicode type
my=rospy.get_time()
try:
msg=Image()
msg.header.seq=message['header']['seq']
msg.header.stamp.secs=message['header']['stamp']['secs']
msg.header.stamp.nsecs=message['header']['stamp']['nsecs']
msg.header.frame_id=message['header']['frame_id']
msg.height=message['height']
msg.width=message['width']
msg.encoding=str(message['encoding'])
msg.is_bigendian=message['is_bigendian']
msg.step=message['step']
msg.data=message['data'].decode('base64')
self._rosPub=rospy.Publisher(self._local_topic_name, Image, queue_size=10) #message type is String instead of msg_stds/String
self._rosPub.publish(msg)
except:
print('Error')
def _start_receiving(self):
self._listener.subscribe(self._receive_message)
def start(self):
signal.signal(signal.SIGINT, lambda *x: self.stop())
self._run_topic_pubsub()
def stop(self):
if self._ros_client is not None:
self._ros_client.terminate()
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_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_add_two_ints_service():
ros_client = Ros('127.0.0.1', 9090)
ros_client.run()
def add_two_ints(request, response):
response['sum'] = request['a'] + request['b']
return False
service_name = '/test_sum_service'
service_type = 'rospy_tutorials/AddTwoInts'
service = Service(ros_client, service_name, service_type)
service.advertise(add_two_ints)
time.sleep(1)
client = Service(ros_client, service_name, service_type)
result = client.call(ServiceRequest({'a': 2, 'b': 40}))
assert (result['sum'] == 42)
service.unadvertise()
time.sleep(2)
service.ros.terminate()
def test_rosapi_topics():
context = dict(wait=threading.Event(), result=None)
ros = Ros(host, port)
ros.run()
def callback(topic_list):
context['result'] = topic_list
context['wait'].set()
ros.get_topics(callback)
if not context['wait'].wait(5):
raise Exception
assert ('/rosout' in context['result']['topics'])
ros.close()
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)
def test_closing_event():
ros = Ros(url)
ros.run()
ctx = dict(closing_event_called=False, was_still_connected=False)
def handle_closing():
ctx['closing_event_called'] = True
ctx['was_still_connected'] = ros.is_connected
time.sleep(1.5)
ts_start = time.time()
ros.on('closing', handle_closing)
ros.close()
ts_end = time.time()
closing_was_handled_synchronously_before_close = ts_end - ts_start >= 1.5
assert ctx['closing_event_called']
assert ctx['was_still_connected']
assert closing_was_handled_synchronously_before_close
def run_rosapi_topics():
ros_client = Ros('127.0.0.1', 9090)
def callback(topic_list):
assert ('/rosout' in topic_list['topics'])
ros_client.terminate()
def get_topics():
ros_client.get_topics(callback)
ros_client.on_ready(get_topics)
ros_client.run_event_loop()
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 test_param_manipulation():
ros = Ros('127.0.0.1', 9090)
ros.run()
param = Param(ros, 'test_param')
assert param.get() is None
param.set('test_value')
assert param.get() == 'test_value'
param.delete()
assert param.get() is None
ros.close()
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()
def test_topic_pubsub():
context = dict(wait=threading.Event(), counter=0)
ros = Ros('127.0.0.1', 9090)
ros.run()
listener = Topic(ros, '/chatter', 'std_msgs/String')
publisher = Topic(ros, '/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()
context['wait'].set()
def start_sending():
while True:
if context['counter'] >= 3:
break
publisher.publish(Message({'data': 'hello world'}))
time.sleep(0.1)
publisher.unadvertise()
def start_receiving():
listener.subscribe(receive_message)
t1 = threading.Thread(target=start_receiving)
t2 = threading.Thread(target=start_sending)
t1.start()
t2.start()
if not context['wait'].wait(10):
raise Exception
t1.join()
t2.join()
assert context[
'counter'] >= 3, 'Expected at least 3 messages but got ' + str(
context['counter'])
ros.close()
def test_tf_test():
context = dict(wait=threading.Event(), counter=0)
ros = Ros('127.0.0.1', 9090)
ros.run()
tf_client = TFClient(ros, fixed_frame='world')
def callback(message):
context['message'] = message
context['counter'] += 1
context['wait'].set()
tf_client.subscribe(frame_id='/world', callback=callback)
if not context['wait'].wait(5):
raise Exception
assert context['counter'] > 0
assert context['message']['translation'] == dict(
x=0.0, y=0.0, z=0.0), 'Unexpected translation received'
assert context['message']['rotation'] == dict(
x=0.0, y=0.0, z=0.0, w=1.0), 'Unexpected rotation received'
ros.close()
if not callback or ('cbs' in frame and len(frame['cbs']) == 0):
self.frame_info.pop(frame_id)
def dispose(self):
"""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)
class ROSCloudSubBridge(object):
def __init__(self, ip, port, local_topic_name, remote_topic_name,
message_type):
self._ip = ip
self._port = port
self._local_topic_name = local_topic_name
self._remote_topic_name = remote_topic_name
self._message_type = message_type
self._ros_client = Ros(self._ip, self._port)
self._rosPub = None # Should be specified by the automation engine
self._listener = None # Should be specified by the automation engine
# self._2listener = None # Should be specified by the automation engine
self._logger = logging.getLogger(self.__class__.__name__)
self._logger.setLevel(logging.DEBUG)
def _run_topic_pubsub(self):
try:
self._listener = Topic(self._ros_client, self._remote_topic_name,
self._message_type)
# self._2listener = Topic(self._ros_client, self._remote_topic_name, self._message_type)
rospy.init_node('cloudsub', anonymous=True)
global my
# global my2
# my2=rospy.get_time()
my = rospy.get_time()
self._ros_client.on_ready(self._start_receiving,
run_in_thread=True)
self._ros_client.run_event_loop()
except:
self._logger.exception("Fatal error in constructor")
def _receive_message(self, message):
global my
rospy.loginfo(rospy.get_caller_id() + " Message type %s ",
self._message_type)
rospy.loginfo(
rospy.get_caller_id() + " Time from previous message %s ",
(rospy.get_time() - my))
my = rospy.get_time()
try:
msg = Status()
msg.header.seq = message['header']['seq']
msg.header.stamp.secs = message['header']['stamp']['secs']
msg.header.stamp.nsecs = message['header']['stamp']['nsecs']
msg.header.frame_id = message['header']['frame_id']
msg.id = message['id']
msg.active = message['active']
msg.heartbeat_timeout = message['heartbeat_timeout']
msg.heartbeat_period = message['heartbeat_period']
msg.instance_id = message['instance_id']
# for i in range(0,8):
# msg.orientation_covariance[i]=message['orientation_covariance'][i]
# msg.angular_velocity_covariance[i]=message['angular_velocity_covariance'][i]
# msg.linear_acceleration_covariance[i]=message['linear_acceleration_covariance'][i]
self._rosPub = rospy.Publisher(self._local_topic_name,
Status,
queue_size=10)
self._rosPub.publish(msg)
except:
print('Error')
# def _2receive_message(self,message):
# global my2
# rospy.loginfo(rospy.get_caller_id() + "I heard a message %s",(rospy.get_time()-my2))
# my2=rospy.get_time()
# try:
# msg=Clock()
# msg.clock.secs=message['clock']['secs']
# msg.clock.nsecs=message['clock']['nsecs']
# self._rosPub=rospy.Publisher('/test/clock2', Clock, queue_size=10) #message type $
# self._rosPub.publish(msg)
# except:
# print('Error')
def _start_receiving(self):
self._listener.subscribe(self._receive_message)
# self._listener2.subscribe(self._2receive_message)
def start(self):
signal.signal(signal.SIGINT, lambda *x: self.stop())
self._run_topic_pubsub()
def stop(self):
if self._ros_client is not None:
self._ros_client.terminate()
def test_connection_fails_when_schema_not_ws():
with pytest.raises(Exception):
Ros(u'http://%s:%d' % (host, port))
