【V-REP学习记录06】Adding the ROS interface to V-REP joint controllers

在本节中，我们将学习如何将七自由度机械臂与vrep_plugin连接起来，以流式传输其关节状态并通过主题接收控制输入。在前面的示例中已经看到，选择机器人的一个组件(例如base_link_respondable)并创建一个Lua脚本，该脚本将管理V-REP和ROS之间的通信。

以下是脚本源代码（~/catkin_ws/src/vrep_demo_pkg/scene/seven_dof_arm.ttt）:

if (sim_call_type==sim.syscb_init) then 
    -- 检查所需的插件是否存在（libv_repExtRos.so或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

    if (pluginNotFound) then
        -- 如果未找到插件，显示错误消息
        sim.displayDialog('Error','ROS plugin was not found.\nSimulation will not run properly',sim.dlgstyle_ok,false,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1})
    else
        -- 获取各个关节的句柄
        shoulder_pan_handle=sim.getObjectHandle('shoulder_pan_joint')
        shoulder_pitch_handle=sim.getObjectHandle('shoulder_pitch_joint')
        elbow_roll_handle=sim.getObjectHandle('elbow_roll_joint')
        elbow_pitch_handle=sim.getObjectHandle('elbow_pitch_joint')
        wrist_roll_handle=sim.getObjectHandle('wrist_roll_joint')
        wrist_pitch_handle=sim.getObjectHandle('wrist_pitch_joint')
        gripper_roll_handle=sim.getObjectHandle('gripper_roll_joint')

        -- 启用关节状态发布者
        simExtROS_enablePublisher('/vrep_demo/seven_dof_arm/shoulder_pan/state', 0, simros_strmcmd_get_joint_state, shoulder_pan_handle, 0, '')
        simExtROS_enablePublisher('/vrep_demo/seven_dof_arm/shoulder_pitch/state', 0, simros_strmcmd_get_joint_state, shoulder_pitch_handle, 0, '')
        simExtROS_enablePublisher('/vrep_demo/seven_dof_arm/elbow_roll/state', 0, simros_strmcmd_get_joint_state, elbow_roll_handle, 0, '')
        simExtROS_enablePublisher('/vrep_demo/seven_dof_arm/elbow_pitch/state', 0, simros_strmcmd_get_joint_state, elbow_pitch_handle, 0, '')
        simExtROS_enablePublisher('/vrep_demo/seven_dof_arm/wrist_roll/state', 0, simros_strmcmd_get_joint_state, wrist_roll_handle, 0, '')
        simExtROS_enablePublisher('/vrep_demo/seven_dof_arm/wrist_pitch/state', 0, simros_strmcmd_get_joint_state, wrist_pitch_handle, 0, '')
        simExtROS_enablePublisher('/vrep_demo/seven_dof_arm/gripper_roll/state', 0, simros_strmcmd_get_joint_state, gripper_roll_handle, 0, '')

        -- 启用关节目标位置订阅者
        simExtROS_enableSubscriber('/vrep_demo/seven_dof_arm/shoulder_pan/ctrl', 0, simros_strmcmd_set_joint_target_position, shoulder_pan_handle, 0, '')
        simExtROS_enableSubscriber('/vrep_demo/seven_dof_arm/shoulder_pitch/ctrl', 0, simros_strmcmd_set_joint_target_position, shoulder_pitch_handle, 0, '')
        simExtROS_enableSubscriber('/vrep_demo/seven_dof_arm/elbow_roll/ctrl', 0, simros_strmcmd_set_joint_target_position, elbow_roll_handle, 0, '')
        simExtROS_enableSubscriber('/vrep_demo/seven_dof_arm/elbow_pitch/ctrl', 0, simros_strmcmd_set_joint_target_position, elbow_pitch_handle, 0, '')
        simExtROS_enableSubscriber('/vrep_demo/seven_dof_arm/wrist_roll/ctrl', 0, simros_strmcmd_set_joint_target_position, wrist_roll_handle, 0, '')
        simExtROS_enableSubscriber('/vrep_demo/seven_dof_arm/wrist_pitch/ctrl', 0, simros_strmcmd_set_joint_target_position, wrist_pitch_handle, 0, '')
        simExtROS_enableSubscriber('/vrep_demo/seven_dof_arm/gripper_roll/ctrl', 0, simros_strmcmd_set_joint_target_position, gripper_roll_handle, 0, '') 
    end
