【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 메시지 유형을 사용하여 개체 핸들러(함수의 네 번째 매개변수)를 통해 지정된 관절 상태를 스트리밍할 수 있도록 합니다.

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】V-REP 조인트 컨트롤러에 ROS 인터페이스 추가

추천