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