def __init__(self,nodeName='b0RemoteApi_pythonClient',channelName='b0RemoteApi',inactivityToleranceInSec=60,setupSubscribersAsynchronously=False):
self._channelName=channelName
self._serviceCallTopic=channelName+'SerX'
self._defaultPublisherTopic=channelName+'SubX'
self._defaultSubscriberTopic=channelName+'PubX'
self._nextDefaultSubscriberHandle=2
self._nextDedicatedPublisherHandle=500
self._nextDedicatedSubscriberHandle=1000
b0.init()
self._node=b0.Node(nodeName)
self._clientId=''.join(random.choice(string.ascii_uppercase+string.ascii_lowercase+string.digits) for _ in range(10))
self._serviceClient=b0.ServiceClient(self._node,self._serviceCallTopic)
self._serviceClient.set_option(3,1000) #read timeout of 1000ms
self._defaultPublisher=b0.Publisher(self._node,self._defaultPublisherTopic)
self._defaultSubscriber=b0.Subscriber(self._node,self._defaultSubscriberTopic,None) # we will poll the socket
print('\n Running B0 Remote API client with channel name ['+channelName+']')
print(' make sure that: 1) the B0 resolver is running')
print(' 2) V-REP is running the B0 Remote API server with the same channel name')
print(' Initializing...\n')
self._node.init()
self._handleFunction('inactivityTolerance',[inactivityToleranceInSec],self._serviceCallTopic)
print('\n Connected!\n')
self._allSubscribers={}
self._allDedicatedPublishers={}
self._setupSubscribersAsynchronously=setupSubscribersAsynchronously
def __init__(self,
nodeName=None,
channelName=None,
inactivityToleranceInSec=60,
setupSubscribersAsynchronously=False,
timeout=3):
if nodeName == None:
nodeName = 'b0RemoteApi_pythonClient' + str(threading.get_ident())
if channelName == None:
channelName = 'b0RemoteApiAddOn'
self._channelName = channelName
self._serviceCallTopic = channelName + 'SerX'
self._defaultPublisherTopic = channelName + 'SubX'
self._defaultSubscriberTopic = channelName + 'PubX'
self._nextDefaultSubscriberHandle = 2
self._nextDedicatedPublisherHandle = 500
# print(f'(nodeName:{{{nodeName}}} channelName:{{{channelName}}})')
self._nextDedicatedSubscriberHandle = 1000
# print('Calling b0.init()')
b0.init()
# print('Calling b0.Node(nodeName)')
self._node = b0.Node(nodeName)
self._clientId = ''.join(
random.choice(string.ascii_uppercase + string.ascii_lowercase +
string.digits) for _ in range(10))
self._serviceClient = b0.ServiceClient(self._node,
self._serviceCallTopic)
self._serviceClient.set_option(3, timeout * 1000) #read timeout
self._defaultPublisher = b0.Publisher(self._node,
self._defaultPublisherTopic)
self._defaultSubscriber = b0.Subscriber(
self._node, self._defaultSubscriberTopic,
None) # we will poll the socket
# print('\n Running B0 Remote API client with channel name ['+channelName+']')
# print(' make sure that: 1) the B0 resolver is running')
# print(' 2) CoppeliaSim is running the B0 Remote API server with the same channel name')
# print(' Initializing...\n')
self._node.init()
self._handleFunction('inactivityTolerance', [inactivityToleranceInSec],
self._serviceCallTopic)
# print('\n Connected!\n')
self._allSubscribers = {}
self._allDedicatedPublishers = {}
self._setupSubscribersAsynchronously = setupSubscribersAsynchronously
def simxCreateSubscriber(self,cb,publishInterval=1,dropMessages=False):
topic=self._channelName+'Pub'+str(self._nextDedicatedSubscriberHandle)+self._clientId
self._nextDedicatedSubscriberHandle=self._nextDedicatedSubscriberHandle+1
sub=b0.Subscriber(self._node,topic,None,0,1)
if dropMessages:
sub.set_option(6,1) #conflate option enabled
else:
sub.set_option(6,0) #conflate option disabled
sub.init()
self._allSubscribers[topic]={}
self._allSubscribers[topic]['handle']=sub
self._allSubscribers[topic]['cb']=cb
self._allSubscribers[topic]['dropMessages']=dropMessages
channel=self._serviceCallTopic
if self._setupSubscribersAsynchronously:
channel=self._defaultPublisherTopic
self._handleFunction('createPublisher',[topic,publishInterval],channel)
return topic
msg.is_dense=True
def callback(ptsbuffer):
time_now=rospy.Time(0)
buff_size=len(ptsbuffer)
# pc=np.frombuffer(ptsbuffer,dtype=np.dtype("f4,f4,f4"))
# msg.data=pc.view(dtype=np.float32).tolist()
msg.header.stamp=time_now
msg.width=buff_size/12
msg.row_step=buff_size
msg.data=ptsbuffer
ros_tf_pub.sendTransform((0,0,1), (0,0,0,1), time_now, "cam", "world")
ros_pub.publish(msg)
node_b0 = b0.Node('pointcloud_sub')
sub_b0 = b0.Subscriber(node_b0, 'pc', callback)
node_b0.init()
rospy.init_node("pointcloud_ros")
print('Subscribed to topic "%s"...' % sub_b0.get_topic_name())
ros_pub = rospy.Publisher("/pointcloud", PointCloud2, queue_size=10)
ros_tf_pub = tf.TransformBroadcaster(queue_size=1)
try:
while not node_b0.shutdown_requested() and not rospy.is_shutdown():
# node_b0.spin()
node_b0.spin_once()
finally:
node_b0.cleanup()
import b0
def callback(msg):
print('Received message "%s"' % msg)
node = b0.Node('python-subscriber')
sub = b0.Subscriber(node, 'A', callback)
node.init()
print('Subscribed to topic "%s"...' % sub.get_topic_name())
node.spin()
node.cleanup()
elif im.shape[2] == 4:
qim = QImage(im.data, im.shape[1], im.shape[0], im.strides[0],
QImage.Format_ARGB32)
return qim.copy() if copy else qim
raise RuntimeError('bad image shape: %s' % im.shape)
def camera_callback(msg):
print('Received message %d' % len(msg))
arr = np.frombuffer(msg, np.uint8)
img = cv2.imdecode(arr, cv2.IMREAD_COLOR)
label.setPixmap(QPixmap.fromImage(toQImage(img).rgbSwapped()))
node = b0.Node('python-camera-gui')
camera_sub = b0.Subscriber(node, 'camera', camera_callback)
#camera_sub.set_option(6, 1) # enable conflate
node.init()
app = QApplication(sys.argv)
window = QMainWindow()
window.resize(320, 200)
window.setWindowTitle('Camera GUI')
label = QLabel()
window.setCentralWidget(label)
window.show()
timer = QTimer()
timer.timeout.connect(lambda: node.spin_once())
timer.start(1000 / 24)
