Back to [[vrep:main|V-REP Main Page]]

The robot inside V-REP can be controlled using the Robot Operating System [[https://www.ros.org/|ROS]]. 
All documentation is available on [[https://www.coppeliarobotics.com/helpFiles/en/rosInterf.htm|Internet]] .

However, this tutorial is intended to make the setup of this interface easier by proposing a working Lua code. 
To implement ROS interface, we will use the same [[https://www.ensta-bretagne.fr/zerr/filerepo/vrep/myLittleBotNoScript.ttt|simple 3 wheels robot]] we used for socket communication. You can rename it 
**myLittleBotRos.ttt**.

{{:vrep:mylittlebot_view.png?300|}}


**Before staring V-REP make sure you have started ROS**, for example by executing **roscore** in a terminal.
If you start V-REP in another terminal with **./vrep.sh**, if ROS V-REP interface is ok, the following message will be displayed on the V-REP terminal : 
<code text>
Plugin 'RosInterface': loading...
Plugin 'RosInterface': warning: replaced variable 'simROS'
Plugin 'RosInterface': load succeeded.
</code>

The first thing to do is to add a **non threaded script** to an object in the scene. We can choose to add it to the **MainBody** object but it can be any other object in the scene :

{{:vrep:mylittlebot_scene_hierarchy.png?300|}}

Right-Clic on the small icon on the left of **MainBody** and select Add->Associated child script->Non threaded.
After this a black icon showing a text file should appear on the right of **MainBody** :


{{:vrep:mylittlebot_script_non_threaded.png?300|}}


To edit, double-clic on this icon. You can edit directly in the V-REP script window. However, if you prefer using you preferred editor, you can remove all the Lua text code in the script and just type one line :
<code lua>
require ("simple_robot_ros_control")
</code> 
Then you will have to create a file called  **simple_robot_ros_control.lua** in the directory where your scene file (here **myLittleBotRos.ttt**) is stored. 


The Lua code (see full code below) is almost self-explaining when knowing the basic principles of ROS. However, we will quickly recall what we need to do to interface our V-REP robot to ROS.

The first function to define is **sysCall_init()**. Here we start defining the handles to access the objects in the scene. We need to know the handles of **MainBody** to get the pose of the robot and of  **MotorLeft** and **MotorRight** to drive the wheels. The we init the ROS interface. Finally the publishers and the subscribers are defined. **simROS.advertise(topic,message)** creates a publisher for a topic with a given message ROS format. Naming of topics and message format is the same as in ROS.
The last thing to do is to define the subscribers. Here we have only one subscriber to the /cmd_vel topic. as in ROS we need to define a callback function to process the topics when they arrive.
The function **subscriber_cmd_vel_callback(msg)** is the callback function that is executed each time a /cmd_vel topic is received by V-REP. This example shows how to handle a topic message in Lua.

After that we define **sysCall_cleanup()** to correctly stop the publishers and the subscribers.

The main loop is done with **sysCall_actuation()** that publishes 2 topics : **/pose** and **/simulationTime** and a transformation **/tf** indicating the pose of **MainBody** in world coordinates.

When all is programmed, the V-REP simulation is started.

In a terminal standard ROS command *rostopic* can be used to check if topics are correctly published :
<code bash>
rostopic list
/cmd_vel
/pose
/rosout
/rosout_agg
/simulationTime
/tf
</code>

To control the robot in V-REP we need to define the **/cmd_vel** topic. We can do it manually with **rostopic pub** :
<code bash>
rostopic pub /cmd_vel geometry_msgs/Twist -r 10 -- '[0.2, 0.0, 0.0]' '[0.0, 0.0, 0.6]'
</code>

Or we can do it graphically with **rqt** by selecting Plugins->Robot Tools-> Robot Steering

The Lua code [[https://www.ensta-bretagne.fr/zerr/filerepo/vrep/simple_robot_ros_control.lua|simple_robot_ros_control.lua]] is as follows:
<code lua>
function subscriber_cmd_vel_callback(msg)
   -- This is the subscriber callback function when receiving /cmd_vel  topic
   -- The msg is a Lua table defining linear and angular velocities
   --   linear velocity along x = msg["linear"]["x"]
   --   linear velocity along y = msg["linear"]["y"]
   --   linear velocity along z = msg["linear"]["z"]
   --   angular velocity along x = msg["angular"]["x"]
   --   angular velocity along y = msg["angular"]["y"]
   --   angular velocity along z = msg["angular"]["z"]
   spdLin = msg["linear"]["x"]
   spdAng = msg["angular"]["z"]
   kLin = -0.5
   kAng = -0.2
   spdLeft = kLin*spdLin+kAng*spdAng
   spdRight = kLin*spdLin-kAng*spdAng
   sim.setJointTargetVelocity(leftMotor,spdLeft)
   sim.setJointTargetVelocity(rightMotor,spdRight)
   sim.addStatusbarMessage('cmd_vel subscriber receiver : spdLin ='..spdLin..',spdAng='..spdAng.." command : spdLeft="..spdLeft..",act="..spdRight)
end

function getPose(objectName)
   -- This function get the object pose at ROS format geometry_msgs/Pose
   objectHandle=sim.getObjectHandle(objectName)
   relTo = -1
   p=sim.getObjectPosition(objectHandle,relTo)
   o=sim.getObjectQuaternion(objectHandle,relTo)
   return {
      position={x=p[1],y=p[2],z=p[3]},
      orientation={x=o[1],y=o[2],z=o[3],w=o[4]}
   }
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 sysCall_init()
   -- The child script initialization
   objectName="MainBody"
   objectHandle=sim.getObjectHandle(objectName)
   -- get left and right motors handles
   leftMotor = sim.getObjectHandle("MotorLeft")
   rightMotor = sim.getObjectHandle("MotorRight")
   rosInterfacePresent=simROS
   -- Prepare the publishers and subscribers :
   if rosInterfacePresent then
      publisher1=simROS.advertise('/simulationTime','std_msgs/Float32')
      publisher2=simROS.advertise('/pose','geometry_msgs/Pose')
      subscriber1=simROS.subscribe('/cmd_vel','geometry_msgs/Twist','subscriber_cmd_vel_callback')
   end
end

function sysCall_actuation()
   -- Send an updated simulation time message, and send the transform of the object attached to this script:
   if rosInterfacePresent then
      -- publish time and pose topics
      simROS.publish(publisher1,{data=sim.getSimulationTime()})
      simROS.publish(publisher2,getPose("MainBody"))
      -- send a TF
      simROS.sendTransform(getTransformStamped(objectHandle,objectName,-1,'world'))
      -- To send several transforms at once, use simROS.sendTransforms instead
   end
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(publisher1)
        simROS.shutdownPublisher(publisher2)
        simROS.shutdownSubscriber(subscriber1)
    end
end
</code>

Now let's try adding some sensors that we will publish to ROS topics. The first we will add is a camera. 
  * In V-REP menu, select Add->Vision Sensor->Perspective Type
  * Rename it as you want, here we call it **OnboardCamera**
  * Set the correct position and orienation on the vehicle
  * Double-clic on the icon on the left of **OnboardCamera** to access the properties
  * Change the resolution to what you need (320x200 is for example the resolution NAO's cameras), don't take care of the warning abour power of 2
  * Select the **OnboardCamera**,  right-clic to add a **Non Threaded Script**. A black icon with the shape of a file should appear on the right of **OnboardCamera** in the scene tree.
  * Double clic on this icon, remove the text and type : <code>require ("onboard_camera") </code>
  * Create the file **onboard_camera.lua** that we will use to publish the camera stream to ROS
  * To see the camera on V-REP, right-clic in the main view to add a **floating view", inside the **floating view** select the view from the onboard camera

The publishing code looks like this
<code lua>
-- This illustrates how to publish and subscribe to an image using the ROS Interface. 

function sysCall_init()        
    -- Get some handles (as usual !):
    onboard_camera=sim.getObjectHandle('OnboardCamera')    
    -- Enable an image publisher and subscriber:
    pub=simROS.advertise('/image', 'sensor_msgs/Image')
    simROS.publisherTreatUInt8ArrayAsString(pub) -- treat uint8 arrays as strings (much faster, tables/arrays are kind of slow in Lua)    
end

function sysCall_sensing()
    -- Publish the image of the vision sensor:
    local data,w,h=sim.getVisionSensorCharImage(onboard_camera)
    sim.transformImage(data,{w,h},4) -- vertical flip
    d={}
    d['header']={seq=0,stamp=simROS.getTime(), frame_id="a"}
    d['height']=h
    d['width']=w
    d['encoding']='rgb8'
    d['is_bigendian']=1
    d['step']=w*3
    d['data']=data
    simROS.publish(pub,d)
end

function sysCall_cleanup()
    -- Shut down publisher and subscriber. Not really needed from a simulation script (automatic shutdown)
    simROS.shutdownPublisher(pub)
end
</code>

Back to [[vrep:ros-com-with-robot|top]].