Adding the ROS interface to V-REP joint controllers

In this section, we will learn how to interface the seven-DOF arm with the vrep_plugin to stream the state of its joints and receive the control input via topics. As already seen in the previous example, select a component of the robot (for example the base_link_respondable) and create a Lua script that will manage the communication between V-REP and ROS.

Here is the script source code:

if (sim_call_type==sim_childscriptcall_initialization) then  
    -- Check if the required plugin is there (libv_repExtRos.so or libv_repExtRos.dylib): 
    local moduleName=0 
    local moduleVersion=0 
    local index=0 
    local pluginNotFound=true 
    while moduleName do 
        moduleName,moduleVersion=simGetModuleName(index) 
        if (moduleName=='Ros') then 
            pluginNotFound=false 
        end 
        index=index+1 
    end 
 
    if (pluginNotFound) then 
        -- Display an error message if the plugin was not found: 
        simDisplayDialog('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 
         
        -- Retrive the handle of all joints 
        shoulder_pan_handle=simGetObjectHandle('shoulder_pan_joint') 
        shoulder_pitch_handle=simGetObjectHandle('shoulder_pitch_joint') 
        elbow_roll_handle=simGetObjectHandle('elbow_roll_joint') 
        elbow_pitch_handle=simGetObjectHandle('elbow_pitch_joint') 
        wrist_roll_handle=simGetObjectHandle('wrist_roll_joint') 
        wrist_pitch_handle=simGetObjectHandle('wrist_pitch_joint') 
        gripper_roll_handle=simGetObjectHandle('gripper_roll_joint') 
 
        -- Enable joint publishing 
        simExtROS_enablePublisher('/vrep_demo/seve_dof_arm/shoulder_pan/state', 10, simros_strmcmd_get_joint_state,shoulder_pan_handle,0,'')        simExtROS_enablePublisher('/vrep_demo/seven_dof_arm/shoulder_pitch/state', 10, simros_strmcmd_get_joint_state,shoulder_pitch_handle,0,'')       simExtRS_enablePublisher('/vrep_demo/seven_dof_arm/elbow_roll/state', 10, simros_strmcmd_get_joint_state,elbow_roll_handle,0,'')       simExtROS_enablePublisher('/vrep_demo/seven_dof_arm/elbow_pitch/state', 10, simros_strmcmd_get_joint_state,elbow_pitch_handle,0,'')       simExtROS_enablePublisher('/vrep_demo/seven_dof_arm/wrist_roll/state', 10, simros_strmcmd_get_joint_state,wrist_roll_handle,0,'')        simExtROS_enablePublisher('/vrep_demo/seven_dof_arm/wrist_pitch/state', 10, simros_strmcmd_get_joint_state,wrist_pitch_handle,0,'')        simExtROS_enablePublisher('/vrep_demo/seven_dof_arm/gripper_roll/state', 10, simros_strmcmd_get_joint_state,gripper_roll_handle,0,'')        
--Enable joint subscriber       
simExtROS_enableSubscriber('/vrep_demo/seven_dof_arm/shoulder_pan/ctrl', 10, simros_strmcmd_set_joint_target_position, shoulder_pan_handle, 0, '')       simExtROS_enableSubscriber('/vrep_demo/seven_dof_arm/shoulder_pitch/ctrl', 10, simros_strmcmd_set_joint_target_position, shoulder_pitch_handle, 0, '') 
        simExtROS_enableSubscriber('/vrep_demo/seven_dof_arm/elbow_roll/ctrl', 10, simros_strmcmd_set_joint_target_position, elbow_roll_handle, 0, '')        simExtROS_enableSubscriber('/vrep_demo/seven_dof_arm/elbow_pitch/ctrl', 10, simros_strmcmd_set_joint_target_position, elbow_pitch_handle, 0, '')        simExtROS_enableSubscriber('/vrep_demo/seven_dof_arm/wrist_roll/ctrl', 10, simros_strmcmd_set_joint_target_position, wrist_roll_handle, 0, '')        simExtROS_enableSubscriber('/vrep_demo/seven_dof_arm/wrist_pitch/ctrl', 10, simros_strmcmd_set_joint_target_position, wrist_pitch_handle, 0, '')        simExtROS_enableSubscriber('/vrep_demo/seven_dof_arm/gripper_roll/ctrl', 10, simros_strmcmd_set_joint_target_position, gripper_roll_handle, 0, '')  
    end 
end  
if (sim_call_type==sim_childscriptcall_cleanup) then  
end  
if (sim_call_type==sim_childscriptcall_sensing) then  
end  
if (sim_call_type==sim_childscriptcall_actuation) then  
end 

Let's look at the following the explanation of the code. After we have checked the correct installation of vrep_plugin, we initialize an object handler for each joint of the arm:

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

Here, we use the simGetObjectHandle function, whose argument is the name of the object as it appear in the scene hierarchy panel that we want to handle. We can now enable the joint state publishers:

simExtROS_enablePublisher('/vrep_demo/seven_dof_arm/shoulder_pan/state', 1, simros_strmcmd_get_joint_state,shoulder_pan_handle,0,'')       simExtROS_enablePublisher('/vrep_demo/seven_dof_arm/shoulder_pitch/state', 1, simros_strmcmd_get_joint_state,shoulder_pitch_handle,0,')     simExtRS_enablePublisher('/vrep_demo/seven_dof_arm/elbow_roll/state', 1, simros_strmcmd_get_joint_state,elbow_roll_handle,0,'')      simExtROS_enablePublisher('/vrep_demo/seven_dof_arm/elbow_pitch/state', 1, simros_strmcmd_get_joint_state,elbow_pitch_handle,0,'')        simExtROS_enablePublisher('/vrep_demo/seven_dof_arm/wrist_roll/state', 1, simros_strmcmd_get_joint_state,wrist_roll_handle,0,'')        simExtROS_enablePublisher('/vrep_demo/seven_dof_arm/wrist_pitch/state', 1, simros_strmcmd_get_joint_state,wrist_pitch_handle,0,'')        simExtROS_enablePublisher('/vrep_demo/seven_dof_arm/gripper_roll/state', 1, simros_strmcmd_get_joint_state,gripper_roll_handle,0,'') 

This code uses the simExtROS_enablePublisher function with the simros_strmcmd_get_joint_state argument, allowing V-REP to stream the state of the joint specified via its object handler (the fourth parameter in the function), using a sensor_msgs::JointState message type:

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

Finally, here we enable the arm to get the user control input. The simExtROS_enableSubscriber functions are called with the simros_strmcmd_set_joint_target_position command, enabling the arm to subscribe to a set of float streams. The received values will be automatically applied to the joints specified via the object handlers.

As usual, we can test that everything works fine by setting a target position to one of the joints of the robot:

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