【Coppeliasim】 通过TCP与coppeliasim通信

仿真客户端，  代码中启动了tcp服务器。

sim=require'sim'
socket=require'socket'

-- 以下函数将数据写入套接字（仅为简单起见只处理单个数据包）：
writeSocketData=function(client,data)
    local header=string.char(59,57,math.mod(#data,256),math.floor(#data/256),0,0)
    -- 数据包头是（在这种情况下）：headerID（59,57），数据大小（WORD），剩余包数（WORD）但在这里未使用
    client:send(header..data)
end

-- 以下函数从套接字读取数据（仅为简单起见只处理单个数据包）：
readSocketData=function(client)
    -- 数据包头是：headerID（59,57），数据大小（WORD），剩余包数（WORD）但在这里未使用
    local header=client:receive(6)
    if (header==nil) then
        return(nil) -- 错误
    end
    if (header:byte(1)==59)and(header:byte(2)==57) then
        local l=header:byte(3)+header:byte(4)*256
        return(client:receive(l))
    else
        return(nil) -- 错误
    end
end

-- 感知系统调用
function sysCall_sensing() 
    local p=sim.getObjectPosition(robot)
    sim.addDrawingObjectItem(drawingCont,p)
end 

-- 清理系统调用
function sysCall_cleanup()
    if client then
        client:close()
    end
end

-- 线程系统调用
function sysCall_thread()
    -- 初始化：
    robot=sim.getObject('.')
    drawingCont=sim.addDrawingObject(sim.drawing_linestrip+sim.drawing_cyclic,2,0,-1,400,{1,1,0},nil,nil,{1,1,0})
    sim.setStepping(true) -- 我们希望手动切换以同步（并且不浪费处理时间！）
    -- 获取一些句柄：
    local leftMotor=sim.getObject("./LeftMotor") -- 左电机的句柄
    local rightMotor=sim.getObject("./RightMotor") -- 右电机的句柄
    local noseSensor=sim.getObject("./SensingNose") -- 接近传感器的句柄

    -- 在可能未被使用的端口上启动服务器（尽量使用相似的代码）：
    local portNb=sim.getInt32Param(sim.intparam_server_port_next)
    local portStart=sim.getInt32Param(sim.intparam_server_port_start)
    local portRange=sim.getInt32Param(sim.intparam_server_port_range)
    local newPortNb=portNb+1
    if (newPortNb>=portStart+portRange) then
        newPortNb=portStart
    end
    sim.setInt32Param(sim.intparam_server_port_next,newPortNb)

    local result=sim.launchExecutable('bubbleRobServer',portNb,0) -- 将最后一个参数设置为1以查看启动服务器的控制台

    if (result==-1) then
        -- 无法启动可执行文件！
        sim.addLog(sim.verbosity_scripterrors,"'bubbleRobServer' 无法启动。模拟将无法正常运行")
    else
        sim.wait(1,false)
        -- 可执行文件可以启动。现在构建一个套接字并连接到服务器：
        client=socket.tcp()
        local result=client:connect('127.0.0.1',portNb)

        if (result==1) then
            -- 我们可以连接到服务器
            while (true) do
                -- 发送接近传感器和模拟时间到bubbleRob服务器应用程序：
                -- 准备要发送的数据：
                local dataOut={0,sim.getSimulationTime()}
                local result,dist=sim.readProximitySensor(noseSensor)
                if (result>0) then
                    dataOut[1]=dist
                end
                -- 将数据打包成字符串：
                dataOut=sim.packFloatTable(dataOut)
                -- 发送数据：
                writeSocketData(client,dataOut)
                -- 从服务器读取回复：
                local returnData=readSocketData(client)
                if (returnData==nil) then
                    break -- 读取错误
                end
                -- 解包接收到的数据：
                returnData=sim.unpackFloatTable(returnData)
                -- 将值应用为电机速度：
                sim.setJointTargetVelocity(leftMotor,returnData[1])
                sim.setJointTargetVelocity(rightMotor,returnData[2])

                -- 如果模拟时间未改变，请不要在这个循环中浪费时间！这也与主脚本同步
                sim.step() -- 这个协程将在下一个模拟步骤中恢复
            end
        end
    end
end