Ce serveur Gitlab sera éteint le 30 juin 2020, pensez à migrer vos projets vers les serveurs gitlab-research.centralesupelec.fr et gitlab-student.centralesupelec.fr !

Commit 0e6802e0 authored by jad's avatar jad

merge done

parent de881130
#!/usr/bin/env python
"""
This node is the sequencer, This node will subscribe to the topic mode ,
and will choose the nodes to activate sequentially
This node is the sequencer, This node will subscribe to the topic mode , and will choose the nodes to activate sequentially
"""
import rospy
from std_msgs.msg import String,Int32,Float32
......@@ -11,8 +10,7 @@ import sys
class Sequencer(object):
def __init__(self):
self.mode_sub = rospy.Subscriber("/mode",String,self.read_mode)
self.mode_pub = rospy.Publisher("/mode",String,queue_size=1)
self.mode_sub = rospy.Subscriber("/mode",String,self.read_mode) self.mode_pub = rospy.Publisher("/mode",String,queue_size=1)
self.actv_publisher = rospy.Publisher("/activations",actMsg,queue_size=1)
self.loop_pub = rospy.Publisher("/loop_seq",Int32,queue_size=1)
self.loop_sub = rospy.Subscriber("/loop_seq",Int32,self.enter_loop)
......@@ -24,9 +22,12 @@ class Sequencer(object):
self.doors_seq_list = ["doors",[["curveMotion"],["curveMotion"]],[["checkDoors","turnAng"],["checkDoors"]]]
self.init_seq_list = ["init",[ ["resetCmd"],[] ] ]
self.hallway_seq_list = ["hallway",[["detectVanish","computeTarX","computeTarY","computeTarZ"], [""] ] ]
self.stairs_seq_list = ["stairs",[["image_processor_drone_vs_stairs_round1","drone_vs_stairs_round1"],["drone_vs_stairs_round1"]],[["image_processor_drone_vs_stairs_round2","drone_vs_stairs_round2"],[""]]]
self.doors_seq = Sequence(self.doors_seq_list)
self.init_seq = Sequence(self.init_seq_list)
self.hallways_seq = Sequence(self.hallway_seq_list)
self.stairs_seq = Sequence(self.stairs_seq_list)
#self.new_seq_list = []
#self.new_seq = Sequence(self.new_seq_list)
......@@ -40,6 +41,7 @@ class Sequencer(object):
self.actv_publisher.publish(msg)
self.doors_seq = Sequence(self.doors_seq_list)
self.hallways_seq = Sequence(self.hallway_seq_list)
self.stairs_seq = Sequence(self.stairs_seq_list)
#self.new_seq = Sequence(self.new_seq_list)
else:
self.loop_pub.publish(3)
......@@ -53,6 +55,8 @@ class Sequencer(object):
#self.new_seq.seq_fun()
if(self.mode == self.hallways_seq.get_mode()):
self.hallways_seq.seq_fun()
if(self.mode == self.stairs_seq.get_mode()):
self.stairs_seq.seq_fun()
def main(args):
rospy.init_node('Sequencer', anonymous=True)
sc = Sequencer()
......
#!/usr/bin/env python
"""
This node is the sequencer, This node will subscribe to the topic mode ,
and will choose the nodes to activate sequentially
"""
import rospy
from std_msgs.msg import String,Int32,Float32
from projectTools import Sequence
from drone_project.msg import actMsg
import sys
class Sequencer(object):
def __init__(self):
self.mode_sub = rospy.Subscriber("/mode",String,self.read_mode)
self.mode_pub = rospy.Publisher("/mode",String,queue_size=1)
self.actv_publisher = rospy.Publisher("/activations",actMsg,queue_size=1)
self.loop_pub = rospy.Publisher("/loop_seq",Int32,queue_size=1)
self.loop_sub = rospy.Subscriber("/loop_seq",Int32,self.enter_loop)
self.mode = "init"
"""
Init should be a list and not a name
"""
self.doors_seq_list = ["doors",[["curveMotion"],["curveMotion"]],[["checkDoors","turnAng"],["checkDoors"]]]
self.init_seq_list = ["init",[ ["resetCmd"],[] ] ]
self.hallway_seq_list = ["hallway",[["detectVanish","computeTarX","computeTarY","computeTarZ"], [""] ] ]
self.doors_seq = Sequence(self.doors_seq_list)
self.init_seq = Sequence(self.init_seq_list)
self.hallways_seq = Sequence(self.hallway_seq_list)
#self.new_seq_list = []
#self.new_seq = Sequence(self.new_seq_list)
self.rate = rospy.Rate(20) #10Hz
def read_mode(self,ros_data):
self.mode = ros_data.data
if(self.mode == "init"):
msg = actMsg()
msg.node_name = "reset"
msg.activate = True
self.actv_publisher.publish(msg)
self.doors_seq = Sequence(self.doors_seq_list)
self.hallways_seq = Sequence(self.hallway_seq_list)
#self.new_seq = Sequence(self.new_seq_list)
else:
self.loop_pub.publish(3)
def enter_loop(self,ros_data):
if(self.mode == self.init_seq.get_mode()):
#self.new_seq.reset_seq()
self.init_seq.seq_fun()
if(self.mode == self.doors_seq.get_mode()):
self.doors_seq.seq_fun()
#if(self.mode == self.new_seq.get_mode()):
#self.new_seq.seq_fun()
if(self.mode == self.hallways_seq.get_mode()):
self.hallways_seq.seq_fun()
def main(args):
rospy.init_node('Sequencer', anonymous=True)
sc = Sequencer()
#rospy.init_node('send_command', anonymous=True)
rospy.spin()
if __name__ == '__main__':
main(sys.argv)
\ No newline at end of file
#!/usr/bin/env python
"""
This node will read the stairs data and compute the desired command to send it to the /vel_in
"""
import numpy as np
import rospy
import sys
from activation_class import NodeActivate
from projectTools import GenTools, DroneCommand
from Regulator import RegulatorClass
from std_msgs.msg import Float32,Int32,String
import math
class ComputeTarStairsUp(NodeActivate):
def __init__(self):
super(ComputeTarStairsUp,self).__init__("drone_vs_stairs_round2") self.dcx = DroneCommand(2.0,0,0.05)
self.dcang = DroneCommand(1.,0,0.0)
self.dcy = DroneCommand(1.0,0.0,0.0)
self.dcz = DroneCommand(1.0,0,0)
self.stairs_presence = rospy.Subscriber("/stairs_presence",Int32,self.callback)
#self.ready_to_get_up = rospy.Subscriber("/ready_to_get_up",Int32,self.position)
self.st_ctr_x = rospy.Subscriber("/stairs_center_x_relative",Float32,self.lateralPositionCheck)
self.low_st_y = rospy.Subscriber("/lowest_stair_y_relative",Float32,self.verticalPositionCheck)
self.st_ang = rospy.Subscriber("/stairs_angle_rad",Float32,self.directionCheck)
self.xcmd_publisher = rospy.Publisher("/vel_in_x",Float32,queue_size=1)
self.ycmd_publisher = rospy.Publisher("/vel_in_y",Float32,queue_size=1)
self.angcmd_publisher = rospy.Publisher("/vel_in_z",Float32,queue_size=1)
self.zcmd_publisher = rospy.Publisher("/vel_in_alt",Float32,queue_size=1)
#self.deact_publisher = rospy.Publisher("/stairs_over",Int32,queue_size=1)
#TESTERS:
#self.lat_pos = rospy.Publisher("/lat_pos_ok",Int32,queue_size=1)
#self.dir_ok = rospy.Publisher("/dir_ok",Int32,queue_size=1)
#self.vert_pos_ok = rospy.Publisher("/get_up",Int32,queue_size=1)
self.stairs_detected = 1
self.directionOK = False
self.lateralPositionOK = False
self.verticalPositionOK = False
self.center_diff_x = 0 # >0 si escaliers a droite
# self.center_diff_y = 0 # >0 si escalier au fond de l'image
self.lowest_st_y = 0
self.diff_angle = 0
self.delta_angle = 0.02 #3 # empirique
self.delta_centerX = 0.01 # a faire varier pour optimiser, normalement 0.04 devrait suffir
self.delta_Y = 0.3 # cinquieme inferieur de l'image
self.xcmd = 0
self.ycmd = 0
self.angcmd = 0
self.zcmd = 0
#self.deact_publisher.publish(0)
#NODE ACTIVATION CONTROL
def stairs_detection(self,ros_data):
self.stairs_detected = ros_data.data
#def position(self,ros_data):
# self.ready = ros_data.data
#NODE DEACTIVATION WHEN STAIRS OVER -- NOT USED YET
#def de_activation(self,ros_data):
# rec_msg = ros_data.data
# if rec_msg == 0:# pas d'escaliers ou pas ready
# self.deactivate = 1
# if rec_msg == 1:
# self.deactivate = 0
def turn(self):
self.angcmd = self.dcx.computeCommand(self.diff_angle,0.0)*0.1
self.xcmd = 0.0
def shift(self):
#center_diff_x > 0 <=> se decaler vers la droite, soit ycmd positif
if self.directionOK:
self.ycmd = self.dcy.computeCommand(self.center_diff_x,0.0)*0.7
else:
self.ycmd = self.dcy.computeCommand(self.center_diff_x,0.0)
def goforwards(self):
self.angcmd = 0.0
self.ycmd = 0.0
self.xcmd = 0.25 #une marche par seconde
def stop(self):
self.angcmd = 0.0
self.ycmd = 0.0
self.xcmd = 0.0
self.zcmd = 0.0
def directionCheck(self,ros_data):
if (self.node_active == True) and self.stairs_detected:
self.diff_angle = ros_data.data - math.pi/2
self.directionOK = abs(self.diff_angle) < self.delta_angle
if not(self.directionOK) :
self.turn()
else:
self.angcmd = 0.0
def lateralPositionCheck(self, ros_data):
if(self.node_active == True) and self.stairs_detected:
self.center_diff_x = ros_data.data
self.lateralPositionOK = abs(self.center_diff_x) < self.delta_centerX
if not(self.lateralPositionOK) :
self.shift()
def verticalPositionCheck(self, ros_data):
if(self.node_active == True):
self.zcmd = 0.
self.lowest_st_y = ros_data.data
self.verticalPositionOK = self.lowest_st_y > self.delta_Y
if self.directionOK and self.lateralPositionOK and not(self.verticalPositionOK):
self.zcmd = 0.35
#si on avance de 25cm on monte de 35cm car normes:
# Hauteur de marches : entre 17 et 21 cm.
# Giron de marche : entre 21 et 27 cm.
def callback(self,ros_data):
if(self.node_active == True):
self.stairs_detection(ros_data)
if self.stairs_detected:
if self.directionOK and self.lateralPositionOK:
self.goforwards()
#saturation
self.angcmd = GenTools.setMax(self.angcmd,0.3)
self.ycmd = GenTools.setMax(self.ycmd,0.1)
#je ne mets pas de else stop pour pouvoir monter les escaliers jusqu'au bout mais ce serait interessant de tester
self.angcmd_publisher.publish(self.angcmd)
self.ycmd_publisher.publish(self.ycmd)
self.xcmd_publisher.publish(self.xcmd)
self.zcmd_publisher.publish(self.zcmd)
#self.positionOK_publisher.publish(self.deactivate)
#self.lat_pos.publish(self.lateralPositionOK)
#self.dir_ok.publish(self.directionOK)
def main(args):
rospy.init_node('drone_vs_stairs_round2', anonymous=True)
sc = ComputeTarStairsUp()
rospy.spin()
if __name__ == '__main__':
main(sys.argv)
\ No newline at end of file
#!/usr/bin/env python
"""
This node will read the stairs data and compute the desired command to send it to the /vel_in
"""
import numpy as np
import rospy
import sys
from activation_class import NodeActivate
from projectTools import GenTools, DroneCommand
from Regulator import RegulatorClass
from std_msgs.msg import Float32, Int32, String
import math
class ComputeStairsTar(NodeActivate):
def __init__(self):
super(ComputeStairsTar,self).__init__("drone_vs_stairs_round1")
self.dcx = DroneCommand(1.0,0,0.05)
self.dcang = DroneCommand(1.0,0.,0)
self.stairs_presence = rospy.Subscriber("/stairs_presence",Int32,self.de_activation)
self.st_ctr_x = rospy.Subscriber("/lower_stairs_center_x_relative",Float32,self.directionCheck)
self.st_ctr_y = rospy.Subscriber("/lower_stairs_center_y_relative",Float32,self.computeStairsCommand)
self.angcmd_publisher = rospy.Publisher("/vel_in_z",Float32,queue_size=1)
self.xcmd_publisher = rospy.Publisher("/vel_in_x",Float32,queue_size=1)
self.positionOK_publisher = rospy.Publisher("/ready_to_get_up",Int32,queue_size=1)
self.check = rospy.Publisher("/check",Int32,queue_size=1)
self.check.publish(0)
self.center_diff_x = 0 # >0 si escaliers a droite
self.center_diff_y = 0 # >0 si escalier au fond de l'image
self.delta_centerX = 0.005 # a faire varier pour optimiser
self.delta_centerY = 0.3 # cinquieme inferieur de l'ecran #plus etait trop restrictif
self.facingstairs = False
self.xcmd = 0
self.angcmd = 0
self.no_stairs = 0
self.ready_to_get_up = 0
self.positionOK_publisher.publish(0)
self.check_test()
def check_test(self):
self.check.publish(0)
def de_activation(self,ros_data):
if(self.node_active == True):
rec_msg = ros_data.data
if rec_msg == 0:
self.no_stairs = 1
#self.positionOK_publisher.publish(self.deactivate)
if rec_msg == 1:
self.no_stairs = 0
def directionCheck(self,ros_data):
if(self.node_active == True):
self.center_diff_x = ros_data.data
self.facingstairs = abs(self.center_diff_x) < self.delta_centerX
def computeStairsCommand(self,ros_data):
if(self.node_active == True):
self.center_diff_y = ros_data.data
if self.no_stairs == 0:
if not(self.facingstairs):
self.angcmd = self.dcx.computeCommand(self.center_diff_x,0.0) #on veut un odg de 0.3 environ
self.xcmd = .1 #on vise 0.1 en odg
else:
self.angcmd = 0.0 #stay still
self.xcmd = .1
if self.center_diff_y - self.delta_centerY > 0: # if lowest stair close:
self.xcmd = 0.0
self.ready_to_get_up = 1
self.send_conf() # ready to get up
self.angcmd = GenTools.setMax(self.angcmd,0.3)
self.angcmd_publisher.publish(self.angcmd)
self.xcmd_publisher.publish(self.xcmd)
self.positionOK_publisher.publish(self.ready_to_get_up)
else:
self.check.publish(1)
def main(args):
rospy.init_node('drone_vs_stairs_round1', anonymous=True)
sc = ComputeStairsTar()
rospy.spin()
if __name__ == '__main__':
main(sys.argv)
\ No newline at end of file
#!/usr/bin/env python
from __future__ import print_function
import sys
import rospy
import cv2
from std_msgs.msg import String, Int32, Float32
from sensor_msgs.msg import CompressedImage, Image
from cv_bridge import CvBridge, CvBridgeError
import display
import tools
import cluster
import numpy as np
import math
from activation_class import NodeActivate
class ImageProcessorStairs(NodeActivate):
def __init__(self):
super(ImageProcessorStairs,self).__init__("image_processor_drone_vs_stairs_round1")
# subscribers
self.image_sub = rospy.Subscriber("/drone_img",CompressedImage, self.callback, queue_size = 1, buff_size = 2**22)
#self.deactivation = rospy.Subscriber("/ready_to_get_up",Int32,self.deactivate)
# publishers
self.image_all_lines_pub = rospy.Publisher("/image_topic_all_lines", Image,queue_size = 1)
self.image_stairs_pub = rospy.Publisher("/image_topic_stairs_lines", Image,queue_size = 1)
self.st_ctr_x_pub = rospy.Publisher("/lower_stairs_center_x_relative", Float32, queue_size=1)
self.st_ctr_y_pub = rospy.Publisher("/lower_stairs_center_y_relative", Float32, queue_size=1)
self.st_ang_pub = rospy.Publisher("/stairs_angle_rad", Float32, queue_size=1)
self.stairs_pres = rospy.Publisher("/stairs_presence", Int32, queue_size=1)
# initialisations
self.bridge = CvBridge()
self.see_stairs = 1 #juste pour ne pas crasher des le debut
self.stairs_pres.publish(1)
self.st_ctr_x_pub.publish(0.0)
self.st_ctr_y_pub.publish(0.0)
self.st_ang_pub.publish(math.pi/2)
def noStairs(self):
if(self.node_active == True):
self.see_stairs = 0
self.stairs_pres.publish(0)
def callback(self,data):
if(self.node_active == True):
try:
frame = self.bridge.compressed_imgmsg_to_cv2(data, "bgr8")
# RESHAPE
heigth, width, depth = frame.shape
W = 1000.
imgscale = 1.0#0.6#W/width
newX,newY = frame.shape[1]*imgscale, frame.shape[0]*imgscale
newframe = cv2.resize(frame, (int(newX), int(newY)))
grayVid = cv2.cvtColor(newframe, cv2.COLOR_BGR2GRAY)
# DETECT SEGMENTS
lsdVid = cv2.createLineSegmentDetector(0)
segmentsArray = lsdVid.detect(grayVid)[0]
# CONVERTIT LES SEGMENTS EN LIGNES
linesArray = tools.findParamSegments(segmentsArray)
if str(linesArray) == "no stairs" or np.size(linesArray) == 0:
self.noStairs()
else:
# NE GARDE QUE LES LIGNES LES PLUS LONGUES
lmin = 30*imgscale
long_lines = tools.segmentsLongerThan(linesArray, lmin)
if np.size(long_lines)==0:
self.noStairs()
else:
drawn_image_long_lines = display.copy_img(grayVid)
display.draw_segments(drawn_image_long_lines,long_lines,(255,0,0),2)
image_long_lines = self.bridge.cv2_to_imgmsg(drawn_image_long_lines, encoding="bgr8")
self.image_all_lines_pub.publish(image_long_lines)
# CLUSTERISE SUR AB
epsAB=0.05
min_sampleAB=2
tab_ab, clustersAB, labels = cluster.clusterize_parallel(long_lines,epsAB,min_sampleAB)
stairs_label = tools.findLabelStairs(tab_ab, clustersAB, labels,0.8,2.4)
if stairs_label == "NoStairs":
self.noStairs()
else:
a_st,b_st = tab_ab[stairs_label]
stairs_angle = np.arctan2(b_st,a_st)
cluster_AB_i = clustersAB[stairs_label]
# CLUSTERISE SUR XY
clusterbis,labelbis = cluster.clusterize_xy(cluster_AB_i,50*imgscale,4)#initialement minsample =2, j'ai fait ca pour eviter de se prndre les rembardes
T=[]
for cluster_ in clusterbis :
taille=np.size(cluster_)
T.append(taille)
if T == [0] or T==[]:
self.noStairs()
else:
i_max = T.index(max(T))
print(i_max)
stairs_lines = clusterbis[i_max]
#LOWER STAIRS
length = len(stairs_lines)
lowest_st_nb = 4
if length > lowest_st_nb:
lowest_stairs_lines = stairs_lines[length-lowest_st_nb:length,:]
else:
lowest_stairs_lines = stairs_lines
low_x_moy, low_y_moy = tools.findMiddle(lowest_stairs_lines)
x_moy, y_moy = tools.findMiddle(stairs_lines)
drawn_image_stairs = display.copy_img(grayVid)
display.draw_segments(drawn_image_stairs, stairs_lines, (255,0,0), 2)
display.draw_segments(drawn_image_stairs, lowest_stairs_lines, (0,255,0), 3)
display.points(drawn_image_stairs, np.array([[x_moy,y_moy]]), 5, np.array([255,0,0]),-1)
display.points(drawn_image_stairs, np.array([[low_x_moy,low_y_moy]]), 5, np.array([0,255,0]),-1)
display.points(drawn_image_stairs, np.array([[newX/2,newY/2]]), 3, np.array([0,0,0]),-1)
display.draw_segments(drawn_image_stairs, np.array([[newX/2,0,newX/2,newY],[0,0.8*newY,newX,0.8*newY]]), (0,0,0), 1)
display.draw_segments(drawn_image_stairs, np.array([[low_x_moy,0,low_x_moy,newY],[0,low_y_moy,newX,low_y_moy]]), (0,0,255), 2)
image_stairs = self.bridge.cv2_to_imgmsg(drawn_image_stairs, encoding="bgr8")
self.image_stairs_pub.publish(image_stairs)
self.st_ctr_x_pub.publish(low_x_moy/newX - 0.5)
self.st_ctr_y_pub.publish(low_y_moy/newY - 0.5)
self.st_ang_pub.publish(stairs_angle)
self.stairs_pres.publish(1)
except CvBridgeError as e:
pass
def main(args):
rospy.init_node('image_processor_drone_vs_stairs_round1', anonymous=True)
ia = ImageProcessorStairs()
try:
rospy.spin()
except KeyboardInterrupt:
print("Shutting down")
cv2.destroyAllWindows()
if __name__ == '__main__':
main(sys.argv)
This diff is collapsed.
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment