【V-REP学習記録06】V-REPジョイントコントローラにROSインターフェースを追加

このセクションでは、7 自由度マニピュレータを vrep_plugin に接続して、そのジョイント状態をストリーミングし、サブジェクトを介して制御入力を受け取る方法を学びます。前の例で見たように、ロボットのコンポーネント (base_link_respondable など) を選択し、V-REP と ROS の間の通信を管理する Lua スクリプトを作成します。

以下はスクリプトのソースコード (~/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 関数を使用します。共同州パブリッシャーが使用できるようになりました。

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 がセンサー_msgs::JointState メッセージタイプを使用してオブジェクトハンドラー (関数の 4 番目のパラメーター) 経由で指定されたジョイント状態をストリーミングできるようにします。

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 コマンドを使用して呼び出されます。受け取った値は、オブジェクトハンドラーを介して指定されたジョイントに自動的に適用されます。

ロボットのジョイントの 1 つに目標位置を設定することで、すべてが正常に動作していることをテストできます。

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

【V-REP学習記録06】V-REPジョイントコントローラにROSインターフェースを追加

おすすめ