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];