#!/usr/bin/env python
# Bamako

# Andrei COROCAESCU, Théophile LEMARIÉ
# 02.12.2019

import rospy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
import sys, tty, termios
from cv_bridge import CvBridge, CvBridgeError
import numpy as np
import cv2
import cv2.cv as cv
import math
import numpy as np

# noeud publiant sur le topic /cmd_vel_mux[...] pour contrôler la vitesse du robot 
velocity_publisher = rospy.Publisher("/cmd_vel_mux/input/teleop", Twist, queue_size=10)

# callback permettant de réagir aux obstacles 
def callback(msg):
    vel_msg = Twist()  

    # valeurs par défaut, dans le cas où aucun obstacle n'est détecté 
    vel_msg.linear.x = 0.25
    vel_msg.angular.z = 0.0

    # extraction des données de la kinect 
    ranges  = msg.ranges
    for i in ranges[100:500]: 
        # condition d'obstacle 
        if (i < 1.0 and i is not float("nan")): # obstacle trouvé devant, dans un angle de 30° environ
            vel_msg.linear.x = 0.05 # diminuer la vitesse linéaire 
            vel_msg.angular.z = 1.5 # tourner à gauche 
            print("OBSTACLE") 
            break  
    
    # envoyer les commande de vitesse / rotation 
    velocity_publisher.publish(vel_msg)
    

if __name__ == '__main__':
    # déclaration des noeuds 
    # noeud d'écoute sur le topic /scan (données de profondeur de la kinect) 
    rospy.init_node('listener', anonymous=True)
    rospy.Subscriber("/scan", LaserScan, callback)
    rospy.spin()