50 lines
1.5 KiB
Python
50 lines
1.5 KiB
Python
|
#!/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()
|
||
|
|
|