Vrep与ROS结合使用

前言与参考

又开新坑了，这次是做TA 自己也不会现学现卖，学完还得教人emmm 慌的一批

测试平台：ROS + Ubuntu 18.04

主要目标：直接截图Project里面的东西了，总的来说就是放一个机器人然后使用键盘控制，建栅格地图，图像识别和定位。但是呢，因为这是Project的目标，所以我大概引个头总不能给做完是把 emmmm 引个头使用而已

参考链接：

CoppeliaSim 下载地址：https://coppeliarobotics.com/downloads
官方的手册：https://www.coppeliarobotics.com/helpFiles/index.html
一个非常基础的Youtub教学视频：https://www.youtube.com/watch?v=nLKLu4Hw_mU
HKUST ELEC 3210的Project

下载与安装

通过https://coppeliarobotics.com/downloads这里下载对应的版本，我这里下载的是ubuntu-18.04

如图，然后解压，我重命名了文件夹VREP。然后在.zshrc或者.bashrc添加一下PATH

export VREP_ROOT=(your path to VREP folder)

注意这里是你自己那边的路径哈

然后就可以运行了

cd ~/VREP
./coppeliaSim.sh

与ROS结合

首先在自己的catkin_ws/src文件夹内clone一下

cd ~/catkin_ws/src
git clone https://gitee.com/kin_zhang/simExtROS.git
git clone https://gitee.com/kin_zhang/ros_bubble_rob.git
catkin_make

如果catkin_ws啥都没有就catkin_make 否则单独编译一下这两个就行，比如：（默认大家都source了哈）

catkin_make -DCATKIN_WHITELIST_PACKAGES=ros_bubble_rob

运行ROSCORE

roscore

根据官方文档来看首先需要运行ROSCORE不然就会出现

[simExtROS:error]   ROS master is not running
[CoppeliaSim:error]   plugin 'ROS': load failed (failed initialization).

接着运行VREP本身，就会出现加载成功：

至此ROS连接成功，开始放车和设置传感器topic pub了

自建小底盘

但其实是可以拿model browser - robots - mobile 里面有小的小车啥的... 但是为了基础就随意搞个长方体+两个轮子放个超声波放个kinect，其实和参考资料里面的youtube是一模一样的... 文字版，但是感觉这种搭建视频更方便点。最后呈现的样子是：文件链接附到最后结尾处了(这个是名为learn.ttt)

首先熟悉一下怎么放东西，点到scene hierachy 然后空白处右键即可添加一系列的东西，右键 -> Add -> Primitive shape -> xxx

熟悉一下界面，红色为可视化的角度调整，绿色部分为对物体的调整，比如选中，①平移/位置设定，②旋转

以下为步骤：

添加Cuboid，尺寸为：[x,y,z] = [0.1,0.2,0.01]，双击一下名字处就能重命名为：RobotBase，然后双击一下蓝色块就能打开特性，点击common然后把object special properties都勾一下

然后点击一下①平移，可以设定这个长方体的位置 [x,y,z]=[0,0,0.035]
添加cylinder，尺寸为：[x,z]=[0.06,0.02]，同上重命名RightWheel，勾选特性，

①位置设定：[x,y,z]=[0.065,-0.05,0.035]，②旋转：beta=90
左轮LeftWheel 可以选中上面右轮，复制粘贴，然后改一下x位置就行

①位置设定：[x,y,z]=[-0.065,-0.05,0.035]
添加一个万向轮，就添加球就行：右键 -> Add -> Primitive shape -> sphere，尺寸：[x]=0.03，同上重命名CastorJoint，如第一步一样勾选特性

①位置设定：[x,y,z]=[0,0.075,-0.15]
然后这之后就开始添加关节Joint了，右键 -> Add -> Joint -> Revolute，重命名为LMotor，双击属性改可视长度为0.06，直径0.02，然后如图勾选电机属性

先点Motor，然后CTRL键选中Wheel，然后再点平移/位置改变，直接点击apply to section 就可以使得电机位置直接到轮子那里了，②旋转：beta=-90
复制一下RMotor然后同上选中对应的轮子，直接点击apply to section，②旋转：beta=90
添加前万向轮的force sensor，给力点。双击属性改object size=0.03

