def _attached_collision_object(self,
attached_collision_object,
operation=CollisionObject.ADD):
attached_collision_object.object.operation = operation
topic = Topic(self, '/attached_collision_object',
'moveit_msgs/AttachedCollisionObject')
topic.publish(attached_collision_object.msg)
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 collision_mesh(self, id_name, root_link, compas_mesh, operation=0):
"""
"""
co = self.build_collision_object(root_link, id_name, compas_mesh,
operation)
topic = Topic(self, '/collision_object', 'moveit_msgs/CollisionObject')
topic.publish(co.msg)
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_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 __init__(self, ros, timeout=None,
omit_feedback=False, omit_status=False, omit_result=False):
super(DirectUrActionClient, self).__init__()
self.server_name = '/ur_driver/URScript'
self.ros = ros
self.timeout = timeout
self.omit_feedback = omit_feedback
self.omit_status = omit_status
self.omit_result = omit_result
self.goal = None
self._received_status = False
# Advertise the UR Script topic
self._urscript_topic = Topic(ros, self.server_name, 'std_msgs/String')
self._urscript_topic.advertise()
# Create the topics associated with actionlib
self.status_listener = Topic(ros, '/tool_velocity', 'geometry_msgs/TwistStamped')
# Subscribe to the status topic
if not self.omit_status:
self.status_listener.subscribe(self._on_status_message)
# If timeout specified, emit a 'timeout' event if the action server does not respond
if self.timeout:
self.ros.call_later(self.timeout / 1000., self._trigger_timeout)
self._internal_state = 'idle'
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()
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 __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, ros: Ros):
self.ros = ros
try:
statistics_topic = Topic(self.ros, '/statistics',
'rosgraph_msgs/TopicStatistics')
self.stats_sub = statistics_topic.subscribe(
self.statistics_callback)
except ImportError:
# not supported before Indigo
pass
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 stop(self, forced: bool = False) -> None:
"""Stop the robot.
This is a blocking call. It will block execution until the robot
is gracefully stopped unless `forced` is set to `True`.
Args:
forced (bool): Forcefully stop the robot or not.
"""
msg = Twist()
t = Topic(self.ros(), "/cmd_vel", msg.ros_type())
t.publish(msg.ros_msg())
def init_planner(self):
self.collision_object_topic = Topic(self,
'/collision_object',
'moveit_msgs/CollisionObject',
queue_size=None)
self.collision_object_topic.advertise()
self.attached_collision_object_topic = Topic(
self,
'/attached_collision_object',
'moveit_msgs/AttachedCollisionObject',
queue_size=None)
self.attached_collision_object_topic.advertise()
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 handle_updates(self, ros: "Ros") -> Callable[[], None]:
"""Handle incoming update for `/scan` topic and update robot's ranges
accordingly.
Args:
ros (Ros): ROS client.
Returns:
A callback for unsubscribing the topic.
"""
topic = Topic(ros, "/scan", "sensor_msgs/LaserScan")
topic.subscribe(self.__scan_handler)
return lambda: topic.unsubscribe()
def handle_updates(self, ros: "Ros") -> Callable[[], None]:
"""Handle incoming update for `/odom` topic and update robot's position
accordingly.
Args:
ros (Ros): ROS client.
Returns:
A callback for unsubscribing the topic.
"""
topic = Topic(ros, "/odom", "nav_msgs/Odometry")
topic.subscribe(self.__odom_handler)
return lambda: topic.unsubscribe()
def handle_updates(self, ros: "Ros") -> Callable[[], None]:
"""Handle incoming update for `/camera/rgb/image_raw/compressed` topic and
update robot's image accordingly.
Args:
ros (Ros): ROS client.
Returns:
A callback for unsubscribing the topic.
"""
topic = Topic(ros, "/camera/rgb/image_raw/compressed",
"sensor_msgs/CompressedImage")
topic.subscribe(self.__image_handler)
return lambda: topic.unsubscribe()
def attached_collision_mesh(self,
id_name,
ee_link,
compas_mesh,
operation=1,
touch_links=None):
"""
"""
aco = self.build_attached_collision_mesh(ee_link,
id_name,
compas_mesh,
operation,
touch_links=None)
topic = Topic(self, '/attached_collision_object',
'moveit_msgs/AttachedCollisionObject')
topic.publish(aco.msg)
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 RunScript(self, ros_client, topic_name, topic_type, interval, start,
stop):
if not topic_name:
raise ValueError('Please specify the name of the topic')
if not topic_type:
raise ValueError('Please specify the type of the topic')
if not hasattr(self, 'message_count'):
self.message_count = 0
self.interval = interval or 25 # in milliseconds
self.is_updating = False
self.is_subscribed = False
self.msg_key = create_id(self, 'last_msg')
key = create_id(self, 'topic')
last_msg = st.get(self.msg_key, None)
topic = st.get(key, None)
if ros_client and ros_client.is_connected:
if start:
self._unsubscribe(topic)
topic = Topic(ros_client, topic_name, topic_type)
topic.subscribe(self.topic_callback)
time.sleep(0.2)
st[key] = topic
if stop:
self._unsubscribe(topic)
self.is_subscribed = topic and topic.is_subscribed
if last_msg:
last_msg = ROSmsg.parse(last_msg, topic_type)
if self.is_subscribed:
self.Message = 'Subscribed, {} messages'.format(self.message_count)
else:
self.Message = 'Not subscribed'
return (topic, last_msg, self.is_subscribed)
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 __init__(self, ros: Ros, parent=None):
super(LogWidget, self).__init__(parent)
self.setMinimumSize(100, 100)
self.listWidget = QListWidget()
self.listWidget.setStyleSheet(
"QListWidget::item { border-bottom: 1px solid black; }")
layout = QVBoxLayout()
layout.addWidget(self.listWidget)
self._clear_button = QPushButton('清除记录')
self._clear_button.setFixedWidth(100)
self._clear_button.clicked.connect(self.clear_logs)
btn_layout = QHBoxLayout()
btn_layout.addWidget(self._clear_button)
layout.addLayout(btn_layout)
self.setLayout(layout)
self.ros_client = ros
self._log_topic = Topic(self.ros_client, '/rosout',
'rosgraph_msgs/Log')
self._log_topic.subscribe(self.on_log)
self.recv_msg.connect(self.on_recv_msg)
class LogWidget(QWidget):
recv_msg = pyqtSignal(dict)
def __init__(self, ros: Ros, parent=None):
super(LogWidget, self).__init__(parent)
self.setMinimumSize(100, 100)
self.listWidget = QListWidget()
self.listWidget.setStyleSheet(
"QListWidget::item { border-bottom: 1px solid black; }")
layout = QVBoxLayout()
layout.addWidget(self.listWidget)
self._clear_button = QPushButton('清除记录')
self._clear_button.setFixedWidth(100)
self._clear_button.clicked.connect(self.clear_logs)
btn_layout = QHBoxLayout()
btn_layout.addWidget(self._clear_button)
layout.addLayout(btn_layout)
self.setLayout(layout)
self.ros_client = ros
self._log_topic = Topic(self.ros_client, '/rosout',
'rosgraph_msgs/Log')
self._log_topic.subscribe(self.on_log)
self.recv_msg.connect(self.on_recv_msg)
def _insert_item_front(self, name: str, value):
pitem = LogItem(name, value, self.listWidget)
litem = QListWidgetItem()
pitem.show()
self.listWidget.insertItem(0, litem)
self.listWidget.setItemWidget(litem, pitem)
litem.setSizeHint(QSize(pitem.rect().width(), pitem.rect().height()))
def on_log(self, msg):
self.recv_msg.emit(msg)
def on_recv_msg(self, msg):
self._insert_item_front(msg['name'], msg['msg'])
def clear_logs(self):
self.listWidget.clear()
def RunScript(self, ros_client, topic_name, topic_type, msg):
if not topic_name:
raise ValueError('Please specify the name of the topic')
if not topic_type:
raise ValueError('Please specify the type of the topic')
key = create_id(self, 'topic')
topic = st.get(key, None)
if ros_client and ros_client.is_connected:
if not topic:
topic = Topic(ros_client, topic_name, topic_type)
topic.advertise()
time.sleep(0.2)
st[key] = topic
self.is_advertised = topic and topic.is_advertised
if msg:
msg = ROSmsg.parse(msg, topic_type)
topic.publish(msg.msg)
self.Message = 'Message published'
else:
if self.is_advertised:
self.Message = 'Topic advertised'
return (topic, self.is_advertised)
def rotate(self, angle: float = 0, duration: float = 0, speed: float = 0) -> None:
"""Rotate the robot at a certain angle at a certain speed or for a
certain duration.
To rotate clockwise, set the speed to positive. To rotate in
anti-clockwise, set the speed to negative. If the given speed is
larger than the maximum speed, it will be set to the maximum
speed.
Args:
angle (float): Angle to rotate (in degrees).
duration (float): Time duration to rotate for (in seconds).
speed (float): Rotation speed (radian per seconds).
"""
freq = DEFAULT_LOOP_FREQUENCY
msg = Twist()
t = Topic(self.ros(), "/cmd_vel", msg.ros_type())
# Rotate at $speed for $angle
if speed > 0 and angle != 0:
rad = np.deg2rad(angle)
duration = abs(rad) / speed
# Rotate for $duration to $angle
if duration > 0 and angle != 0:
rad = np.deg2rad(angle)
speed = rad / duration
# Rotate for $duration at $speed
if duration > 0 and speed != 0:
msg.zz = speed
end_time = time.time() + duration
while time.time() <= end_time:
t.publish(msg.ros_msg())
time.sleep(1 / freq)
self.stop()
def start_chat():
print('Connected as {}! [type a message or q to quit]'.format(name))
talker = Topic(client, '/messages', 'std_msgs/String')
talker.advertise()
listener = Topic(client, '/messages', 'std_msgs/String')
listener.subscribe(receive_message)
while client.is_connected:
text = get_input()
if text == 'q': break
send_message(talker, text)
client.terminate()
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 move(self, distance: float = 0, duration: float = 0, speed: float = 0) -> None:
"""Move the robot at a certain distance at a certain speed or for a
certain duration.
To move backwards, set speed to negative. If the given speed is
larger than the maximum speed, it will be set to the maximum
speed.
Args:
distance (float): Distance to travel.
duration (float): Time duration to travel for (in seconds).
speed (float): Travel speed.
"""
freq = DEFAULT_LOOP_FREQUENCY
msg = Twist()
t = Topic(self.ros(), "/cmd_vel", msg.ros_type())
# Move at $speed for $distance
if speed > 0 and distance != 0:
duration = distance / speed
# Move for $duration to $distance
if duration > 0 and distance != 0:
speed = distance / duration
# Move for $duration at $speed
if duration > 0 and speed != 0:
msg.x = speed
end_time = time.time() + duration
while time.time() <= end_time:
t.publish(msg.ros_msg())
time.sleep(1 / freq)
self.stop()
def __init__(self, ros: Ros, parent=None):
super(ImageWidget, self).__init__(parent)
if not os.path.exists(IMAGE_SAVE_DIR):
os.mkdir(IMAGE_SAVE_DIR)
self.ros_client = ros
layout = QVBoxLayout()
self._image_label = ImageLabel()
self._image_label.shot.connect(self.on_shot)
layout.addWidget(self._image_label)
btn_layout = QHBoxLayout()
self._subscribe_btn = QPushButton('订阅图像')
self._subscribe_btn.clicked.connect(self.subscribe_image)
self._type_combo_box = QComboBox()
self._type_combo_box.addItems(['None', 'Origin', 'Result'])
self._type_combo_box.currentIndexChanged.connect(self.on_type_changed)
self._func_combo_box = QComboBox()
self._func_combo_box.addItems(['function1', 'function2'])
self._save_btn = QPushButton('保存')
self._save_btn.clicked.connect(self.on_save_clicked)
self._auto_save_check_box = QCheckBox('自动保存')
self._auto_save_check_box.toggled.connect(self.on_auto_save_check)
btn_layout.addWidget(self._subscribe_btn)
btn_layout.addWidget(self._type_combo_box)
btn_layout.addWidget(self._func_combo_box)
btn_layout.addWidget(self._save_btn)
btn_layout.addWidget(self._auto_save_check_box)
layout.addLayout(btn_layout)
self.setLayout(layout)
self._save = False
self._auto_save = False
self._subscribe = False
self._image_topic = Topic(self.ros_client, '/result/vision/compressed',
'sensor_msgs/CompressedImage')
self.on_image.connect(self.on_recv_image)
self._last_seq = 0
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()
