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()
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._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=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)
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._file = file
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)
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()
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']
self._rosPub = rospy.Publisher(self._local_topic_name,
OccupancyGrid,
queue_size=10)
self._rosPub.publish(msg)
time.sleep(3)
self.start()
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()
