User Tools

Site Tools


vrep:ros-com-with-robot

Differences

This shows you the differences between two versions of the page.


vrep:ros-com-with-robot [2023/03/31 12:14] (current) – created - external edit 127.0.0.1
Line 1: Line 1:
 +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]].
 +