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()