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()
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 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()
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()
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=TFMessage()
msg1=TransformStamped()
list_msg=[]
for i in range(len(message['transforms'])) :
msg1.header.seq=message['transforms'][i]['header']['seq']
msg1.header.stamp.secs=message['transforms'][i]['header']['stamp']['secs']
msg1.header.stamp.nsecs=message['transforms'][i]['header']['stamp']['nsecs']
msg1.header.frame_id=message['transforms'][i]['header']['frame_id']
msg1.child_frame_id=message['transforms'][i]['child_frame_id']
msg1.transform.translation.x=message['transforms'][i]['transform']['translation']['x']
msg1.transform.translation.y=message['transforms'][i]['transform']['translation']['y']
msg1.transform.translation.z=message['transforms'][i]['transform']['translation']['z']
msg1.transform.rotation.x=message['transforms'][i]['transform']['rotation']['x']
msg1.transform.rotation.y=message['transforms'][i]['transform']['rotation']['y']
msg1.transform.rotation.z=message['transforms'][i]['transform']['rotation']['z']
msg1.transform.rotation.w=message['transforms'][i]['transform']['rotation']['w']
list_msg.append(msg1)
msg=TFMessage(list_msg)
self._rosPub=rospy.Publisher(self._local_topic_name, TFMessage, queue_size=10)
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()
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)
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()
# try:
msg = OccupancyGrid()
msg.header.seq = message['header']['seq']
msg.header.stamp = rospy.Time.now(
) #.secs=message['header']['stamp']['secs']
msg.header.frame_id = message['header']['frame_id'] #type unicode
msg.info.map_load_time.secs = message['info']['map_load_time']['secs']
msg.info.map_load_time.nsecs = message['info']['map_load_time'][
'nsecs']
msg.info.resolution = message['info']['resolution']
msg.info.width = message['info']['width']
msg.info.height = message['info']['height']
msg.info.origin.position.x = message['info']['origin']['position']['x']
msg.info.origin.position.y = message['info']['origin']['position']['y']
msg.info.origin.position.z = message['info']['origin']['position']['z']
msg.info.origin.orientation.x = message['info']['origin'][
'orientation']['x']
msg.info.origin.orientation.y = message['info']['origin'][
'orientation']['y']
msg.info.origin.orientation.z = message['info']['origin'][
'orientation']['z']
msg.info.origin.orientation.w = message['info']['origin'][
'orientation']['w']
msg.data = message['data']
# for i in range(len(message['data'])):
# print(i)
# msg.data[i]=message['data'][i]
self._rosPub = rospy.Publisher(self._local_topic_name,
OccupancyGrid,
queue_size=10)
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 MainWindow(QMainWindow):
on_run = pyqtSignal(dict)
disconnected = pyqtSignal()
def __init__(self):
super(MainWindow, self).__init__()
self.host, ok_pressed = QInputDialog.getText(self, '连接', '机器人地址',
QLineEdit.Normal,
'127.0.0.1')
if not ok_pressed or len(self.host) < 7:
exit(0)
try:
self.ros_client = Ros(self.host, 9090)
self.ros_client.run(3)
self.ros_client.on('close', self.on_lost_connect)
except Exception as e:
QMessageBox.critical(self, "错误", e.args[0])
exit(0)
self.setWindowTitle('调试器')
self.statusBar().setSizeGripEnabled(False)
self.statusBar().setStyleSheet('border: 1px solid black;')
self._toolMenu = self.menuBar().addMenu('工具')
self._srv_action = QAction('Service表', self._toolMenu)
self._srv_action.setCheckable(True)
self._srv_action.toggled.connect(self.on_srv_table_widget)
self._topics_action = QAction('Topics图', self._toolMenu)
self._topics_action.setCheckable(True)
self._topics_action.toggled.connect(self.on_topics_widget)
self._remote_action = QAction('遥控', self._toolMenu)
self._remote_action.setCheckable(True)
self._remote_action.toggled.connect(self.on_remote_widget)
self._logs_action = QAction('日志', self._toolMenu)
self._logs_action.setCheckable(True)
self._logs_action.toggled.connect(self.on_log_widget)
self._image_action = QAction('图像', self._toolMenu)
self._image_action.setCheckable(True)
self._image_action.toggled.connect(self.on_image_widget)
self._params_action = QAction('参数', self._toolMenu)
self._params_action.setCheckable(True)
self._params_action.toggled.connect(self.on_params_widget)
self._toolMenu.addAction(self._srv_action)
self._toolMenu.addAction(self._topics_action)
self._toolMenu.addAction(self._remote_action)
self._toolMenu.addAction(self._logs_action)
self._toolMenu.addAction(self._image_action)
self._toolMenu.addAction(self._params_action)
self.menuBar().addSeparator()
self._act_debug_action = self.menuBar().addAction('动作调试器')
self._act_debug_action.triggered.connect(self.on_act_debug)
self.menuBar().addSeparator()
self._state_monitor_action = self.menuBar().addAction('状态监测器')
self._state_monitor_action.triggered.connect(self.on_state_monitor)
self.hostLabel = QLabel(self.host)
self.hostLabel.setText(self.host)
self.statusBar().addWidget(self.hostLabel)
self.mainWidget = QWidget()
self.main_layout = QVBoxLayout()
self.main_splitter = QSplitter(Qt.Horizontal, self)
self.left_splitter = QSplitter(Qt.Vertical, self.main_splitter)
self.middle_splitter = QSplitter(Qt.Vertical, self.main_splitter)
self.right_splitter = QSplitter(Qt.Vertical, self.main_splitter)
self.main_splitter.addWidget(self.left_splitter)
self.main_splitter.addWidget(self.middle_splitter)
self.main_splitter.addWidget(self.right_splitter)
self.boxqss = '''QGroupBox { border: 2px solid black;
border-radius: 5px;
margin-top:1ex; }
QGroupBox::title {
subcontrol-origin: margin;
subcontrol-position: top center;
padding: 0 3px; }'''
self.main_layout.addWidget(self.main_splitter)
self.mainWidget.setLayout(self.main_layout)
self.setCentralWidget(self.mainWidget)
self.action_debug_client = None
self.on_run.connect(self.run_add_angles)
self.disconnected.connect(self.on_disconnected)
self.srvWidget = None
self.graphWidget = None
self.remoteWidget = None
self.logWidget = None
self.imageWidget = None
self.paramsWidget = None
self.on_srv_table_widget()
self.on_topics_widget()
self.on_remote_widget()
self.on_log_widget()
self.on_image_widget()
self.on_params_widget()
def on_recv_action(self, req, res):
self.on_run.emit(req)
return True
def run_add_angles(self, req):
try:
run_service = Service(self.ros_client, '/add_angles',
'common/AddAngles')
run_service.call(ServiceRequest(req))
except Exception as e:
QMessageBox.critical(self, "错误", e.args[0])
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 on_state_monitor(self):
cmd = 'rosrun team_monitor team_monitor '
os.popen(cmd)
def on_lost_connect(self, proto):
self.disconnected.emit()
def on_disconnected(self):
QMessageBox.critical(self, "错误", '连接已断开')
self.close()
def on_srv_table_widget(self, show=None):
if show is None:
groupbox = QGroupBox('Service表')
groupbox.setStyleSheet(self.boxqss)
layout = QVBoxLayout()
self.srvWidget = ServiceTableWidget(self.ros_client)
layout.addWidget(self.srvWidget)
groupbox.setLayout(layout)
groupbox.hide()
self.left_splitter.insertWidget(0, groupbox)
elif show:
self.left_splitter.widget(0).show()
else:
self.left_splitter.widget(0).hide()
self.adjustSize()
def on_topics_widget(self, show=None):
if show is None:
groupbox = QGroupBox('Topic拓扑图')
groupbox.setStyleSheet(self.boxqss)
layout = QVBoxLayout()
self.graphWidget = TopicGraphWidget(self.ros_client)
layout.addWidget(self.graphWidget)
groupbox.setLayout(layout)
groupbox.hide()
self.left_splitter.insertWidget(1, groupbox)
elif show:
self.left_splitter.widget(1).show()
else:
self.left_splitter.widget(1).hide()
self.adjustSize()
def on_remote_widget(self, show=None):
if show is None:
groupbox = QGroupBox('遥控')
groupbox.setStyleSheet(self.boxqss)
layout = QVBoxLayout()
self.remoteWidget = RemoteWidget(self.ros_client)
layout.addWidget(self.remoteWidget)
groupbox.setLayout(layout)
groupbox.hide()
self.middle_splitter.insertWidget(0, groupbox)
elif show:
self.middle_splitter.widget(0).show()
else:
self.middle_splitter.widget(0).hide()
self.adjustSize()
def on_log_widget(self, show=None):
if show is None:
groupbox = QGroupBox('运行日志')
groupbox.setStyleSheet(self.boxqss)
layout = QVBoxLayout()
self.logWidget = LogWidget(self.ros_client)
layout.addWidget(self.logWidget)
groupbox.setLayout(layout)
groupbox.hide()
self.middle_splitter.insertWidget(1, groupbox)
elif show:
self.middle_splitter.widget(1).show()
else:
self.middle_splitter.widget(1).hide()
self.adjustSize()
def on_image_widget(self, show=None):
if show is None:
groupbox = QGroupBox('在线图像')
groupbox.setStyleSheet(self.boxqss)
layout = QVBoxLayout()
self.imageWidget = ImageWidget(self.ros_client)
layout.addWidget(self.imageWidget)
groupbox.setLayout(layout)
groupbox.hide()
self.right_splitter.insertWidget(0, groupbox)
elif show:
self.right_splitter.widget(0).show()
else:
self.right_splitter.widget(0).hide()
self.adjustSize()
def on_params_widget(self, show=None):
if show is None:
groupbox = QGroupBox('参数')
groupbox.setStyleSheet(self.boxqss)
layout = QVBoxLayout()
self.paramsWidget = ParamsWidget(self.ros_client)
layout.addWidget(self.paramsWidget)
groupbox.setLayout(layout)
groupbox.hide()
self.right_splitter.insertWidget(1, groupbox)
elif show:
self.right_splitter.widget(1).show()
else:
self.right_splitter.widget(1).hide()
self.adjustSize()
def closeEvent(self, a0: QtGui.QCloseEvent) -> None:
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):
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)
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()
try:
msg=LaserScan()
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.angle_min=message['angle_min']
msg.angle_max=message['angle_max']
msg.angle_increment=message['angle_increment']
msg.time_increment=message['time_increment']
msg.scan_time=message['scan_time']
msg.range_min=message['range_min']
msg.range_max=message['range_max']
ranges_list=[]
intensities_list=[]
for i in range(len(message['ranges'])):
ranges_list.append(float)
if message['ranges'][i]==None:
ranges_list[i]=float('NaN')
else:
ranges_list[i]=message['ranges'][i]
for i in range(len(message['intensities'])):
intensities_list.append(float)
if message['intensities'][i]==None:
intensities_list[i]=float('NaN')
else:
intensities_list[i]=message['intensities'][i]
msg.ranges=ranges_list
msg.intensities=intensities_list
self._rosPub=rospy.Publisher(self._local_topic_name, LaserScan, queue_size=10)
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()
