备忘 - Sim联合Python仿真接口

Note - Sim联合Python仿真接口

备忘自用

配置

# just in case, close all opened connections
sim.simxFinish(-1)
clientID=sim.simxStart('127.0.0.1',19997,True,True,5000,5)
res=sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot)
sim.simxSynchronous(clientID,1)

# 获得左电机Sim对象
res, motorLeftHandle = sim.simxGetObjectHandle(clientID, 'DynamicLeftJoint', sim.simx_opmode_oneshot_wait)
# 获得右侧电机Sim对象
res, motorRightHandle = sim.simxGetObjectHandle(clientID, 'DynamicRightJoint', sim.simx_opmode_oneshot_wait)

# 获得摄像头0 Sim对象
err_code,roadVisionHandle0 = sim.simxGetObjectHandle(clientID,"MiddleUpSensor", sim.simx_opmode_blocking)
# 另外一个远处的摄像头
#err_code,roadVisionHandle1 = sim.simxGetObjectHandle(clientID,"road_sensor1", sim.simx_opmode_blocking)

# 获得摄像头0图像，数据流模式传输数据
_, resolution, image= sim.simxGetVisionSensorImage(clientID,roadVisionHandle0,0,sim.simx_opmode_streaming)
#_, resolution, far_image= sim.simxGetVisionSensorImage(clientID,roadVisionHandle1,0,sim.simx_opmode_streaming)

# 设置左右轮速度为0， 单触发非阻塞模式传输数据
sim.simxSetJointTargetVelocity(clientID,motorLeftHandle,0,sim.simx_opmode_oneshot)
sim.simxSetJointTargetVelocity(clientID,motorRightHandle,0,sim.simx_opmode_oneshot)

sim.simxSynchronousTrigger(clientID)

Sim对象树

图传

def getImage(sensor_handle):
    # 通过最新的数据流信息，获得当前传感器图片
    # simx_opmode_buffer: A previous reply to the same command is returned (if available). The command is not send, nor does the function wait for the actual reply.
    # 可以参考：https://www.coppeliarobotics.com/helpFiles/en/remoteApiModusOperandi.htm
    _, resolution, image= sim.simxGetVisionSensorImage(clientID, sensor_handle, 0, sim.simx_opmode_buffer)
    # 处理图片
    image = np.array(image,dtype=np.uint8)
    image.resize([resolution[1],resolution[0],3])
    image = cv2.flip(image, 1)
    crop_img = image[110:255, 0:255] 
    # 灰度化
    gray = cv2.cvtColor(crop_img, cv2.COLOR_BGR2GRAY)
    plt.imshow(gray,cmap='gray')
    return gray

# 获得传感器图片
img=getImage(roadVisionHandle0)
with open('temp.npy', 'wb') as f:
    np.save(f, img)
plt.imshow(img,cmap='gray')

驱动

def driveCar(centerX, angle):
    desiredLeftWheelRotSpeed = SPEED_LEFT
    desiredRightWheelRotSpeed = SPEED_RIGHT
    # 设置速度
    sim.simxSetJointTargetVelocity(clientID,motorLeftHandle,desiredLeftWheelRotSpeed,sim.simx_opmode_oneshot)
    sim.simxSetJointTargetVelocity(clientID,motorRightHandle,desiredRightWheelRotSpeed,sim.simx_opmode_oneshot)
    
    sim.simxSynchronousTrigger(clientID)