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: | ||
+ | |||
+ | The robot inside V-REP can be controlled using the Robot Operating System [[https:// | ||
+ | All documentation is available on [[https:// | ||
+ | |||
+ | 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:// | ||
+ | **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 **./ | ||
+ | <code text> | ||
+ | Plugin ' | ||
+ | Plugin ' | ||
+ | Plugin ' | ||
+ | </ | ||
+ | |||
+ | 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-> | ||
+ | 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 : | ||
+ | <code lua> | ||
+ | require (" | ||
+ | </ | ||
+ | Then you will have to create a file called | ||
+ | |||
+ | |||
+ | 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, | ||
+ | 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 **/ | ||
+ | |||
+ | 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 | ||
+ | / | ||
+ | /tf | ||
+ | </ | ||
+ | |||
+ | To control the robot in V-REP we need to define the **/ | ||
+ | <code bash> | ||
+ | rostopic pub /cmd_vel geometry_msgs/ | ||
+ | </ | ||
+ | |||
+ | Or we can do it graphically with **rqt** by selecting Plugins-> | ||
+ | |||
+ | The Lua code [[https:// | ||
+ | <code lua> | ||
+ | function subscriber_cmd_vel_callback(msg) | ||
+ | -- This is the subscriber callback function when receiving / | ||
+ | -- The msg is a Lua table defining linear and angular velocities | ||
+ | | ||
+ | | ||
+ | | ||
+ | | ||
+ | | ||
+ | | ||
+ | | ||
+ | | ||
+ | kLin = -0.5 | ||
+ | kAng = -0.2 | ||
+ | | ||
+ | | ||
+ | | ||
+ | | ||
+ | | ||
+ | end | ||
+ | |||
+ | function getPose(objectName) | ||
+ | -- This function get the object pose at ROS format geometry_msgs/ | ||
+ | | ||
+ | relTo = -1 | ||
+ | | ||
+ | | ||
+ | | ||
+ | position={x=p[1], | ||
+ | orientation={x=o[1], | ||
+ | } | ||
+ | end | ||
+ | |||
+ | function getTransformStamped(objHandle, | ||
+ | -- This function retrieves the stamped transform for a specific object | ||
+ | | ||
+ | | ||
+ | | ||
+ | | ||
+ | header={ | ||
+ | stamp=t, | ||
+ | frame_id=relToName | ||
+ | }, | ||
+ | child_frame_id=name, | ||
+ | transform={ | ||
+ | translation={x=p[1], | ||
+ | rotation={x=o[1], | ||
+ | } | ||
+ | } | ||
+ | end | ||
+ | |||
+ | function sysCall_init() | ||
+ | -- The child script initialization | ||
+ | | ||
+ | | ||
+ | -- get left and right motors handles | ||
+ | | ||
+ | | ||
+ | | ||
+ | -- Prepare the publishers and subscribers : | ||
+ | if rosInterfacePresent then | ||
+ | publisher1=simROS.advertise('/ | ||
+ | publisher2=simROS.advertise('/ | ||
+ | subscriber1=simROS.subscribe('/ | ||
+ | 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, | ||
+ | simROS.publish(publisher2, | ||
+ | -- send a TF | ||
+ | simROS.sendTransform(getTransformStamped(objectHandle, | ||
+ | -- 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-> | ||
+ | * 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**, | ||
+ | * Double clic on this icon, remove the text and type : < | ||
+ | * 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(' | ||
+ | -- Enable an image publisher and subscriber: | ||
+ | pub=simROS.advertise('/ | ||
+ | simROS.publisherTreatUInt8ArrayAsString(pub) -- treat uint8 arrays as strings (much faster, tables/ | ||
+ | end | ||
+ | |||
+ | function sysCall_sensing() | ||
+ | -- Publish the image of the vision sensor: | ||
+ | local data, | ||
+ | sim.transformImage(data, | ||
+ | d={} | ||
+ | d[' | ||
+ | d[' | ||
+ | d[' | ||
+ | d[' | ||
+ | d[' | ||
+ | d[' | ||
+ | d[' | ||
+ | simROS.publish(pub, | ||
+ | end | ||
+ | |||
+ | function sysCall_cleanup() | ||
+ | -- Shut down publisher and subscriber. Not really needed from a simulation script (automatic shutdown) | ||
+ | simROS.shutdownPublisher(pub) | ||
+ | end | ||
+ | </ | ||
+ | |||
+ | Back to [[vrep: | ||
+ | |||