text: import sys, time
  from pdb import set_trace
  
  sys.path.insert(0, '/Users/luizeduardocartolano/Dropbox/DUDU/Unicamp/IC/MC906/workspace/robot-fuzzy/vrep-robot-master/lib')
  import vrep
  
  class Robot():
      def __init__(self):
          self.ROBOT_WIDTH = 0.381
          self.WHEEL_RADIUS = 0.195/2.0
          self.SERVER_IP = "127.0.0.1"
          self.SERVER_PORT = 25000
          self.clientID = self.start_sim()
          self.us_handle, self.vision_handle, self.laser_handle = self.start_sensors()
          self.motors_handle = self.start_motors()
          self.robot_handle = self.start_robot()
  
      def start_sim(self):
          """
              Function to start the simulation. The scene must be running before running this code.
              Returns:
                  clientID: This ID is used to start the objects on the scene.
          """
          vrep.simxFinish(-1)
          clientID = vrep.simxStart(self.SERVER_IP, self.SERVER_PORT, True, True, 2000, 5)
          if clientID != -1:
              print("Connected to remoteApi server.")
          else:
              vrep.simxFinish(clientID)
              sys.exit("\033[91m ERROR: Unable to connect to remoteApi server. Consider running scene before executing script.")
  
          return clientID
  
      def get_connection_status(self):
          """
              Function to inform if the connection with the server is active.
              Returns:
                  connectionId: -1 if the client is not connected to the server.
                  Different connection IDs indicate temporary disconections in-between.
          """
          return vrep.simxGetConnectionId(self.clientID)
  
      def start_sensors(self):
          """
              Function to start the sensors.
              Returns:
                  us_handle: List that contains each ultrassonic sensor handle ID.
                  vision_handle: Contains the vision sensor handle ID.
                  laser_handle: Contains the laser handle ID.
          """
          #Starting ultrassonic sensors
          us_handle = []
          sensor_name=[]
          for i in range(0,16):
              sensor_name.append("Pioneer_p3dx_ultrasonicSensor" + str(i+1))
  
              res, handle = vrep.simxGetObjectHandle(self.clientID, sensor_name[i], vrep.simx_opmode_oneshot_wait)
              if(res != vrep.simx_return_ok):
                  print ("\033[93m "+ sensor_name[i] + " not connected.")
              else:
                  print ("\033[92m "+ sensor_name[i] + " connected.")
                  us_handle.append(handle)
  
          #Starting vision sensor
          res, vision_handle = vrep.simxGetObjectHandle(self.clientID, "Vision_sensor", vrep.simx_opmode_oneshot_wait)
          if(res != vrep.simx_return_ok):
              print ("\033[93m Vision sensor not connected.")
          else:
              print ("\033[92m Vision sensor connected.")
  
          #Starting laser sensor
          res, laser_handle = vrep.simxGetObjectHandle(self.clientID, "fastHokuyo", vrep.simx_opmode_oneshot_wait)
          if(res != vrep.simx_return_ok):
              print ("\033[93m Laser not connected.")
          else:
              print ("\033[92m Laser connected.")
  
          return us_handle, vision_handle, laser_handle
  
      def start_motors(self):
          """
              Function to start the motors.
              Returns:
                  A dictionary that contains both motors handle ID.
          """
  
          res, left_handle = vrep.simxGetObjectHandle(self.clientID, "Pioneer_p3dx_leftMotor", vrep.simx_opmode_oneshot_wait)
          if(res != vrep.simx_return_ok):
              print("\033[93m Left motor not connected.")
          else:
              print("\033[92m Left motor connected.")
  
          res, right_handle = vrep.simxGetObjectHandle(self.clientID, "Pioneer_p3dx_rightMotor", vrep.simx_opmode_oneshot_wait)
          if(res != vrep.simx_return_ok):
              print("\033[93m Right motor not connected.")
          else:
              print("\033[92m Right motor connected.")
  
          return {"left": left_handle, "right":right_handle}
  
      def start_robot(self):
          """
              Function to start the robot.
              Returns:
                  robot_handle: Contains the robot handle ID.
          """
          res, robot_handle = vrep.simxGetObjectHandle(self.clientID, "Pioneer_p3dx", vrep.simx_opmode_oneshot_wait)
          if(res != vrep.simx_return_ok):
              print("\033[93m Robot not connected.")
          else:
              print("\033[92m Robot connected.")
  
          return robot_handle
  
      def read_ultrassonic_sensors(self):
          """
              Reads the distances from the 16 ultrassonic sensors.
              Returns:
                  distances: List with the distances in meters.
          """
          distances = []
          noDetectionDist = 5.0 #Here we define the maximum distance as 5 meters
  
          for sensor in self.us_handle:
              res, status, distance,_,_ = vrep.simxReadProximitySensor(self.clientID, sensor, vrep.simx_opmode_streaming)
              while(res != vrep.simx_return_ok):
                  res, status, distance,_,_ = vrep.simxReadProximitySensor(self.clientID, sensor, vrep.simx_opmode_buffer)
  
              if(status != 0):
                  distances.append(distance[2])
              else:
                  distances.append(noDetectionDist)
  
  
          return distances
  
      def read_vision_sensor(self):
          """
              Reads the image raw data from vrep vision sensor.
              Returns:
                  resolution: Tuple with the image resolution.
                  image: List with the image data.
          """
          res, resolution, image = vrep.simxGetVisionSensorImage(self.clientID, self.vision_handle, 0, vrep.simx_opmode_streaming)
          while(res != vrep.simx_return_ok):
              res, resolution, image = vrep.simxGetVisionSensorImage(self.clientID, self.vision_handle, 0, vrep.simx_opmode_buffer)
  
          return resolution, image
  
      def read_laser(self):
          """
              Gets the 572 points read by the laser sensor. Each reading contains 3 values (x, y, z) of the point relative to the sensor position.
              Returns:
                  laser: List with 1716 values of x, y and z from each point.
          """
          res, laser = vrep.simxGetStringSignal(self.clientID,"LasermeasuredDataAtThisTime", vrep.simx_opmode_streaming)
          laser = vrep.simxUnpackFloats(laser)
          while(res != vrep.simx_return_ok):
              res, laser = vrep.simxGetStringSignal(self.clientID,"LasermeasuredDataAtThisTime", vrep.simx_opmode_buffer)
              laser = vrep.simxUnpackFloats(laser)
  
          return laser
  
      def stop(self):
          """
              Sets the motors velocities to 0.
          """
          vrep.simxSetJointTargetVelocity(self.clientID, self.motors_handle["left"], 0, vrep.simx_opmode_streaming)
          vrep.simxSetJointTargetVelocity(self.clientID, self.motors_handle["right"], 0, vrep.simx_opmode_streaming)
          time.sleep(0.1)
  
      def set_left_velocity(self, vel):
          """
              Sets the velocity on the left motor.
              Args:
                  vel: The velocity to be applied in the motor (rad/s)
          """
          vrep.simxSetJointTargetVelocity(self.clientID, self.motors_handle["left"], vel, vrep.simx_opmode_streaming)
  
      def set_right_velocity(self, vel):
          """
              Sets the velocity on the right motor.
              Args:
                  vel: The velocity to be applied in the motor (rad/s)
          """
          vrep.simxSetJointTargetVelocity(self.clientID, self.motors_handle["right"], vel, vrep.simx_opmode_streaming)
  
      def set_velocity(self, V, W):
          """
              Sets a linear and a angular velocity on the robot.
              Args:
                  V: Linear velocity (m/s) to be applied on the robot along its longitudinal axis.
                  W: Angular velocity (rad/s) to be applied on the robot along its axis of rotation, positive in the counter-clockwise direction.
          """
          left_vel = (V - W*(self.ROBOT_WIDTH/2))/self.WHEEL_RADIUS
          right_vel = (V + W*(self.ROBOT_WIDTH/2))/self.WHEEL_RADIUS
          vrep.simxSetJointTargetVelocity(self.clientID, self.motors_handle["left"], left_vel, vrep.simx_opmode_streaming)
          vrep.simxSetJointTargetVelocity(self.clientID, self.motors_handle["right"], right_vel, vrep.simx_opmode_streaming)
  
      def get_current_position(self):
          """
              Gives the current robot position on the environment.
              Returns:
                  position: Array with the (x,y,z) coordinates.
          """
          res, position = vrep.simxGetObjectPosition(self.clientID, self.robot_handle, -1, vrep.simx_opmode_streaming)
          while(res != vrep.simx_return_ok):
              res, position = vrep.simxGetObjectPosition(self.clientID, self.robot_handle, -1, vrep.simx_opmode_streaming)
  
          return position
  
      def get_current_orientation(self):
          """
              Gives the current robot orientation on the environment.
              Returns:
                  orientation: Array with the euler angles (alpha, beta and gamma).
          """
          res, orientation = vrep.simxGetObjectOrientation(self.clientID, self.robot_handle, -1, vrep.simx_opmode_streaming)
          while(res != vrep.simx_return_ok):
              res, orientation = vrep.simxGetObjectOrientation(self.clientID, self.robot_handle, -1, vrep.simx_opmode_streaming)
  
          return orientation
  