User Tools

Site Tools


vrep:socket-com-with-robot

Differences

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


vrep:socket-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 socket communication allows for an external program to control the robot inside V-REP. The external program is a Python script. We will use for the socket a server/client communication. The server is written in Lua inside a threaded script in V-REP. The client is written in Python, It will :
 +  * connect to the server
 +  * send commands to the robot
 +  * read data from the robot (sensors, status, ...)
 +  * disconnect from the server
 +
 +On the server side of the socket, we use a [[https://www.coppeliarobotics.com/helpFiles/en/childScripts.htm|threaded script]] in Lua as we want to listen to the client's commands synchronously with the V-REP simulation loop.
 +
 +We will use a  [[https://www.ensta-bretagne.fr/zerr/filerepo/vrep/myLittleBotNoScript.ttt|simple 3 wheels robot]] and we load it in V-REP. You can rename it **myLittleBot.ttt**.
 +
 +{{:vrep:mylittlebot_view.png?300|}}
 +
 +
 +The first thing to do is to add a threaded script to a object. 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->Threaded.
 +After this a blue icon showing a text file should appear on the right of **MainBody** :
 +
 +
 +{{:vrep:mylittlebot_script_threaded.png?300|}}
 +
 +
 +To edit, double-clic on this icon. You can edit directely 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_control_bot")
 +</code> 
 +Then you will have to create a file called  **simple_robot_control.lua** in the directory where your scene file (here **myLittleBot.ttt**) is stored. 
 +
 +The [[https://www.ensta-bretagne.fr/zerr/filerepo/vrep/simple_robot_control.lua|Lua code]] looks like this :
 +<code lua>
 +-- load pack library
 +-- must have been added previoulsy in V-REP folder
 +-- if not done yet, see tutorial ...
 +require ("pack"
 +bpack=string.pack
 +bunpack=string.unpack
 +
 +-- define socket
 +portNb = 33211
 +serverOn = false
 +connexionTimeout = 0.001 -- set 1 ms time out
 +socket=require("socket")
 +srv = nil
 +clt1 = nil
 +nCharMessage = 10
 +
 +function sysCall_threadmain()
 +   -- initialization
 +   -- get left and right motors handles
 +   leftMotor = sim.getObjectHandle("MotorLeft")
 +   rightMotor = sim.getObjectHandle("MotorRight")
 +   -- control loop
 +   while true do
 +      -- get simulation time at beginning of the loop
 +      simTime = sim.getSimulationTime()
 +      -- if server is not running, start it
 +      if not serverOn then
 + printToConsole ("not connected")
 + srv = assert(socket.bind('127.0.0.1',portNb))
 + if (srv==nil) then
 +     printToConsole ("bad connect")
 + else
 +     printToConsole ("get socket")
 +     ip, port = srv:getsockname()
 +     printToConsole ("server ok at "..ip.." on port "..port)
 +     serverOn = true
 +     printToConsole ("connexion granted !!! ")
 + end
 +      end
 +      -- if server is running, accept command and send back status
 +      if serverOn then
 + -- to prevent lock on accept, a timeout is set
 + srv:settimeout(connexionTimeout)
 + clt1 = srv:accept()
 + if clt1 ~= nil then
 +     -- to prevent lock, a timeout is set
 +     --clt1:settimeout(connexionTimeout)
 +     -- get the data (command) from the client
 +     dataIn = clt1:receive(nCharMessage)
 +     if dataIn ~= nil then
 +        -- unpack the received data with following format :
 +        --   char : first synchro char 'A' 
 +        --   char : second synchro char 'Z' 
 +        --   float : speed left
 +        --   float : speed right
 +        nrd1,ch1,ch2,speedLeft,speedRight = bunpack(dataIn,"AAff")
 +        if ch1 ~= 'A' or ch2 ~= 'Z' then
 +   print ("bad data from py client ...")
 +        else
 +   -- apply the command
 +   sim.setJointTargetVelocity(leftMotor,speedLeft)
 +   sim.setJointTargetVelocity(rightMotor,speedRight)
 +        end
 +        -- prepare the status to send back  the client :
 +        -- simulation time and wheel angular position (radians)
 +        wheelAngleLeft =  sim.getJointPosition(leftMotor)
 +        wheelAngleRight = sim.getJointPosition(rightMotor)
 +        ch1 = 'A' -- sync code 1
 +        ch2 = 'Z' -- sync code 2
 +        -- pack the status data
 +        dataPacked = bpack("AAfff",ch1,ch2,simTime,wheelAngleLeft,wheelAngleRight)
 +        -- Send the status data back to the client:
 +        clt1:send(dataPacked)  
 +     else
 +        printToConsole ("no data")
 +     end
 +     clt1:close()
 + end
 +      end
 +   end
 +end
 +
 +function sysCall_cleanup()
 +    -- Put some clean-up code here
 +end
 +</code>
 +
 +The socket transfers binary data, so we need to pack the data before sending them and unpack them at the reception. For Lua we use the [[http://webserver2.tecgraf.puc-rio.br/~lhf/ftp/lua/|lpack library]]. You can either download the code and recompile the **pack.so** shared library or get it  [[https://www.ensta-bretagne.fr/zerr/filerepo/vrep/pack.so|already compiled here]] if you work on x86_64 linux computer. The file **pack.so** should be added in the main V-REP directory. 
 +
 +//Todo : provide a .dll for windows//
 +
 +The socket is between processes on the same computer so the IP address is **localhost** or **127.0.0.1**. The communication port is set to **33211**. The port number is arbitrary, if this port is already used, we will get an error message. To get a list of used port, you can use **lsof**. Here we print only the ports used by V-REP: 
 +<code bash>
 +sudo lsof -i -P -n | grep LISTEN | grep vrep
 +[sudo] password for user: 
 +vrep      13871          newubu   29u  IPv4 17679486      0t0  TCP *:19997 (LISTEN)
 +vrep      13871          newubu   38u  IPv4 20187130      0t0  TCP 127.0.0.1:33211 (LISTEN)
 +</code>
 +
 +The Lua server expects to receive a command from the Python client with 4 values :
 +  * character A (1 byte)
 +  * character Z (1 byte)
 +  * float speed_left (4 bytes)
 +  * float speed_right (4 bytes)
 +It has first to unpack these data with **bunpack()** function. Then it checks if the two first values are 'A' and 'Z'. This is just to ensures that the connection is synchronized properly. In principle, TCP/IP sockets already check if all is OK, so may no used it and transmit only the usefull values.
 +
 +If the message received is OK (starting with 'A' and 'Z'), the Lua server applies the command to the left and right motors.
 +
 +Finally it reads the status data. Here the status has 3 values : the simulation time and the angular position of the two wheels. The angular position of the wheels is very useful if we want to simulate odometers. The status message is made of five values :
 +  * character A (1 byte)
 +  * character Z (1 byte)
 +  * float simulation time (4 bytes)
 +  * float angular_position_wheel_left (4 bytes)
 +  * float angular_position_wheel_right (4 bytes)
 +The 5 values are packed with **bpack()** function and sent back to the python client.
 +
 +The Python3 code of the client controls the robot by sending the motor speed for the two wheels. It can be executed via a terminal. For example to set the left motor speed to 0.5 and the right to -0.5, type:
 +<code bash>
 +python3 simple_robot_control.py 0.5 -0.5
 +sockect time=6.8 ms, simTime=4.85 s, left angle=-0.0, right angle=0.0
 +sockect time=3.9 ms, simTime=4.95 s, left angle=2.4, right angle=-2.4
 +sockect time=6.7 ms, simTime=4.95 s, left angle=3.8, right angle=-3.8
 +sockect time=7.0 ms, simTime=5.05 s, left angle=5.2, right angle=-5.2
 +sockect time=0.4 ms, simTime=5.10 s, left angle=8.0, right angle=-8.0
 +</code>
 +The robot will turn in place to the right.  
 +
 +Do not forget to start the simulation in V-REP before executing the Python client code. Otherwise, the communication socket will not work :
 +<code bash>
 +python3 simple_robot_control.py 0.5 -0.5
 +Cannot connect to server
 +</code>
 +
 +The [[https://www.ensta-bretagne.fr/zerr/filerepo/vrep/simple_robot_control.py|Python3 client code]] is as follows:
 +<code python>
 +import socket
 +import time
 +import struct
 +import signal
 +import sys
 +import math
 +
 +code_sync_1 = 'A'
 +code_sync_2 = 'Z'
 +
 +# set the speed of the two wheels
 +try:
 +    speed_left = float(sys.argv[1])
 +    speed_right = float(sys.argv[2])
 +except:
 +    speed_left = 0.0
 +    speed_right = 0.0
 +
 +# set address and port for the socket communication
 +server_address_port = ('localhost', 33211)
 +
 +# interrupt handler
 +def interrupt_handler(signal, frame):
 +    print ('You pressed CTRL-C, end of communication with V-REP robot')
 +    time.sleep(0.5)
 +    try:
 +        sock.close()
 +    except:
 +        print (sock,"socket already closed")
 +    sys.exit(0)
 +    
 +# trap hit of ctrl-C to stop the communication with V-REP
 +signal.signal(signal.SIGINT, interrupt_handler)
 +
 +# client connects to server every 50 ms
 +loop_duration = 0.050
 +while True:
 +    # Create a TCP/IP socket
 +    t0 = time.time()
 +    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
 +    try:
 +        sock.connect(server_address_port)
 +    except:
 +        print ("Cannot connect to server")
 +        break
 +
 +    sock.settimeout(0.1) # set connection timeout
 +    # pack command
 +    strSend = struct.pack('<BBff',ord(code_sync_1),ord(code_sync_2),speed_left,speed_right)
 +    # send command to V-REP
 +    sock.sendall(strSend)
 +
 +    # wait for status back from V-REP
 +    data = b''
 +    nch_rx = 14 # expect receiving 14 bytes from  V-REP 
 +    try:
 +        while len(data) < nch_rx:
 +            data += sock.recv(nch_rx)
 +    except:
 +        print ("socker error , duration is %f ms, try to reconnect !!!"%((time.time() - t0)*1000.0))
 +    # unpack the received data
 +    data_ok = False
 +    if len(data) == nch_rx:
 +        vrx = struct.unpack('<ccfff',data)
 +        if vrx[0] == b'A' and  vrx[1] == b'Z':
 +            data_ok = True
 +    else:
 +        pass
 +
 +    sock.close()
 +    tsock = (time.time() - t0)*1000.0
 +
 +    if data_ok:
 +        print ("sockect time=%.1f ms, simTime=%.2f s, left angle=%.1f, right angle=%.1f"%
 +               (tsock,vrx[2],vrx[3]*180.0/math.pi,vrx[4]*180.0/math.pi))
 +    else:
 +        print ("tsock",tsock,"ms, bad data !!")
 +
 +    tsleep = loop_duration - (time.time()-t0)
 +    if tsleep>0:
 +        time.sleep (tsleep)  
 +</code>
 +
 +Go to [[vrep:socket-com-with-robot|top]]