tensorflow訓練cnn網絡實現避障與導航（二）V-rep仿真環境的搭建

    順着第一篇的內容，進入第一項工作——V-rep中搭建仿真環境

    V-rep是一款較爲優秀的仿真軟件（雖然依然有很多bug），其豐富的傳感器、衆多集成好的現有機器人、較爲強大的軟件內編程都是這款軟件吸引人的地方，但與此同時，V-rep也有一些缺陷，首先是其在PC機上運行較多、較複雜傳感器是幀率下降非常明顯，其次是其與ROS的橋接雖然可以實現但明顯資料不足，且ros與V-rep都具有衆多版本，各版本之間的依賴與差別也是令初學者頭疼的問題。

    要實現在仿真環境中訓練網絡執行避障等決策，首先需要一個可以採集數據及驗證網絡的環境——V-rep。

    V-rep主要負責以下工作:

    1/ 要有一個可以行進、控制、採集數據的小車模型：

                                       

    上圖就是我們用來採集數據的基本地圖，其實是從油管上一個教學視頻的基礎上改造的，但是爲了能夠採集激光數據，這裏用了HoKuYo 單線激光當作傳感器，這種單線激光可以調整分辨率、測量範圍等參數，並且經過尋找發現一款與ROS可以完美配合的單線激光——Hokuyo_URG_04LX_UG01_ROS

                                                                                                                                        

    這款Hokuyo激光雷達可以輸出非常完備的信息，並且可以通過發送/vrep/front_scan數據自動pub出當前幀的激光數據，並不需要像v-rep其他傳感器那樣需要自己手寫pub文件，可以說是非常方便了。

     有了小車，同時還需要一個供小車遊玩的遊樂場，這裏用磚塊隨便堆一堆就好了，但注意要想讓path-planning工作則需要將自己加入的障礙物列入collections 中，並且需要對其屬性進行相應調整，具體參見之前的文章。

   

    2/ 信息交互

    在vrep中，仿真世界的物體會可以對應着一個“句柄”（handle），通過gethandle 可以獲得仿真世界物體的參數，而通過一衆set函數則可以利用handle設置仿真世界裏物體的參數，例如，我們可以通過SimGetObjectPosition(**handle,-1) 這個函數來獲得大部分物體的位置信息（以數組的形式給出），反過來我們也可以通過SimSetObjectPosition(**handle,-1) 這個函數來移動物體的位置，這樣就可以實現在V-rep中的信息的傳遞（程序到仿真可視化環境的傳遞）。

    而V-rep與ROS的交互會稍微麻煩一丁點，首先需要按照之前的文章中所示配置好vrep和ros的bridge，接着可以通過simExtROS_enableSubscriber(...)來接收ros發來的消息，這裏必須保證兩邊消息類型的一致性，否則會無法解析消息。同理也可以通過simExtROS_enablePublisher函數來對ROS發消息，通常我們的運行中獲得的數據就是通過這兩個函數來與外界的花花世界相溝通。

    通過以上步驟，得到了一個可以與外界交互信息的仿真環境，我的v-rep 中小車的LUA  腳本是這樣的：

simSetThreadSwitchTiming(2) -- Default timing for automatic thread switching
simDelegateChildScriptExecution()


lm=simGetObjectHandle('left_motor')
rm=simGetObjectHandle('right_motor')

robot_handle = simGetObjectHandle('Vehicle')
path_handle = simGetObjectHandle('Path')

pos_on_path = 0 
dis = 0-- distance from the robot to the point on the path

path_plan_handle = simGetPathPlanningHandle('PathPlanningTask')
simSearchPath(path_plan_handle,200)

start_dummy_handle = simGetObjectHandle('start')

----------------------------------------------------add by jkwang 7.11
robot_handle = simGetObjectHandle('Dummy')
target_handle = simGetObjectHandle('Target')
jointV_handle = simGetObjectHandle('jointV')
uV_handle = simGetObjectHandle('uV')
control_handle = simGetObjectHandle('control')
----------------------------------------------------add by jkwang 7.11

