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