local path = "/root/.arcs_test/end_collision_temp"

function createFolder(path)	
    os.execute("mkdir -p "..path)
end
function moveFolder(file,topath)
    os.execute("mv ".. file .. " " .. topath)
end

function recordTrajData()
    -- createFolder(path)
    startRecord("cal_threshold_data.csv")
end

function serializePayload()
    m,cog,aom,inertia = getPayload()
    
    local separator = "/"
    local params = {
        tostring(m),
        table.concat(cog, ","),
        table.concat(inertia, ",")
    }

    return table.concat(params, separator)
end

function calEndCollisionThreshold()
    stopRecord()

    local str1 = getRobotType()
    local str2 = getRobotSubType()
    local robot_name
    if str2 == "" or str2 == "10" then
        robot_name = str1
    else
        robot_name = str1 .. "_" .. str2
    end
    textmsg("robot_name" .. robot_name)

    --获取版本号
    local version = getControlSoftwareFullVersion()
    local aubo_version = string.match(version, "(.-)%+")
    textmsg("aubo_version" .. aubo_version)

    local payload = serializePayload()
    textmsg("payload" .. payload)

    local cmd = 'cd /opt/arcs/' .. aubo_version .. '/bin && ./cal_end_collision_threshold '.. robot_name .. ' /root/arcs_ws/log/.trace/cal_threshold_data.csv ' .. payload
    textmsg("计算末端碰撞阈值...")
    local file = io.popen(cmd)
    file:close()
    textmsg("末端碰撞阈值计算完成.")
end

function calEndCollisionThresholdWidhTarj()
    recodeTrajData()

    -- TODO 运行出厂轨迹

    calEndCollisionThreshold()
end