①位置设定：[x,y,z]=[0,0.075,0.03]
最后就是按住鼠标把各自的东西放在子分支下，如此gif展示：
添加传感器，右键 -> Add -> Joint -> Proximity sensor -> Ray type，重命名为FrontUS，并放到RobotBase Tree下，如8步骤

双击传感器进行属性设置如下：

①位置设定：[x,y,z]=[0,0.75,0.06]，②旋转：alpha=90
真的是最后拉，这样的设置界面应该出现这个东西了，然后选中RobotBase -> 右键 -> Associated customization script -> Non threaded，在RobotBase旁边就有个文本小标，双击就是代码区了

代码区

直接复制下面，虽然是lua语言，其实认真看... 也能看得懂在干啥，就是不同的传感器发布的可能需要变一下，比如这里是按超声波的直接发的数值，并没有做tf变换，也没有指定是什么frame_id等等等

function callback_MotorsVelocity(msg)
    --TODO: change the function to get robot velocity and use the inverse kinematic equations
    --      discussed in the class to calculate wheel velocities
    Vl = msg.linear.x + msg.angular.z
    Vr = msg.linear.y - msg.angular.z
    sim.setJointTargetVelocity(hLeftMotor,Vl)
    sim.setJointTargetVelocity(hRightMotor,Vr)
    sim.addStatusbarMessage(string.format("Vl:%f Vr:%f",Vl, Vr))
end

if (sim_call_type==sim.syscb_init) then

    -- Get handle to simulation entities
    hLeftMotor = sim.getObjectHandle("LMotor")
    hRightMotor = sim.getObjectHandle("RMotor")
    hFrontUS = sim.getObjectHandle("FrontUS")
    -- TODO: Create the handles of your two new ultrasonic sensors
    
    -- Check if the required ROS plugin is loaded (libv_repExtRos.so or libv_repExtRos.dylib):
    local moduleName=0
    local moduleVersion=0
    local index=0
    local pluginNotFound=true
    while moduleName do
        moduleName,moduleVersion=sim.getModuleName(index)
        if (moduleName=='ROS') then
            pluginNotFound=false
        end
        index=index+1
    end
    rosInterfacePresent=simROS
    
    if (pluginNotFound) then
        -- Display an error message if the plugin was not found:
        sim.addStatusbarMessage("The RosPlugin was not found. Simulation will not run properly!")
    else
        -- Now enable topic publishing and streaming of the vision sensor's data
        sim.addStatusbarMessage("The RosPlugin was found. Everything is OK!")
        sensorPub=simROS.advertise('/frontUS','std_msgs/Float32')   --TODO: change the message type name as described in the assignment 
        motorsSub=simROS.subscribe('/cmd_vel','geometry_msgs/Twist','callback_MotorsVelocity')
        -- simROS.publisherTreatUInt8ArrayAsString(pub) -- treat uint8 arrays as strings (much faster, tables/arrays are kind of slow in Lua)
    end

end


if (sim_call_type==sim.syscb_actuation) then
    -- Send an updated sensor and simulation time message, and send the transform of the robot:
    if not pluginNotFound then
        --TODO: write the code to get the distance from your new sensors and publish the message
        result,distance=sim.readProximitySensor(hFrontUS)
        if(distance~=nil) then
            simROS.publish(sensorPub,{data=distance})
        end
    end
    
end

if (sim_call_type==sim.syscb_cleanup) then
    if rosInterfacePresent then
        simROS.shutdownPublisher(sensorPub)
        simROS.shutdownSubscriber(motorsSub)
    end
end

这个搞完后，点击界面上的开始，(记得先打开roscore)，然后就可以看到topic里有数据了，控制车辆可以自己弄个键盘控车也可以直接pub出来，比如：

rostopic pub /cmd_vel geometry_msgs/Twist "linear:
  x: 1.1
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.01" -r 1

更为复杂的场景

上述是自建的小底盘，其实Vrep里有很多模型可以直接拖进来使用，以robots -> mobile -> Pioneer为例，拖进来后可以查看这里面有的东西，我们再添加一个camera，一个2D的laser scan

然后代码区则是：

