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
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(req):
print('Received request "%s"...' % req)
rep = 'hi'
print('Sending reply "%s"...' % rep)
return rep.encode('utf-8')
node = b0.Node('python-service-server')
srv = b0.ServiceServer(node, 'control', callback)
node.init()
print('Offering service "%s"...' % srv.get_service_name())
node.spin()
node.cleanup()
# -*- coding: utf-8 -*-
import b0
node = b0.Node('python-service-client')
cli = b0.ServiceClient(node, 'control')
node.init()
print('Using service "%s"...' % cli.get_service_name())
req_str = u'hellò'
print('Sending "%s"...' % req_str)
req = req_str.encode('utf-8')
rep = cli.call(req)
rep_str = rep.decode('utf-8')
print('Received "%s"' % rep_str)
node.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()
#coding:utf-8
import b0
import pyrealsense2 as rs
import numpy as np
import time
pc = rs.pointcloud()
points = rs.points()
pipe = rs.pipeline()
pipe.start()
node = b0.Node('pointcloud-pub')
pub = b0.Publisher(node, 'pc')
node.init()
print('Publishing to topic "%s"...' % pub.get_topic_name())
try:
while not node.shutdown_requested():
frames = pipe.wait_for_frames()
depth = frames.get_depth_frame()
# color = frames.get_color_frame()
# pc.map_to(color)
points = pc.calculate(depth)
pts = [pt for pt in np.asanyarray(points.get_vertices()) if any(pt)]
pub.publish(np.array(pts).tobytes())
# time.sleep(0.1)
# print("sending point cloud")
return qim.copy() if copy else qim
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())
# -*- coding: utf-8 -*-
import b0
import time
if not b0.is_initialized():
b0.init()
node = b0.Node('python-publisher')
pub = b0.Publisher(node, 'A')
node.init()
print('Publishing to topic "%s"...' % pub.get_topic_name())
i = 0
while not node.shutdown_requested():
msg_str = u'µsg-%d' % i
i += 1
print('Sending message "%s"...' % msg_str)
msg = msg_str.encode('utf-8')
pub.publish(msg)
node.spin_once()
time.sleep(1)
node.cleanup()
# -*- coding: utf-8 -*-
import b0
import numpy as np
import cv2
import time
cam = cv2.VideoCapture(0)
node = b0.Node('python-camera-node')
camera_pub = b0.Publisher(node, 'camera')
node.init()
while not node.shutdown_requested():
ret_val, img = cam.read()
img = cv2.flip(img, 1)
ok, arr = cv2.imencode('.jpg', img)
if ok:
msg = arr.tostring()
print('Sending message %d' % len(msg))
camera_pub.publish(msg)
node.spin_once()
time.sleep(0.1)
node.cleanup()
cv2.destroyAllWindows()