num=0
while (simGetSimulationState()~=sim_simulation_advancing_abouttostop ) do

    
        --simAddStatusbarMessage(string.format("A NEW WHILE "))

        rob_pos = simGetObjectPosition(robot_handle,-1)
        rob_ori = simGetObjectOrientation(robot_handle,-1)   
        path_pos = simGetPositionOnPath(path_handle,pos_on_path)
        simSetObjectPosition(start_dummy_handle,-1,path_pos)
        --simAddStatusbarMessage(string.format("path_pos: %d,%d",path_pos[1],path_pos[2]))
        m = simGetObjectMatrix(robot_handle,-1)
        m = simGetInvertedMatrix(m)
        ---------------------------------add by jkwang 7.26---------------------
        simExtROS_enableSubscriber('/talker',1,simros_strmcmd_set_object_pose,control_handle,-1,'l2')
        control_pos = simGetObjectPosition(control_handle,-1)
        simAddStatusbarMessage(string.format("in the while: control = %f,%f",control_pos[1],control_pos[2]))
        ------------------------------------------------------------------------

        path_pos = simMultiplyVector(m,path_pos)
        dis = math.sqrt(path_pos[1]^2+(path_pos[2]^2))
        --simAddStatusbarMessage(string.format("dis1: %f",dis))
        phi = math.atan2(path_pos[2],path_pos[1])
        deg = phi*180/math.pi
        --simAddStatusbarMessage(string.format("deg: %f",deg))
----------------------------------------------------------------------add by xh
        n = simGetObjectMatrix(target_handle,-1)
        n = simGetInvertedMatrix(n)
        final_dis = simMultiplyVector(n,rob_pos)
        dis2 = math.sqrt(final_dis[1]^2+(final_dis[2]^2))
        --simAddStatusbarMessage(string.format("dis2: %f",dis2))

        --simAddStatusbarMessage(string.format(" IF PLAN "))
        if dis2 < 0.2 then
            local next_pos={0,0,0}
            omega_right=0
            omega_left=0
            simSetJointTargetVelocity(rm,omega_right)
            simSetJointTargetVelocity(lm,omega_left)
            simSetObjectPosition(start_dummy_handle,-1,rob_pos)
            simSetObjectOrientation(start_dummy_handle,-1,rob_ori)
            pos_on_path = 0     
      
            while ( 1 ) do
                break_while_flag = 1
                x = 3*simGetFloatParameter(sim_floatparam_rand) - 1.5
                y = 3*simGetFloatParameter(sim_floatparam_rand) - 1.5
                if math.sqrt((x+0.8)^2+y^2) <= 0.44 then
                    break_while_flag=0;
                end   

                if x >= (0.4-0.2) and x<=(0.9+0.2) and y>=(-1-0.2) and y<=(1+0.2)  then 
                    break_while_flag=0;
                end

                if break_while_flag == 1 then
                    break
                end
            end
            next_pos[1] = x 
            next_pos[2] = y 
            next_pos[3] = 0 
            num = num + 1

            simSetObjectPosition(target_handle,-1,next_pos)
            simSearchPath(path_plan_handle,200)
            plan_flag = 0
            --simAddStatusbarMessage(string.format("in the if %d",flag_back_pose[1]))
        end
        --simAddStatusbarMessage(string.format("dis2: %f",dis2))
---------------------------------------------------------------------------------
        run_flag=1
        --simAddStatusbarMessage(string.format(" WHEATHER TO GO "))

        v_des = control_pos[1]
        om_des = control_pos[2]

        d = 0.06
        v_r = v_des+d*om_des
        v_l = v_des-d*om_des
        
        r_w = 0.0275
        omega_right = v_r/r_w
        omega_left  = v_l/r_w
        local info_pose = {0,omega_left,omega_right}
        local info_ori = {0,v_des,om_des}

        simSetJointTargetVelocity(rm,-omega_right)
        simSetJointTargetVelocity(lm,-omega_left)
        simSetObjectPosition(jointV_handle,-1,info_pose)
        simSetObjectPosition(uV_handle,-1,info_ori)

        if (dis<0.15) then
            pos_on_path = pos_on_path+0.01
        end    
--simAddStatusbarMessage(string.format("6"))

    ---------------------------------------add by jkwang 7.11
    simExtROS_enablePublisher('/car_position',1,simros_strmcmd_get_object_pose,robot_handle,-1,'f1')
    simExtROS_enablePublisher('/end_position',1,simros_strmcmd_get_object_pose,target_handle,-1,'f2')
    simExtROS_enablePublisher('control_info_joint',1,simros_strmcmd_get_object_pose,jointV_handle,-1,'l1')
    simExtROS_enablePublisher('control_info_uv',1,simros_strmcmd_get_object_pose,uV_handle,-1,'l2')
    ---------------------------------------add by jkwang 7.11

end

   注意這裏調用了

simSearchPath(path_plan_handle,200)

    這個函數用於在運行的中途重新規劃路徑，可以大大節約採集數據的時間，使用自動設置end_point與path planning可以節省大量的時間。