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 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 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()
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 __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
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()
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)
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()
import time from roslibpy import Topic from compas_fab.backends import RosClient def receive_message(message): print('Heard talking: ' + message['data']) with RosClient() as client: listener = Topic(client, '/chatter', 'std_msgs/String') listener.subscribe(receive_message) while client.is_connected: time.sleep(1)
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 Muse_app: 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 on_connect(self, client, userdata, flags, rc): print("Connected with result code "+str(rc)) # Subscribing in on_connect() means that if we lose the connection and # reconnect then subscriptions will be renewed. self.client.subscribe("topic/" + self.muse_left) #self.client.subscribe("topic/test") def on_connect2(self, client, userdata, flags, rc): print("Connected with result code "+str(rc)) # Subscribing in on_connect() means that if we lose the connection and # reconnect then subscriptions will be renewed. self.client2.subscribe("topic/" + self.muse_right) def worker(self): self.client.loop_forever() return def worker2(self): self.client2.loop_forever() return def on_message(self, client, userdata, message): print("message received " ,str(message.payload.decode("utf-8"))) print("message topic=",message.topic) if message.topic == "topic/" + self.muse_left: if(message.payload.decode("utf-8") == '0'): self.status_left = False elif(message.payload.decode("utf-8") == '1'): self.status_left = True print("left = " , self.status_left) if message.topic == "topic/" + self.muse_right: if(message.payload.decode("utf-8") == '0'): self.status_right = False elif(message.payload.decode("utf-8") == '1'): self.status_right = True print("right = " , self.status_right) # print("message qos=",message.qos) # print("message retain flag=",message.retain) def subscribing(self): print("subscribe to Ros..") self.publisher.subscribe(self.receive_message) def uninitialize(self): # nop return def receive_message(self, message): # context['counter'] += 1 print('Receive data from Ros, ' , message['data']) #assert message['data'] == 'hello world', 'Unexpected message content' if(message['data'] == "action"): self.client.publish("topic/parameters", 'action') time.sleep(3) print("lastest status from left = " , self.status_left) print("lastest status from right = " , self.status_right) if self.status_left and not self.status_right: ##======================TURN LEFT self.action_mode = 1 elif not self.status_left and self.status_right: ##======================TURN RIGHT self.action_mode = 2 elif not self.status_left and not self.status_right: ##======================MOVE FORWARD self.action_mode = 3 elif self.status_left and self.status_right: ##======================MOVE BACKWARD self.action_mode = 4 print("summary action mode = ", str(self.action_mode)) self.client.publish("topic/parameters", 'wait') isConnected = self.ros_client.is_connected print("isConnected = ", isConnected) if(isConnected): self.publisher.publish(Message({'data': str(self.action_mode)})) def worker3(self): self.ros_client.run_forever()
class DirectUrActionClient(EventEmitterMixin): """Direct UR Script action client to simulate an action interface on arbitrary URScript commands. """ 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' def _on_status_message(self, message): self._received_status = True self.goal.emit('status', message) twist = message['twist'] total_velocity = math.fsum(list(twist['linear'].values()) + list(twist['angular'].values())) in_motion = total_velocity != 0.0 if self._internal_state == 'idle': if in_motion: self._internal_state = 'executing_goal' elif self._internal_state == 'executing_goal': if not in_motion: self._internal_state = 'stopped' self.goal.emit('result', message) def _trigger_timeout(self): if not self._received_status: self.emit('timeout') def add_goal(self, goal): """Add a goal to this action client. Args: goal (:class:`.Goal`): Goal to add. """ self.goal = goal def cancel(self): """Cancel all goals associated with this action client.""" pass # self.cancel_topic.publish(Message()) def dispose(self): """Unsubscribe and unadvertise all topics associated with this action client.""" # And the UR Script topic self._urscript_topic.unadvertise() if not self.omit_status: self.status_listener.unsubscribe(self._on_status_message) def send_goal(self, goal, result_callback=None, timeout=None): """Send goal to the action server. Args: goal (:class:`URGoal`): The goal containing the script lines timeout (:obj:`int`): Timeout for the goal's result expressed in milliseconds. callback (:obj:`callable`): Function to be called when a result is received. It is a shorthand for hooking on the ``result`` event. """ self._internal_state == 'idle' if result_callback: goal.on('result', result_callback) ur_script = goal.goal_message['goal']['script'] message = Message({'data': ur_script}) self._urscript_topic.publish(message) if timeout: self.ros.call_later( timeout / 1000., goal._trigger_timeout)
class ImageWidget(QWidget): on_image = pyqtSignal(str) 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 def check_connect(self): if not self.ros_client.is_connected: QMessageBox.critical(self, "错误", '连接中断,请重新连接') return False return True def subscribe_image(self): if not self.check_connect(): return self._subscribe = not self._subscribe if self._subscribe: self._image_topic.subscribe(self.recv_image) self._subscribe_btn.setText('取消订阅') t = self.ros_client.get_param('image') if t is not None: self._type_combo_box.setCurrentIndex(t) else: self._image_topic.unsubscribe() self._subscribe_btn.setText('订阅图像') def recv_image(self, msg): base64_bytes = msg['data'].encode('ascii') image_bytes = base64.b64decode(base64_bytes) fmt = msg['format'] seq = msg['header']['seq'] name = IMAGE_SAVE_DIR + 'temp.{}'.format(fmt) if self._auto_save or self._save: if seq - self._last_seq > 100 or self._save: self._save = False name = IMAGE_SAVE_DIR + datetime.datetime.now().strftime( '%Y%m%d%H%M%S') + '.' + fmt self._last_seq = seq with open(name, 'wb') as image_file: image_file.write(image_bytes) self.on_image.emit(name) def on_recv_image(self, name): self._image_label.set_image(name) def on_auto_save_check(self, val): self._auto_save = val def on_save_clicked(self): self._save = True def on_shot(self, rect: QRect): if not self.check_connect(): return try: service = Service(self.ros_client, '/debug/image/snap', 'common/ImageSnap') result = service.call( ServiceRequest({ 'type': self._func_combo_box.currentIndex(), 'info': { 'x': rect.topLeft().x(), 'y': rect.topLeft().y(), 'w': rect.width(), 'h': rect.height() } })) except Exception as e: QMessageBox.critical(self, "错误", e.args[0]) def on_type_changed(self, idx): if not self.check_connect(): return try: service = Service(self.ros_client, '/setting/sendtype', 'common/SetInt') service.call(ServiceRequest({'number': idx})) except Exception as e: QMessageBox.critical(self, "错误", e.args[0])
class PepperRLExplorationEnv(habitat.RLEnv): def __init__(self, config: Config, dataset: Optional[Dataset] = None): # Initialize ROS Bridge self._pepper_config = config.PEPPER self._ros = roslibpy.Ros(host='localhost', port=9090) self._ros.run() assert self._ros.is_connected, "ROS not connected" sim_config = config.TASK_CONFIG.SIMULATOR self._image_width = sim_config.RGB_SENSOR.WIDTH self._image_height = sim_config.RGB_SENSOR.HEIGHT self._norm_depth = sim_config.DEPTH_SENSOR.NORMALIZE_DEPTH self.goal_sensor_uuid = config.TASK_CONFIG.TASK.GOAL_SENSOR_UUID print(self.goal_sensor_uuid) self._goal_sensor_dim = config.TASK_CONFIG.TASK.\ POINTGOAL_WITH_GPS_COMPASS_SENSOR.DIMENSIONALITY self._buffer_size = self._pepper_config.BufferSize self._forward_step = self._pepper_config.ForwardStep self._turn_step = self._pepper_config.TurnStep self._sonar_buffer = [] self._depth_buffer = [] self._rgb_buffer = [] self._pose_buffer = [] self._goal_buffer = [] self._odom_buffer = [] self._collected_positions = set() # Subscribe to RGB and Depth topics # RGBTopic: '/pepper_robot/naoqi_driver/camera/front/image_raw' # DepthTopic: '/pepper_robot/naoqi_driver/camera/depth/image_raw' # MoveTopic: '/move_base_simple/goal' self._listener_rgb = Topic(self._ros, self._pepper_config.RGBTopic, 'sensor_msgs/Image') self._listener_depth = Topic(self._ros, self._pepper_config.DepthTopic, 'sensor_msgs/Image') self._publisher_move = Topic(self._ros, self._pepper_config.MoveTopic, 'geometry_msgs/PoseStamped') self._listener_pose = Topic(self._ros, self._pepper_config.PoseTopic, 'geometry_msgs/PoseStamped') self._listener_odom = Topic(self._ros, self._pepper_config.OdomTopic, 'nav_msgs/Odometry') self._listener_goal = Topic(self._ros, self._pepper_config.GoalTopic, 'geometry_msgs/PoseStamped') self._listener_sonar = Topic(self._ros, self._pepper_config.SonarTopic, 'sensor_msgs/Range') self._listener_rgb.subscribe(lambda message: self._fetch_rgb(message)) self._listener_depth.subscribe( lambda message: self._fetch_depth(message)) self._listener_pose.subscribe( lambda message: self._fetch_pose(message)) self._listener_odom.subscribe( lambda message: self._fetch_odom(message)) self._listener_goal.subscribe( lambda message: self._fetch_goal(message)) self._listener_sonar.subscribe( lambda message: self._fetch_sonar(message)) self._fresh_sonar = False self._fresh_pose = False self._fresh_rgb = False self._fresh_depth = False self._fresh_odom = False self.init_obs_space() self.init_action_space() #super().__init__(self._core_env_config, dataset) def init_action_space(self): self.action_space = spaces.Discrete(3) def init_obs_space(self): self.observation_space = Namespace() rgb_space = spaces.Box( low=0, high=255, shape=(self._image_height, self._image_width, 3), dtype=np.uint8, ) if self._norm_depth: min_depth = 0 max_depth = 1 else: min_depth = 0 max_depth = 255 depth_space = spaces.Box( low=min_depth, high=max_depth, shape=(self._image_height, self._image_width, 1), dtype=np.float, ) pointgoal_with_gps_compass_space = spaces.Box( low=np.finfo(np.float32).min, high=np.finfo(np.float32).max, shape=(self._goal_sensor_dim, ), dtype=np.float32, ) self.observation_space.spaces = { "rgb": rgb_space, "depth": depth_space, self.goal_sensor_uuid: pointgoal_with_gps_compass_space } @property def habitat_env(self) -> Env: return None @property def episodes(self) -> List[Type[Episode]]: return [] @property def current_episode(self) -> Type[Episode]: ep = Namespace() ep.episode_id = 0 ep.scene_id = 0 return ep def seed(self, seed: Optional[int] = None) -> None: pass def render(self, mode: str = "rgb") -> np.ndarray: pass def close(self) -> None: self._ros.close() def _fetch_goal(self, message): self._goal_buffer.append(message) def _fetch_odom(self, message): self._odom_buffer.append(message) self._fresh_odom = True if self._buffer_size != -1: if len(self._odom_buffer) > self._buffer_size: self._odom_buffer.pop(0) def _fetch_pose(self, message): import time self._pose_buffer.append(message) self._fresh_pose = True if self._buffer_size != -1: if len(self._pose_buffer) > self._buffer_size: self._pose_buffer.pop(0) def _fetch_sonar(self, message): self._sonar_buffer.append(message) self._fresh_sonar = True if self._buffer_size != -1: if len(self._sonar_buffer) > self._buffer_size: self._sonar_buffer.pop(0) def _fetch_rgb(self, message): img = np.frombuffer(base64.b64decode(message['data']), np.uint8) img = img.reshape((message['height'], message['width'], 3)) img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR) img = cv2.resize(img, (self._image_width, self._image_height)) self._rgb_buffer.append(img) self._fresh_rgb = True if self._buffer_size != -1: if len(self._rgb_buffer) > self._buffer_size: self._rgb_buffer.pop(0) def _fetch_depth(self, message): img = np.frombuffer(base64.b64decode(message['data']), np.uint16) img = img.reshape((message['height'], message['width'], 1)) img = (img.astype(np.float) / 9100).astype(np.float) img = img.clip(0.0, 1.0) img = cv2.resize(img, (self._image_width, self._image_height)) img = img.reshape((self._image_height, self._image_width, 1)) self._depth_buffer.append(img) self._fresh_depth = True if self._buffer_size != -1: if len(self._depth_buffer) > self._buffer_size: self._depth_buffer.pop(0) def _wait_move_done(self): import time time.sleep(4) def _send_command(self, action): action = action['action'] if action == 0: print("Action:", "Forward", self._forward_step, self._turn_step) m = _get_movement_ros_message(self._forward_step, 0) #self._publisher_move.publish(m) elif action == 1: print("Action:", "Left", 0, self._turn_step) m = _get_movement_ros_message(0, self._turn_step) #self._publisher_move.publish(m) elif action == 2: print("Action:", "Right", 0, self._turn_step) m = _get_movement_ros_message(0, -1 * self._turn_step) #self._publisher_move.publish(m) #self._wait_move_done() def wait_all_fresh(self): print(self._fresh_depth, self._fresh_rgb, self._fresh_pose, self._fresh_sonar, self._fresh_odom) return self._fresh_depth and self._fresh_rgb and self._fresh_pose \ and self._fresh_sonar and self._fresh_odom def get_obs(self): while not self.wait_all_fresh(): print("Did not receive new obs.") time.sleep(0.5) rgb = self._rgb_buffer[-1] self._fresh_rgb = False depth = self._depth_buffer[-1] self._fresh_depth = False if self._pepper_config.DisplayImages: cv2.imshow("RGB", rgb) cv2.imshow("Depth", depth) cv2.waitKey(1) robot_position, robot_rotation = self.get_position() goal_position = self.get_goal() dist = np.linalg.norm(robot_position - goal_position) inc_y = goal_position[1] - robot_position[1] inc_x = goal_position[0] - robot_position[0] angle_between_robot_and_goal = math.atan2(inc_y, inc_x) angle = robot_rotation[0] - angle_between_robot_and_goal angle = -1 * angle print(dist, angle) return { "rgb": rgb, "depth": depth, "robot_position": robot_position, "robot_rotation": robot_rotation, "sonar": self.get_sonar(), "odom": self.get_odom(), "gps_with_pointgoal_compass": [dist, angle] } def reset(self): self._previous_action = None self._collected_positions = set() self._depth_buffer = [] self._rgb_buffer = [] observations = self.get_obs() reward = self.get_reward(observations) done = self.get_done(observations) info = self.get_info(observations) return observations, reward, done, info def step(self, *args, **kwargs): self._previous_action = kwargs["action"] self._send_command(self._previous_action) observations = self.get_obs() reward = self.get_reward(observations) done = self.get_done(observations) info = self.get_info(observations) return observations, reward, done, info def get_reward_range(self): return ( 0, 1, ) def get_goal(self): if len(self._goal_buffer) == 0: return np.array([0, 0, 0]) else: position = self._goal_buffer[-1]['pose']['position'] c_pos = np.array([position['x'], position['y'], position['z']]) return c_pos def get_sonar(self): self._fresh_sonar = False sonar = self._sonar_buffer[-1]['range'] return sonar def get_odom(self): print(self._odom_buffer[-1]) position = self._odom_buffer[-1]['pose']['pose']['position'] rotation = self._odom_buffer[-1]['pose']['pose']['orientation'] c_pos = np.array([position['x'], position['y'], position['z']]) c_rot = np.array( quaternion_to_euler(rotation['x'], rotation['y'], rotation['z'], rotation['w'])) self._fresh_pose = False return c_pos, c_rot def get_position(self): position = self._pose_buffer[-1]['pose']['position'] rotation = self._pose_buffer[-1]['pose']['orientation'] c_pos = np.array([position['x'], position['y'], position['z']]) c_rot = np.array( quaternion_to_euler(rotation['x'], rotation['y'], rotation['z'], rotation['w'])) self._fresh_pose = False return c_pos, c_rot def get_reward(self, observations): #(x, y, z), (x, y, z, w) = self.get_position() reward = 0 return reward def _episode_success(self): return False def get_done(self, observations): done = False # if self._env.episode_over or self._episode_success(): # done = True return done def get_info(self, observations): map = np.random.rand(32, 32, 3) info = { "top_down_map": { "map": np.array([[0, 0, 0]]), "valid_map": map, "explored_map": map, "ful_fog_of_war_mask": map, "fog_of_war_mask": None, "agent_map_coord": np.array([0, 0, 0]), "agent_angle": 0 } } return info
import time from roslibpy import Topic from compas_fab.backends import RosClient client = RosClient() client.run() topic = Topic(client, '/messages', 'std_msgs/String') topic.subscribe(print) while True: time.sleep(1) client.terminate()
class MyOVBox(OVBox): 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 } # ros_client.on_ready(start_receiving, run_in_thread=True) def initialize(self): self.counter = 0 t = threading.Thread(target=self.worker) t.start() s = threading.Thread(target=self.workerMQTT) s.start() # self.ros_client.run_forever() return def on_connect(self, client, userdata, flags, rc): print("Connected with result code " + str(rc)) # Subscribing in on_connect() means that if we lose the connection and # reconnect then subscriptions will be renewed. self.client.subscribe("topic/parameters") # The callback for when a PUBLISH message is received from the server. def on_message(self, client, userdata, msg): #print(msg.topic+" "+str(msg.payload)) parameters = msg.payload print("parameters = ", parameters) self.parameters = json.loads(parameters) self.phase = int(self.parameters.get('phase')) print("phase = ", self.phase) def worker(self): self.ros_client.run_forever() def workerMQTT(self): self.client = mqtt.Client() self.client.on_connect = self.on_connect self.client.on_message = self.on_message self.client.connect("localhost", 1883, 60) # Blocking call that processes network traffic, dispatches callbacks and # handles reconnecting. # Other loop*() functions are available that give a threaded interface and a # manual interface. self.client.loop_forever() def process(self): self.counter += 1 for chunkIndex in range(len(self.input[0])): if (type(self.input[0][chunkIndex]) == OVSignalHeader): self.signalHeader = self.input[0].pop() outputHeader = OVSignalHeader( self.signalHeader.startTime, self.signalHeader.endTime, [1, self.signalHeader.dimensionSizes[1]], ['Mean'] + self.signalHeader.dimensionSizes[1] * [''], self.signalHeader.samplingRate) self.output[0].append(outputHeader) elif (type(self.input[0][chunkIndex]) == OVSignalBuffer): #Check if Condition != 0 , Meaning user has been clicked start scenario #if(self.phase != 0): chunk = self.input[0].pop() numpyBuffer = np.array(chunk).reshape( tuple(self.signalHeader.dimensionSizes)) #with open("C:/Users/h4wk/Desktop/Temp/foo.csv",'ab') as f: #path = os.getcwd() + datetime.datetime.now().strftime("%B %d, %Y") + ".csv" # if(self.phase != 0): path = self.setting[ 'OutputPath'] + '/BCI_GTEC_' + datetime.datetime.now( ).strftime("%Y%m%d_" + self.setting['subject_no']) + ".csv" with open(path, 'ab') as f: #t = time.localtime() #print("Current path:" + path) x = datetime.datetime.now() #a = np.asarray([[time.mktime(x.timetuple()), #Header Pattern: Timestamp, phase, scenario, subjectno, gender, age, type, count a = np.asarray(int(round(time.time() * 1000)), dtype=object) # print(a) # np.savetxt(f, a, delimiter=",", fmt='%5s') for channel in numpyBuffer: channel = np.insert(channel, 0, a, axis=0) # print(channel.shape) np.savetxt(f, [channel], delimiter=",") #==================Sending to ROS Server==========================# #print(self.counter) # if(self.counter >= 20): # self.counter = 0 # #self.ros_client.on_ready(self.publisher.publish(Message({'data': numpyBuffer}))) # isConnected = self.ros_client.is_connected # #print("isConnected = ", isConnected) # if(isConnected): # print("sending data... ", numpyBuffer.tolist()) # #self.publisher.publish(Message({'data': numpyBuffer.tolist()})) # a = " ".join(str(i) for i in numpyBuffer.tolist()) # self.publisher.publish(Message({'data': a})) # #self.publisher.publish(Message({'data': 'hello test'})) # #self.publisher.unadvertise() # ==================End Sending to ROS Server==========================# numpyBuffer = numpyBuffer.mean(axis=0) chunk = OVSignalBuffer(chunk.startTime, chunk.endTime, numpyBuffer.tolist()) self.output[0].append(chunk) elif (type(self.input[0][chunkIndex]) == OVSignalEnd): self.output[0].append(self.input[0].pop()) def subscribing(self): self.publisher.subscribe(receive_message) def uninitialize(self): # nop return
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._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()