CoppeliaSim(VREP)和ROS Moveit的联合仿真

目标

完成一套多视觉和点云感知的机械臂场景，与Moveit实现联合仿真，通过ROS通信实现传感器数据的获取（视觉、点云、机械臂状态等）并实现对机器人的控制。

实现过程

环境配置

CoppeliaSim自带视觉传感器（VIsion sensor、Kinect、雷达、GPS等传感器）直接拖拽进入即可，要调整的是他们之间的位置关系，通过菜单栏的调整平移，调整旋转。

菜单

通过CoppeliaSim的URDF导入功能，直接可以完成机械臂的导入，这部分没有遇到难点，入口在Plugins的URDF导入按钮。

这部分没有太多的问题，主要难点都在如何给每个模块单元（机械臂、摄像头）等配置脚本，实现数据到ROS的通信转换上。

脚本

CoppeliaSim默认脚本采用Lua语言，而每个添加的元素都可以添加相应的脚本，选中左侧相关的元素邮件点击“add”-‘Associated child script”-“Non Threaded ”即可，之后就可以添加代码了。

搭建的机械臂场景

function sysCall_init()
    -- do some initialization here
end

function sysCall_actuation()
    -- put your actuation code here
end

function sysCall_sensing()
    -- put your sensing code here
end

function sysCall_cleanup()
    -- do some clean-up here
end

-- See the user manual or the available code snippets for additional callback functions and details

如上，默认脚本主要分为上述三个部分，我用到的就init和actutation部分，以及cleanup。

sysCall_init()就是C++里面的初始化函数，变量等都可以在这里面设置，这个函数只调用一次，比如ROS订阅、发布变量初始化定义；sysCall_actuation()是一个周期执行的函数，相当于一个循环，所以相关代码可以放置在这里，比如发布topic等。

CoppeliaSim所有函数基本在Sim里面，而ROS的在SimROS里面，

 机械臂脚本

如果要实现moveit的使用，需要两个部分，一个是把机械臂的状态通过topic发出，通常moveit默认接收的topic为

名称：/joint_states

定义：sensor_msgs/JointState

而发布的控制指令为：

名称：/move_group/fake_controller_joint_states

定义：sensor_msgs/JointState

所以我们只要按照这两个topic封装即可，

具体请看我写个脚本，大体思路和我们用C和python写发布和订阅差不多，但是会省去一些步骤，下面这个脚本就实现了每个关节信息的发布和控制指令的接收，也发布了tf。

function subscriber_callback(msg)
    -- This is the subscriber callback function
    for i=1,#msg.position,1 do
        sim.setJointPosition(jointHandles[i],msg.position[i])
    end
end

function getTransformStamped(objHandle,name,relTo,relToName)
    -- This function retrieves the stamped transform for a specific object
    t=sim.getSystemTime()
    p=sim.getObjectPosition(objHandle,relTo)
    o=sim.getObjectQuaternion(objHandle,relTo)
    return {
        header={
            stamp=t,
            frame_id=relToName
        },
        child_frame_id=name,
        transform={
            translation={x=p[1],y=p[2],z=p[3]},
            rotation={x=o[1],y=o[2],z=o[3],w=o[4]}
        }
    }
end

function getJointStates()
    for i=1,6,1 do
        p = sim.getJointPosition(jointHandles[i])
        jointPosition[i] = p
    end
end

function sysCall_init()
    -- do some initialization here

    jointHandles={-1,-1,-1,-1,-1,-1}
    jointNames = {'joint1','joint2','joint3','joint4','joint5','joint6'}
    jointPosition = {-1,-1,-1,-1,-1,-1}
    for i=1,6,1 do
        jointHandles[i]=sim.getObjectHandle('joint'..i)
    end
    
    -- The child script initialization
    rosInterfacePresent=simROS

    -- Prepare the float32 publisher and subscriber (we subscribe to the topic we advertise):
    if rosInterfacePresent then
        publisher=simROS.advertise('/joint_states','sensor_msgs/JointState')
        subscriber=simROS.subscribe('/move_group/fake_controller_joint_states','sensor_msgs/JointState','subscriber_callback')
        -- subscriber=simROS.subscribe('/joint_states','sensor_msgs/JointState','subscriber_callback')
    end
