def get_depth(self):
# camera 1
res, resolution, depth1 = vrep.simxGetVisionSensorDepthBuffer(
self.client_id,
self.camera_handles[0],
operationMode=vrep.simx_opmode_blocking)
depth1 = np.array(depth1)
depth1 = np.reshape(depth1, [resolution[1], resolution[0]])[::-1, ...]
# camera 1
res, resolution, depth2 = vrep.simxGetVisionSensorDepthBuffer(
self.client_id,
self.camera_handles[1],
operationMode=vrep.simx_opmode_blocking)
depth2 = np.array(depth2)
depth2 = np.reshape(depth2, [resolution[1], resolution[0]])[::-1, ...]
# camera 1
res, resolution, depth3 = vrep.simxGetVisionSensorDepthBuffer(
self.client_id,
self.camera_handles[2],
operationMode=vrep.simx_opmode_blocking)
depth3 = np.array(depth3)
depth3 = np.reshape(depth3, [resolution[1], resolution[0]])[::-1, ...]
resolution = resolution[::-1]
return resolution, [depth1, depth2, depth3]
def setup_devices():
""" Assign the devices from the simulator to specific IDs """
global robotID, left_motorID, right_motorID, kinect_depthID, collisionID
# res: result (1(OK), -1(error), 0(not called))
# robot
res, robotID = vrep.simxGetObjectHandle(clientID, 'robot#', WAIT)
# people
# motors
res, left_motorID = vrep.simxGetObjectHandle(clientID, 'leftMotor#', WAIT)
res, right_motorID = vrep.simxGetObjectHandle(clientID, 'rightMotor#',
WAIT)
# kinetic
res, kinect_depthID = vrep.simxGetObjectHandle(clientID, 'kinect_depth#',
WAIT)
# if res == vrep.simx_return_ok: # [debug]
# print("vrep.simxGetDistanceHandle executed fine")
# collision object
res, collisionID = vrep.simxGetCollisionHandle(clientID, "Collision#",
BLOCKING)
# start up devices
# wheels
vrep.simxSetJointTargetVelocity(clientID, left_motorID, 0, STREAMING)
vrep.simxSetJointTargetVelocity(clientID, right_motorID, 0, STREAMING)
# pose
vrep.simxGetObjectPosition(clientID, robotID, -1, MODE_INI)
vrep.simxGetObjectOrientation(clientID, robotID, -1, MODE_INI)
# collision
vrep.simxReadCollision(clientID, collisionID, STREAMING)
# kinect
vrep.simxGetVisionSensorDepthBuffer(clientID, kinect_depthID, STREAMING)
return
def get_camera_data(self):
"""
-- Read images data from vrep and convert into np array
"""
# Get color image from simulation
res, resolution, raw_image = vrep.simxGetVisionSensorImage(
self.clientID, self.kinectRGB_handle, 0,
vrep.simx_opmode_oneshot_wait)
self._error_catch(res)
color_img = np.array(raw_image, dtype=np.uint8)
color_img.shape = (resolution[1], resolution[0], 3)
color_img = color_img.astype(np.float) / 255
color_img[color_img < 0] += 1
color_img *= 255
color_img = np.fliplr(color_img)
color_img = color_img.astype(np.uint8)
# Get depth image from simulation
res, resolution, depth_buffer = vrep.simxGetVisionSensorDepthBuffer(
self.clientID, self.kinectDepth_handle,
vrep.simx_opmode_oneshot_wait)
self._error_catch(res)
depth_img = np.array(depth_buffer)
depth_img.shape = (resolution[1], resolution[0])
depth_img = np.fliplr(depth_img)
# zNear = 0.01
# zFar = 10
# depth_img = depth_img * (zFar - zNear) + zNear
depth_img[depth_img < 0] = 0
depth_img[depth_img > 1] = 1
depth_img = depth_img * self.INT16
return depth_img, color_img
def __init__(self, proxy_map):
super(SpecificWorker, self).__init__(proxy_map)
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if clientID != -1:
print('Connected to remote API server')
mode = vrep.simx_opmode_blocking
#res, camhandle = vrep.simxGetObjectHandle(clientID, 'ePuck_lightSensor', vrep.simx_opmode_oneshot_wait)
res, self.camhandle = vrep.simxGetObjectHandle(
clientID, 'camera_2_rgb', vrep.simx_opmode_oneshot_wait)
res, camDhandle = vrep.simxGetObjectHandle(
clientID, 'camera_2_depth', vrep.simx_opmode_oneshot_wait)
res, resolution, image = vrep.simxGetVisionSensorImage(
clientID, self.camhandle, 0, vrep.simx_opmode_oneshot_wait)
resD, resolutionD, depth = vrep.simxGetVisionSensorDepthBuffer(
clientID, camDhandle, vrep.simx_opmode_oneshot_wait)
if self.camhandle < 0:
sys.exit()
#self.camhandle = camhandle
self.camDhandle = camDhandle
self.clientID = clientID
self.timer.timeout.connect(self.compute)
self.Period = 50
self.timer.start(self.Period)
def depth_buffer(self):
"""
Retrieves the depth buffer of a vision sensor.
"""
code, resolution, buffer = v.simxGetVisionSensorDepthBuffer(
self._id, self._handle, self._def_op_mode)
return buffer
def vrep_get_kinect_images(client_id, kinect_rgb_id, kinect_depth_id):
"""Getting rgb and depth images from VREP
Gets one-dimension array and turns it into two(tree)-dimension numpy array
Arguments:
client_id (int): id which allows program connect with VREP API
Returns:
rgb (numpy.array): [resolution[0]xresolution[1]x3] rgb array normalized to [0, 1]
depth (numpy.array): [resolution[0]xresolution[1]] depth array
"""
_, depth_resolution, depth_image = vrep.simxGetVisionSensorDepthBuffer(
client_id, kinect_depth_id, vrep.simx_opmode_oneshot_wait)
_, rgb_resolution, rgb_image = vrep.simxGetVisionSensorImage(
client_id, kinect_rgb_id, 0, vrep.simx_opmode_oneshot_wait)
image_byte_array = bytes(array.array('b', rgb_image))
image_buffer = I.frombuffer("RGB", [rgb_resolution[0], rgb_resolution[1]],
image_byte_array, "raw", "RGB", 0, 1)
rgb = np.asarray(image_buffer)
rgb = cv2.normalize(rgb.astype('float'), None, 0.0, 1.0, cv2.NORM_MINMAX)
depth = np.reshape(depth_image, (depth_resolution[1], depth_resolution[0]))
return depth, rgb
def get_depth_buffer(self, prec=None):
"""Retrieve depth buffer."""
# Retrieve depth buffer from the vision sensor simulated in V-REP
if self._handle < 0:
if self._handle == MISSING_HANDLE:
raise RuntimeError(
"Could not retrieve depth buffer from {}: missing name or "
"handle.".format(self._name))
if self._handle == REMOVED_OBJ_HANDLE:
raise RuntimeError("Could not retrieve depth buffer from {}: "
"object removed.".format(self._name))
client_id = self.client_id
if client_id is None:
raise ConnectionError(
"Could not retrieve depth buffer from {}: not connected to "
"V-REP remote API server.".format(self._name))
res, resolution, buffer = vrep.simxGetVisionSensorDepthBuffer(
client_id, self._handle, vrep.simx_opmode_blocking)
if res != vrep.simx_return_ok:
raise ServerError(
"Could not retrieve depth buffer from {}.".format(self._name))
# If necessary, round depth values
if prec is not None:
buffer = [round(val, prec) for val in buffer]
# Arrange pixels in rows, reversing from bottom up to top down order
width, height = resolution
return [
buffer[p:p + width]
for p in reversed(range(0, width * height, width))
]
def record_kinect_depth(position_array):
resetRobotWithArray()
give_positions(position_array)
err, resolution, kinect = vrep.simxGetVisionSensorDepthBuffer(
client_id, kinect_depth, vrep.simx_opmode_oneshot_wait)
kinect_buffer = []
for i in range(30):
err, resolution, kinect_buff = vrep.simxGetVisionSensorDepthBuffer(
client_id, kinect_depth, vrep.simx_opmode_oneshot_wait)
if len(kinect_buff) != 0:
kinect_buffer.append(kinect_buff)
file_name = create_file_name(position_array)
if len(kinect_buffer) != 0:
write_out(file_name, kinect_buffer)
vrep.simxGetVisionSensorDepthBuffer(client_id, kinect_depth,
vrep.simx_opmode_discontinue)
def get_depth_buffer(self) -> np.ndarray:
ret, res, d = vrep.simxGetVisionSensorDepthBuffer(
self.client_id, self.handle, self.BLOCK)
if ret != 0:
raise ConnectionError(VRepError.create(ret))
else:
d = np.array(d, np.float32).reshape((res[1], res[0]))
d = np.flipud(d) # the depth buffer is upside-down
d = cv2.resize(
d, (256, 256)) # TODO make codebase resolution-agnostic
return res, d
def getDepth(self):
resD, resolutionD, depth = vrep.simxGetVisionSensorDepthBuffer(
self.clientID, self.camDhandle, vrep.simx_opmode_buffer)
imgD = np.array(depth, dtype=np.float32)
imgD.resize([resolutionD[1], resolutionD[0], 1])
imgD = cv2.flip(imgD, 0)
#
dep = TDepth()
dep.image = dep.data
dep.width, dep.height = img.shape[:2]
return dep
def record_kinect_depth(position_array,sample_size):
resetRobotWithArray()
give_positions(position_array)
err, resolution, kinect_buff = vrep.simxGetVisionSensorDepthBuffer(client_id, kinect_depth,
vrep.simx_opmode_oneshot_wait)
kinect_buffer = []
while joints_in_position(position_array) == False:
print("why am i here?")
if (joints_in_position(position_array)):
err, resolution, kinect_buff = vrep.simxGetVisionSensorDepthBuffer(client_id, kinect_depth,
vrep.simx_opmode_oneshot_wait)
err, resolution, kinect_buff = vrep.simxGetVisionSensorDepthBuffer(client_id, kinect_depth,
vrep.simx_opmode_oneshot_wait)
kinect_buffer.append(kinect_buff)
file_name = create_file_name(position_array,sample_size)
if len(kinect_buffer) != 0 :
write_out(file_name, kinect_buffer)
vrep.simxGetVisionSensorDepthBuffer(client_id, kinect_depth, vrep.simx_opmode_discontinue)
def fetchKinect(depthSTR,rgbSTR):
errorCodeKinectRGB,kinectRGB=vrep.simxGetObjectHandle(clientID,rgbSTR,vrep.simx_opmode_oneshot_wait)
errorCodeKinectDepth,kinectDepth=vrep.simxGetObjectHandle(clientID,depthSTR,vrep.simx_opmode_oneshot_wait)
errorHere,resolution,image=vrep.simxGetVisionSensorImage(clientID,kinectRGB,0,vrep.simx_opmode_oneshot_wait)
img,imgArr=Raftaar.ProcessImage(image,resolution)
rgbArr=np.array(imgArr)
errorHere,resol,depth=vrep.simxGetVisionSensorDepthBuffer(clientID,kinectDepth,vrep.simx_opmode_oneshot_wait)
depthArr=np.array(depth)
return depthArr,rgbArr
def Depth2Cloud(depthSTR):
errorCodeKinectDepth, kinectDepth = vrep.simxGetObjectHandle(
clientID, depthSTR, vrep.simx_opmode_oneshot_wait)
errorHere, resol, depth = vrep.simxGetVisionSensorDepthBuffer(
clientID, kinectDepth, vrep.simx_opmode_oneshot_wait)
camXAngleInDegrees = 57
camXResolution = 640
camYResolution = 480
camXHalfAngle = camXAngleInDegrees * 0.5 * math.pi / 180
camYHalfAngle = (camXAngleInDegrees * 0.5 * math.pi /
180) * camYResolution / camXResolution
nearClippingPlane = 0.01
depthAmplitude = 3.5
closest_dis = 100
closest_x_angle = 0
closest_y_angle = 0
#calculate the inner parameters
f_x = -camXResolution / (2 * depthAmplitude *
math.tan(camXAngleInDegrees * math.pi / 180))
f_y = -camXResolution / (2 * depthAmplitude * math.tan(
camXAngleInDegrees * math.pi / 180 * camYResolution / camXResolution))
# print(f_x)
print(len(depth))
u0 = camXResolution / 2
v0 = camYResolution / 2
cloud_graph = []
for i in range(camXResolution):
for j in range(camYResolution):
tmp = []
depth_value = depth[i + (j) * camXResolution]
zCoor = nearClippingPlane + depthAmplitude * depth_value
xCoor = (i - u0) * zCoor / f_x
yCoor = (j - v0) * zCoor / f_y
tmp.append(xCoor)
tmp.append(yCoor)
tmp.append(zCoor)
cloud_graph.append(tmp)
return cloud_graph
def fetchKinect(kinectDepth):
errorHere, resol, depth = vrep.simxGetVisionSensorDepthBuffer(
clientID, kinectDepth, vrep.simx_opmode_oneshot_wait)
# plt.imshow(np.array(depth).reshape([480,640]), cmap = "gray")
# plt.show()
camXAngleInDegrees = 57
camXResolution = 640
camYResolution = 480
camXHalfAngle = camXAngleInDegrees * 0.5 * math.pi / 180
camYHalfAngle = (camXAngleInDegrees * 0.5 * math.pi /
180) * camYResolution / camXResolution
nearClippingPlane = 0.01
depthAmplitude = 3.5
closest_dis = 100
closest_x_angle = 0
closest_y_angle = 0
for i in range(camXResolution):
Xangle = (camXResolution / 2 - i -
0.5) / camXResolution * camXHalfAngle * 2
for j in range(225, camYResolution):
Yangle = (j - camYResolution / 2 +
0.5) / camYResolution * camYHalfAngle * 2
depth_value = depth[i + (j) * camXResolution]
zCoor = nearClippingPlane + depthAmplitude * depth_value
xCoor = math.tan(Xangle) * zCoor
yCoor = math.tan(Yangle) * zCoor
dist = math.sqrt(zCoor * zCoor + xCoor * xCoor + yCoor * yCoor)
if (dist < closest_dis):
closest_dis = dist
closest_x_angle = Xangle
closest_y_angle = Yangle
p = i
q = j
return closest_dis, closest_x_angle, closest_y_angle
def sense(self):
"""
Sense the world and return data
:return: constructed dictionary of the form {DEPTH, {BLOB_COLOR, BLOB_POSITION}, BLOB_SIZE}
"""
out = {}
# retrieve depth data
result, resolution, data = vrep.simxGetVisionSensorDepthBuffer(self._clientID,
self.sensors_handles['kinect_depth'],
self._operation_mode)
if result != vrep.simx_return_ok: # checking the reading result.
exit(result)
# get clean depth data
out['depth'] = self.get_depth(data) # appending the distance depth.
# retrieve vision sensor image
result_vision, resolution, image = vrep.simxGetVisionSensorImage(self._clientID,
self.sensors_handles['kinect_rgb'],
0,
vrep.simx_opmode_blocking)
# retrieve vision sensor filtered image (blob)
result_blob, t0, t1 = vrep.simxReadVisionSensor(self._clientID,
self.sensors_handles['kinect_rgb'],
vrep.simx_opmode_blocking)
# extract blob data
out['vision'] = self.get_vision(resolution, image, t1)
# get load status
out['load'] = self._load
self._term.write("sensed: {}".format(out))
return out
def camera_images(self):
err_rgb, sensorRGB = vrep.simxGetObjectHandle(
self.clientID, 'Vision_sensor_rgb', vrep.simx_opmode_oneshot_wait)
err_depth, sensorDepth = vrep.simxGetObjectHandle(
self.clientID, 'Vision_sensor_depth',
vrep.simx_opmode_oneshot_wait)
err, resolution, rgb_image = vrep.simxGetVisionSensorImage(
self.clientID, sensorRGB, 0, vrep.simx_opmode_oneshot_wait)
rgb_image = np.array(rgb_image)
rgb_image = np.uint8(
rgb_image.reshape([resolution[1], resolution[0], 3]))
err, resolution, depth_image = vrep.simxGetVisionSensorDepthBuffer(
self.clientID, sensorDepth, vrep.simx_opmode_blocking)
err, nearClippingPlane = vrep.simxGetObjectFloatParameter(
self.clientID, sensorDepth,
vrep.sim_visionfloatparam_near_clipping, vrep.simx_opmode_blocking)
err, farClippingPlane = vrep.simxGetObjectFloatParameter(
self.clientID, sensorDepth, vrep.sim_visionfloatparam_far_clipping,
vrep.simx_opmode_blocking)
depth_image = np.array(depth_image)
# print("before scaling ~ max/min")
# print(np.max(depth_image))
# print(np.min(depth_image))
ratio = farClippingPlane - nearClippingPlane
depth_image = nearClippingPlane + (depth_image * ratio)
# print("after scaling ~ max/min")
# print(np.max(depth_image))
# print(np.min(depth_image))
return rgb_image, depth_image
# create motion panning
motion = MotionPanning(grid, robot)
# robot grid token
robotCircleToken = Circle((0, 0), 2, color='green')
robotVisionToken = Polygon([[0, 0]], True, color='red', alpha=0.5)
grid.add_obj(robotCircleToken)
grid.add_obj(robotVisionToken)
# enable sensor streaming
vrep.simxGetObjectPosition(clientID, sensorHandle, stageHandle,
vrep.simx_opmode_streaming)
vrep.simxGetObjectOrientation(clientID, robotHandle, stageHandle,
vrep.simx_opmode_streaming)
vrep.simxGetVisionSensorDepthBuffer(clientID, sensorHandle,
vrep.simx_opmode_streaming)
try:
while vrep.simxGetConnectionId(clientID) != -1 and not grid.finish:
# read sensor position
returnCode, position = vrep.simxGetObjectPosition(
clientID, sensorHandle, stageHandle, vrep.simx_opmode_buffer)
if returnCode == vrep.simx_error_noerror:
(x, y, _) = position
# adjust by stage size
x, y = (x - stageMinX) / stageWidth, (y - stageMinY) / stageHeight
robot.update_position((x, y, None))
robotCircleToken.center = grid.position((x, y))
# read robot orientation
returnCode, orientation = vrep.simxGetObjectOrientation(
def get_depth_from_kinect(self):
_, depth_resolution, depth_image = vrep.simxGetVisionSensorDepthBuffer(
self.client_id, self.handles['kinect_depth' + self.postfix], ONE_SHOT_MODE)
depth_arr = np.reshape(depth_image, (480, 640))
return depth_arr.tolist()
train_frames = 200000 # Number of frames to play.
replay = []
loss_log = []
data_collect = []
params = {"batchSize": 200, "buffer": 50000, "nn": nn_param}
batchSize = params['batchSize']
buffer = params['buffer']
model = neural_net(NUM_INPUT, nn_param)
# Initial the speed
errorCode = vrep.simxSetJointTargetVelocity(clientID, motor_handle,
target_speed,
vrep.simx_opmode_streaming)
errorCode, res, img = vrep.simxGetVisionSensorDepthBuffer(
clientID, cam_handle, vrep.simx_opmode_streaming)
im = np.array(img)
im.resize(kinectMapsize[0], kinectMapsize[1]) #Can be adjusted in VREP
#Depth 0~1 to distance 0.2 ~ 3.3
#dis = np.ones(kinectMapsize) * near + im * (far-near)
dis = im * resolution
#Get the readings
readings = []
for i in range(0, NUM_INPUT):
readings.append(dis[kinectHeight][kinectStart + i * kinectInterval])
#errorCode,linearVelocity,angularVelocity=vrep.simxGetObjectVelocity(clientID,cam_handle,vrep.simx_opmode_buffer)
#print(linearVelocity)
weightReadings = [0.5, 0.7, 1, 2, 1, 0.7, 0.5]
err, camera_RGB_handle = vrep.simxGetObjectHandle(
clientID, 's1',
vrep.simx_opmode_oneshot_wait) # запрос на использование камеры
err, camera_depth_handle = vrep.simxGetObjectHandle(
clientID, 's2',
vrep.simx_opmode_oneshot_wait) # запрос на использование камеры глубины
err, laser_handle = vrep.simxGetObjectHandle(clientID, 'laserR',
vrep.simx_opmode_oneshot_wait)
err, VelocityU_handle = vrep.simxGetObjectHandle(clientID, 'active1',
vrep.simx_opmode_oneshot_wait)
err, VelocityD_handle = vrep.simxGetObjectHandle(clientID, 'active2',
vrep.simx_opmode_oneshot_wait)
_, resolution_RGB, image_RGB = vrep.simxGetVisionSensorImage(
clientID, camera_RGB_handle, 0, vrep.simx_opmode_streaming)
_, resolution_Depth, image_depth = vrep.simxGetVisionSensorDepthBuffer(
clientID, camera_depth_handle, vrep.simx_opmode_streaming)
_, _, laser, _, _ = vrep.simxReadProximitySensor(clientID, laser_handle,
vrep.simx_opmode_streaming)
time.sleep(1)
camera_angle = 60 # задается в V-Rep
angle_in_pixel = camera_angle / 256 # делим на количество пикселей
rad_in_pixel = math.radians(angle_in_pixel)
# создаем кватернион тангажа, рысканья, крена
# рысканье
y_x = 0
y_y = 0
y_z = 0
y_w = 0
# тангаж
def fetch_kinect():
res, resolution, depth = vrep.simxGetVisionSensorDepthBuffer(
clientID, kinect_depthID, BUFFER)
depthData = np.array(depth)
return depthData
def getLidarSensorReading(handle):
return vrep.simxGetVisionSensorDepthBuffer(__clientID, handle, vrep.simx_opmode_blocking)
Tip_target = np.zeros(6, dtype='int32')
for i in range(0, 6):
res, Tip_target[i] = vrep.simxGetObjectHandle(
clientID, 'TipTarget' + str(i + 1), vrep.simx_opmode_blocking)
INIT_POSITION = np.zeros((6, 3))
TWO_INIT_POSITION = np.zeros((4, 3))
# 设置全局坐标系
# setGBS()
res, kinect_rgb_resoluton, kinect_rgb_buffer = vrep.simxGetVisionSensorImage(
clientID, kinect_rgb_camera, 0, vrep.simx_opmode_oneshot_wait)
# write_rgb_data(kinect_rgb_buffer)
show_rgb_img(kinect_rgb_buffer)
res, resolution, kinect_depth_buffer = vrep.simxGetVisionSensorDepthBuffer(
clientID, kinect_depth_camera, vrep.simx_opmode_oneshot_wait)
print(kinect_depth_buffer)
print(len(kinect_depth_buffer))
print('begin')
# write_depth_data(kinect_depth_buffer) #write the depth data in the text file
show_depth_img(kinect_depth_buffer)
print('end')
# 初始化位置，把脚的位置置0
recover(n=30)
# 得到初始位置
for i in range(6):
res, INIT_POSITION[i] = vrep.simxGetObjectPosition(
clientID, S1[i], BCS, vrep.simx_opmode_oneshot_wait)
#四足行走状态下的初始状态
vrep.simxSetJointTargetVelocity(clientID, Left_Motor, speed,
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetVelocity(clientID, Right_Motor, speed,
vrep.simx_opmode_oneshot)
#Initialize sensors
errorCode, IsObstacle, DistanceVector_left, ObstacleHandle, SurfaceNormalVector = vrep.simxReadProximitySensor(
clientID, leftsensor, vrep.simx_opmode_streaming)
errorCode, IsObstacle, DistanceVector_front, ObstacleHandle, SurfaceNormalVector = vrep.simxReadProximitySensor(
clientID, frontsensor, vrep.simx_opmode_streaming)
errorCode, detection, auxPackets = vrep.simxReadVisionSensor(
clientID, frontcam, vrep.simx_opmode_streaming)
errorCode, detection, auxPackets = vrep.simxReadVisionSensor(
clientID, rightcam, vrep.simx_opmode_streaming)
returnCode, resolution, buffer = vrep.simxGetVisionSensorDepthBuffer(
clientID, frontcam, vrep.simx_opmode_streaming)
returnCode, resolution, buffer = vrep.simxGetVisionSensorDepthBuffer(
clientID, rightcam, vrep.simx_opmode_streaming)
##############################################################################################################
k = 0
dist_opt = 1
while clientID != -1:
errorCode, IsObstacle_left, left, ObstacleHandle_left, SurfaceNormalVector_left = vrep.simxReadProximitySensor(
clientID, leftsensor, vrep.simx_opmode_buffer)
errorCode, IsObstacle_front, front, ObstacleHandle_front, SurfaceNormalVector_front = vrep.simxReadProximitySensor(
clientID, frontsensor, vrep.simx_opmode_buffer)
if left != [0, 0, 0] and k == 0:
robot_LeftMotorHandle.append(temp)
res = vrep.simxSetJointTargetVelocity(clientID, temp, 0.0,
vrep.simx_opmode_streaming)
#Right
res, temp = vrep.simxGetObjectHandle(clientID,
"RightMotor" + str(i + 1),
vrep.simx_opmode_blocking)
robot_RightMotorHandle.append(temp)
res = vrep.simxSetJointTargetVelocity(clientID, temp, 0.0,
vrep.simx_opmode_streaming)
#Get kinect handle
res, kinectd_h = vrep.simxGetObjectHandle(clientID, "kinect_depth",
vrep.simx_opmode_oneshot_wait)
res, res1, buffer = vrep.simxGetVisionSensorDepthBuffer(
clientID, kinectd_h, vrep.simx_opmode_streaming)
else:
print('Failed connecting to remote API server!!')
#Position the robot
vrep.simxSetObjectPosition(clientID, robot_Handle, -1, [0.0, 0.0, 0.0],
vrep.simx_opmode_oneshot)
vrep.simxSetObjectOrientation(clientID, robot_Handle, -1, [0.0, 0.0, 0.0],
vrep.simx_opmode_oneshot)
time.sleep(4) #So that the motion is completed
#Start the simulation
res = vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
if res == vrep.simx_return_ok:
print("---!!! Started Simulation !!! ---")
rate = rospy.Rate(10) # 10hz
kinect_rgb_pub = rospy.Publisher('/camera/rgb/image_raw',
Image,
queue_size=10)
kinect_depth_pub = rospy.Publisher('/camera/depth/image_raw',
Image,
queue_size=10)
errorCodeKinectRGB, kinectRGB = vrep.simxGetObjectHandle(
clientID, 'kinect_depth', vrep.simx_opmode_oneshot_wait)
errorCodeKinectDepth, kinectDepth = vrep.simxGetObjectHandle(
clientID, 'kinect_rgb', vrep.simx_opmode_oneshot_wait)
errorHere, resolution, image = vrep.simxGetVisionSensorImage(
clientID, kinectRGB, 0, vrep.simx_opmode_streaming)
errorHere, resol, depth = vrep.simxGetVisionSensorDepthBuffer(
clientID, kinectDepth, vrep.simx_opmode_streaming)
time.sleep(1)
errorHere, resolution, image = vrep.simxGetVisionSensorImage(
clientID, kinectRGB, 0, vrep.simx_opmode_buffer)
errorHere, resol, depth = vrep.simxGetVisionSensorDepthBuffer(
clientID, kinectDepth, vrep.simx_opmode_buffer)
while not rospy.is_shutdown():
errorHere, resolution, image = vrep.simxGetVisionSensorImage(
clientID, kinectRGB, 0, vrep.simx_opmode_buffer)
image_byte_array = array.array('b', image)
image_buffer = I.frombuffer("RGB", (resolution[0], resolution[1]),
image_byte_array, "raw", "RGB", 0, 1)
img2 = numpy.asarray(image_buffer)
msg_kinect_rgb = CvBridge().cv2_to_imgmsg(img2, "bgr8")
loss_log = []
data_collect = []
params = {
"batchSize": 200,
"buffer": 50000,
"nn": nn_param
}
batchSize = params['batchSize']
buffer = params['buffer']
model = neural_net(NUM_INPUT, nn_param)
# Initial the speed
errorCode=vrep.simxSetJointTargetVelocity(clientID,motor_handle,target_speed, vrep.simx_opmode_streaming)
errorCode, res, img = vrep.simxGetVisionSensorDepthBuffer(clientID, cam_handle, vrep.simx_opmode_streaming)
im = np.array(img)
im.resize(kinectMapsize[0], kinectMapsize[1]) #Can be adjusted in VREP
#Depth 0~1 to distance 0.2 ~ 3.3
#dis = np.ones(kinectMapsize) * near + im * (far-near)
dis = im * resolution
#Get the readings
readings = []
for i in range(0, NUM_INPUT):
readings.append(dis[kinectHeight][kinectStart + i * kinectInterval])
#errorCode,linearVelocity,angularVelocity=vrep.simxGetObjectVelocity(clientID,cam_handle,vrep.simx_opmode_buffer)
#print(linearVelocity)
weightReadings = [0.5, 0.7, 1, 2, 1, 0.7, 0.5]
def getSensorReading(sensor_name):
return vrep.simxGetVisionSensorDepthBuffer(__clientID, getHandle(sensor_name), vrep.simx_opmode_blocking)
#Connection
vrep.simxFinish(-1)
clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5)
if clientID != -1:
print "Connected to remote API Server"
else:
print "Connection not succesfull"
sys.exit("Could not connect")
#Getting kinet Data
errorCodeKinectDepth,kinectDepth=vrep.simxGetObjectHandle(clientID,'kinect_depth',vrep.simx_opmode_oneshot_wait)
errorCodeKinectDepthVals,resol,depth=vrep.simxGetVisionSensorDepthBuffer(clientID,kinectDepth,vrep.simx_opmode_oneshot_wait)
depthArr=np.array(depth)
print(len(depthArr))
img = np.zeros((resol[1],resol[0],3), np.uint8)
x,y = 0,0
for i in range(0,len(depthArr)):
#print(depthArr[i])
# Create a black image
if x == resol[0]:
x = 0
y+=1
cv2.rectangle(img,(x,y),(x+1,y),(depthArr[i]*255,depthArr[i]*255,depthArr[i]*255),-1)
x+=1
err, nearClip = vrep.simxGetObjectFloatParameter(
clientID, sensorHandle, vrep.sim_visionfloatparam_near_clipping,
vrep.simx_opmode_oneshot_wait)
err, farClip = vrep.simxGetObjectFloatParameter(
clientID, sensorHandle, vrep.sim_visionfloatparam_far_clipping,
vrep.simx_opmode_oneshot_wait)
print("farclip:%s nearclip:%s" % (farClip, nearClip))
#深度相机位置
err, sensorPostion = vrep.simxGetObjectPosition(
clientID, sensorHandle, -1, vrep.simx_opmode_oneshot_wait)
#发送流传送命令
if IMG_TYPE == IMG_RGB:
err, resolution, image = vrep.simxGetVisionSensorImage(
clientID, sensorHandle, 0, vrep.simx_opmode_streaming)
elif IMG_TYPE == IMG_DEPTH:
err, resolution, image = vrep.simxGetVisionSensorDepthBuffer(
clientID, sensorHandle, vrep.simx_opmode_streaming)
else:
err, resolution, image = vrep.simxGetVisionSensorImage(
clientID, sensorHandle, 1, vrep.simx_opmode_streaming)
if err == 0:
print("First Image Get Ok")
time.sleep(2)
#读取视觉传感器的状态,packs={intensity, red, green, blue, depth value}*{min,max,average}
# err, state, packs=vrep.simxReadVisionSensor(clientID,sensorHandle,vrep.simx_opmode_oneshot_wait)
# vrep.simxSetObjectPosition(clientID,sensorHandle,sensorHandle,center)
while (vrep.simxGetConnectionId(clientID) != -1):
FRAME_NUM = FRAME_NUM + 1
if IMG_TYPE == IMG_RGB:
#参数2,options为1时表示灰度图,为0时RGB图,官方教程错误
err, resolution, img = vrep.simxGetVisionSensorImage(
import numpy as np
from array import *
print('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if clientID != -1:
print('Connected to remote API server')
returnCode, cam = vrep.simxGetObjectHandle(clientID, 'cam',
vrep.simx_opmode_oneshot_wait)
returnCode, resolution, image = vrep.simxGetVisionSensorImage(
clientID, cam, 0, vrep.simx_opmode_streaming)
# result,state,packet=vrep.simxGetVisionSensorDepthBuffer(clientID,cam,vrep.simx_opmode_streaming)
result, State, Packet = vrep.simxReadVisionSensor(
clientID, cam, vrep.simx_opmode_streaming)
returnCode1, resolution1, array1 = vrep.simxGetVisionSensorDepthBuffer(
clientID, cam, vrep.simx_opmode_streaming)
i = 1
while (1):
result, state, packet = vrep.simxReadVisionSensor(
clientID, cam, vrep.simx_opmode_buffer)
#returnCode,resolution,array=vrep.simxGetVisionSensorDepthBuffer(clientID,cam,vrep.simx_opmode_buffer)
#print(len(array))
#print(packet[1:])
if packet:
d = packet[0]
bd = packet[1]
ex = packet[2]
#x=bd[4];
#print(d)
#y=bd[5];
