Creating & Programming Robots

User Tools

Site Tools

Trace: • ros-com-with-robot
vrep:ros-com-with-robot

Back to V-REP Main Page

The robot inside V-REP can be controlled using the Robot Operating System ROS. All documentation is available on 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 simple 3 wheels robot we used for socket communication. You can rename it myLittleBotRos.ttt.

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 : 

Plugin 'RosInterface': loading...
Plugin 'RosInterface': warning: replaced variable 'simROS'
Plugin 'RosInterface': load succeeded.

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 :

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 :

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 : 

require ("simple_robot_ros_control")

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 : 

rostopic list
/cmd_vel
/pose
/rosout
/rosout_agg
/simulationTime
/tf

To control the robot in V-REP we need to define the /cmd_vel topic. We can do it manually with rostopic pub : 

rostopic pub /cmd_vel geometry_msgs/Twist -r 10 -- '[0.2, 0.0, 0.0]' '[0.0, 0.0, 0.6]'

Or we can do it graphically with rqt by selecting Plugins→Robot Tools→ Robot Steering

The Lua code simple_robot_ros_control.lua is as follows: 

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

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 (320×200 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 : 
    require ("onboard_camera")
  • 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 

-- 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

Back to top.

vrep/ros-com-with-robot.txt · Last modified: 2023/03/31 12:14 by 127.0.0.1