end

function sysCall_actuation()
    -- put your actuation code here
 
    -- Send an updated simulation time message, and send the transform of the object attached to this script:
    if rosInterfacePresent then
        getJointStates()
        simROS.publish(publisher,{header = {stamp =  sim.getSystemTime()} ,name = jointNames, position=jointPosition})
        for i=1, 6, 1 do 
            simROS.sendTransform(getTransformStamped(jointHandles[i],sim.getObjectName(jointHandles[i]),-1,'world'))
        end
            -- To send several transforms at once, use simROS.sendTransforms instead
    end
end

function sysCall_sensing()
    -- put your sensing code here
end


function sysCall_cleanup()
    -- Following not really needed in a simulation script (i.e. automatically shut down at simulation end):
    if rosInterfacePresent then
        simROS.shutdownPublisher(publisher)
        simROS.shutdownSubscriber(subscriber)
    end
end

传感器脚本

这部分卡在了一个是需要做句柄的获取，sim.getObjectHandle('Vision_sensor_ortho')，Vision_sensor_ortho就是添加元素的名字；另外一个卡在了如何不用循环把数据转换成sensor_msg/Image发布，我对lua不熟悉，这块用了很长时间，最后通过代码rgb_data = {string.byte(color_buf,1,img_width*img_height*3)}搞定。

这里需要对函数

    color_buf = sim.getVisionSensorImage(visionhandle,0,0,img_width,img_height,1)
说明，

1）最后一个参数如果为0，则返回长witdh*height*channel，值0-1的table，在ros image 值为0-255，所以直接用这个图像为黑色；如果参数设置为1，则返回一个string类型，值为0-255，长度也是witdh*height*channel。

而发布image的topic代码为，

simROS.publish(image_pub,{step=img_width*3,height=img_height, width=img_width, encoding='rgb8',data=rgb_data})

其中data部分需要说int值的table，因此需要进行string到table的转换，经过大量测试的调研，发现了string.byte(str,i,j)这个函数，i,j表示要截取的端，可以从1到最后，但是这部分我也是经过了很久才搞定，它的输出是多个变量，左侧必须设置width*height*channel个变量才可以，后来发现只要价格大括号即可， 也就是{string.byte(color_buf,1,img_width*img_height*3)}。

2）getVisionSensorImage这个函数有时候会报错，大概率是因为分辨率不对，CopperliaSim添加的VIsion Sensor都是有默认分辨率的，可以手动修改，这块必须和之前设置的一样，否则会报错。

查看方法可以通过函数

    resolution = sim.getVisionSensorResolution(visionhandle)

 然后打印获取，修改分辨率方法，选中元素点击按钮，跳出“Scene Object Properties”界面，修改即可，这个按钮对其他元素也适用。

传感器参数配置界面

function sysCall_init()
    -- The child script initialization
    visionhandle = sim.getObjectHandle('Vision_sensor_ortho')
    rosInterfacePresent=simROS
    if rosInterfacePresent then
        image_pub = simROS.advertise('vision_top','sensor_msgs/Image') 
    end
    img_width = 512
    img_height = 512
end

function sysCall_actuation()
    -- put your actuation code here
    resolution = sim.getVisionSensorResolution(visionhandle)
    color_buf = sim.getVisionSensorImage(visionhandle,0,0,img_width,img_height,1)
    if type(color_buf) == 'string' then
        -- k1,k2,k3 = string.byte("abcdef",1,3)
        -- print (k1,k2,k3)    --97    98    99
        rgb_data = {string.byte(color_buf,1,img_width*img_height*3)}
    end
    
    if rosInterfacePresent then
        simROS.publish(image_pub,{step=img_width*3,height=img_height, width=img_width, encoding='rgb8',data=rgb_data})
    end
end

function sysCall_sensing()
    -- put your sensing code here
    
end

function sysCall_cleanup()
    -- do some clean-up here
    if rosInterfacePresent then
        simROS.shutdownPublisher(image_pub)
        -- simROS.shutdownSubscriber(subscriber)
    end
end