-- set velocity
function velocity_callback(msg)
    -- This is the sub_velocity callback function
    -- sim.addStatusbarMessage('sub_velocity receiver: ' .. msg.linear.x)
    sim.auxiliaryConsolePrint(console_debug,'linear: ' .. msg.linear.x)
    sim.auxiliaryConsolePrint(console_debug,' angular: ' .. msg.angular.z .. '\n')
    vLeft = 6*msg.linear.x - 0.6*msg.angular.z
    vRight = 6*msg.linear.x + 0.6*msg.angular.z
    sim.setJointTargetVelocity(motorLeft,vLeft)
    sim.setJointTargetVelocity(motorRight,vRight)
end


-- disable or enable laser
function laserSwitch_callback(msg)
    if msg.data then
        sim.auxiliaryConsolePrint(console_debug,"laser switch cmd: publish laserscan")
    else
        sim.auxiliaryConsolePrint(console_debug,"laser switch cmd: stop publishing laserscan")
    end
    IS_LASER_ENABLE = msg.data
end

-- disable or enable camera
function cameraSwitch_callback(msg)
    if msg.data then
        sim.auxiliaryConsolePrint(console_debug,"camera switch cmd: publish image")
    else
        sim.auxiliaryConsolePrint(console_debug,"camera switch cmd: stop publishing image")
    end
    IS_CAMERA_ENABLE = msg.data
end



if (sim_call_type==sim.syscb_init) then 
    -- create the console to print some debug infomation
    console_debug=sim.auxiliaryConsoleOpen('debug info',100000,1)

    -- Check the required RosInterface first
    moduleName=0
    index=0
    rosInterfacePresent=false
    while moduleName do
        moduleName=sim.getModuleName(index)
        if (moduleName=='ROS') then
            rosInterfacePresent=true
        end
        index=index+1
    end
    if rosInterfacePresent then   
        sub_velocity = simROS.subscribe('/vrep/cmd_vel','geometry_msgs/Twist','velocity_callback')
        sub_laser   = simROS.subscribe('/vrep/laser_switch','std_msgs/Bool','laserSwitch_callback')
        sub_camera  = simROS.subscribe('/vrep/camera_switch','std_msgs/Bool','cameraSwitch_callback')
        pub_camera  = simROS.advertise('/vrep/image','sensor_msgs/Image')
        pub_laser   = simROS.advertise('/vrep/scan','sensor_msgs/LaserScan')
        pub_timer   = simROS.advertise('/simTime','std_msgs/Float32')    
    else
        sim.auxiliaryConsolePrint(console_debug,'rosinterface has not been loaded -- pause...')
        sim.pauseSimulation()
    end
    
    -- pioneer car
    car = sim.getObjectHandle("Pioneer_p3dx")
    motorLeft=sim.getObjectHandle("Pioneer_p3dx_leftMotor")
    motorRight=sim.getObjectHandle("Pioneer_p3dx_rightMotor")

    -- camera
    IS_CAMERA_ENABLE = true
    cameraHandle=sim.getObjectHandle('camera')
    image_seq=0

    -- laser
    IS_LASER_ENABLE = true
    laser_cnt = 0
    laserscanHandle = sim.getObjectHandle("LaserScanner_2D")
    laserHandle=sim.getObjectHandle("LaserScannerLaser_2D")
    jointHandle=sim.getObjectHandle("LaserScannerJoint_2D")
    graphHandle=sim.getObjectHandle("LaserScannerGraph_2D")
    objName=sim.getObjectName(laserscanHandle)
    communicationTube=sim.tubeOpen(0,objName..'_2D_SCANNER_DATA',1)
    scanningAngle= 180
    scanningDensity= 5
    laser_seq = 0

    -- get transform
    local tl = sim.getObjectPosition(laserscanHandle,sim.handle_parent)
    local tc = sim.getObjectPosition(cameraHandle,sim.handle_parent)
    local q = sim.getObjectQuaternion(cameraHandle,sim.handle_parent)
    laser2robot= {
        header={
            seq=0,
            stamp=0.0,
            frame_id="base_link"
        },
        child_frame_id="laser_link",
        transform={
            translation = {x=tl[1],y=tl[2],z=tl[3]},
            rotation= {x=0,y=0, z=0,w=1}
        }
    }
    
    camera2robot= {
        header={
            seq=0,
            stamp=0.0,
            frame_id="base_link"
        },
        child_frame_id="camera_link",
        transform={
            translation = {x=tc[1],y=tc[2],z=tc[3]},
            rotation= {x=q[1],y=q[2], z=q[3],w=q[4]}
        }
    }
    tf_seq = 0;
