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可以节省大量的时间。