end 

if (sim_call_type==sim.syscb_cleanup) then 
    -- 在cleanup回调函数中，可以添加一些恢复代码
end 

if (sim_call_type==sim.syscb_sensing) then 
    -- 在sensing回调函数中，可以添加主要的感知代码
end 

if (sim_call_type==sim.syscb_actuation) then 
    -- 在actuation回调函数中，可以添加执行代码
end

在正确安装Vrep_plugin之后，我们为手臂的每个关节初始化一个对象处理程序:

        shoulder_pan_handle=sim.getObjectHandle('shoulder_pan_joint')
        shoulder_pitch_handle=sim.getObjectHandle('shoulder_pitch_joint')
        elbow_roll_handle=sim.getObjectHandle('elbow_roll_joint')
        elbow_pitch_handle=sim.getObjectHandle('elbow_pitch_joint')
        wrist_roll_handle=sim.getObjectHandle('wrist_roll_joint')
        wrist_pitch_handle=sim.getObjectHandle('wrist_pitch_joint')
        gripper_roll_handle=sim.getObjectHandle('gripper_roll_joint')

在这里，我们使用simGetObjectHandle函数，它的参数是对象的名称，因为它出现在我们想要处理的场景层次面板中。现在可以使用 joint state publishers:

simExtROS_enablePublisher('/vrep_demo/seven_dof_arm/shoulder_pan/state', 0, simros_strmcmd_get_joint_state,shoulder_pan_handle,0,'')

这段代码使用simExtROS_enablePublisher函数和simros_strmcmd_get_joint_state参数，允许V-REP通过其对象处理程序(函数中的第四个参数)使用sensor_msgs::JointState消息类型来流式传输指定的关节状态:

simExtROS_enableSubscriber('/vrep_demo/seven_dof_arm/shoulder_pan/ctrl', 0, simros_strmcmd_set_joint_target_position, shoulder_pan_handle, 0, '')
simExtROS_enableSubscriber('/vrep_demo/seven_dof_arm/shoulder_pitch/ctrl', 0, simros_strmcmd_set_joint_target_position, shoulder_pitch_handle, 0, '')
simExtROS_enableSubscriber('/vrep_demo/seven_dof_arm/elbow_roll/ctrl', 0, simros_strmcmd_set_joint_target_position, elbow_roll_handle, 0, '')
simExtROS_enableSubscriber('/vrep_demo/seven_dof_arm/elbow_pitch/ctrl', 0, simros_strmcmd_set_joint_target_position, elbow_pitch_handle, 0, '')
simExtROS_enableSubscriber('/vrep_demo/seven_dof_arm/wrist_roll/ctrl', 0, simros_strmcmd_set_joint_target_position, wrist_roll_handle, 0, '')
simExtROS_enableSubscriber('/vrep_demo/seven_dof_arm/wrist_pitch/ctrl', 0, simros_strmcmd_set_joint_target_position, wrist_pitch_handle, 0, '')
simExtROS_enableSubscriber('/vrep_demo/seven_dof_arm/gripper_roll/ctrl', 0, simros_strmcmd_set_joint_target_position, gripper_roll_handle, 0, '')

最后，这里我们使手臂能够获得用户控制输入。simextras_enablessubscriber函数使用simros_strmcmd_set_joint_target_position命令调用。接收到的值将自动应用于通过对象处理程序指定的关节。

我们可以通过为机器人的一个关节设置目标位置来测试一切是否正常:

rostopic pub /vrep_demo/seven_dof_arm/wrist_pitch/ctrl std_msgs/Float64 "data: 1.0"

【V-REP学习记录06】Adding the ROS interface to V-REP joint controllers

猜你喜欢