end 

if (sim_call_type==sim.syscb_sensing) then 
    --
    -- get image from vision sensor
    --
    local current_sim_time = sim.getSimulationTime()
    simROS.publish(pub_timer, {data=current_sim_time})
    if IS_CAMERA_ENABLE then
        local img={}
        local data,w,h=sim.getVisionSensorCharImage(cameraHandle)
        img['header']={seq=image_seq,stamp=current_sim_time, frame_id="camera_link"}
        img['height']=h
        img['width']=w
        img['encoding']='rgb8'
        img['is_bigendian']=1
        img['step']=w*3
        img['data']= sim.unpackUInt8Table(data)
        simROS.publish(pub_camera,img) 
        image_seq = image_seq +1;
    end
    --
    -- get laserscan
    -- 
    if IS_LASER_ENABLE and laser_cnt >= 4 then
        laser_cnt = 0
        sim.resetGraph(graphHandle)
        pts=scanningAngle*scanningDensity+1
        p=-scanningAngle*math.pi/360
        stepSize=math.pi/(scanningDensity*180)
        ranges={}
        modelInverseMatrix=simGetInvertedMatrix(sim.getObjectMatrix(laserscanHandle,-1))
        -- generate data
        for i=0,pts,1 do
            sim.setJointPosition(jointHandle,p)
            p=p+stepSize
            r,dist,pt=sim.handleProximitySensor(laserHandle) -- pt is RELATIVE to te rotating laser beam!
            if r>0 then
                table.insert(ranges, dist)
            end
            sim.handleGraph(graphHandle,0.0)
        end
        -- Now send the data:
        if #ranges>0 then
            local scan={}
            scan['header']={seq=laser_seq, stamp=current_sim_time, frame_id="laser_link"}
            scan['angle_min']=0
            scan['angle_max']=scanningAngle*math.pi/180
            scan['angle_increment']=-1*stepSize
            scan['time_increment']=0
            scan['scan_time']= 0
            scan['range_min'] = 0
            scan['range_max'] = 50
            scan['ranges']= ranges
            scan['intensities']= {}
            simROS.publish(pub_laser,scan)  
            laser_seq = laser_seq + 1
        end
    end
    laser_cnt = laser_cnt + 1
    --
    -- transform
    --
    laser2robot['header']['stamp']=current_sim_time
    laser2robot['header']['seq'] = tf_seq
    simROS.sendTransform(laser2robot)

    camera2robot['header']['stamp']=current_sim_time
    camera2robot['header']['seq'] = tf_seq
    simROS.sendTransform(camera2robot)

    tf_seq = tf_seq + 1;
    
end



if (sim_call_type==sim.syscb_cleanup) then 
-- Following not really needed in a simulation script (i.e. automatically shut down at simulation end):
    if rosInterfacePresent then
        simROS.shutdownSubscriber(sub_laser)
        simROS.shutdownSubscriber(sub_camera)
        simROS.shutdownSubscriber(sub_velocity)
        simROS.shutdownPublisher(pub_camera)
        simROS.shutdownPublisher(pub_laser)
    end
end 


if (sim_call_type==sim.syscb_actuation) then 

end

上述相关文件如有vrep可以自行下载打开查看：链接：https://pan.baidu.com/s/1KzBhkvSQjtFiAlDdm_-6Ww 提取码：95ne

posted @ 2021-10-25 14:44 Kin_Zhang 阅读(1472) 评论(0) 收藏举报

刷新页面返回顶部

张聪明 (Kin_Zhang)

HKUST-MPhil 学生无人驾驶、强化学习研究中

Vrep与ROS结合使用

前言与参考

下载与安装

与ROS结合

运行ROSCORE

自建小底盘

代码区

更为复杂的场景

公告

张聪明 (Kin_Zhang)

HKUST-MPhil 学生 无人驾驶、强化学习研究中

Vrep与ROS结合使用

前言与参考

下载与安装

与ROS结合

运行ROSCORE

自建小底盘

代码区

更为复杂的场景

公告

HKUST-MPhil 学生无人驾驶、强化学习研究中