Commit 7d767553 authored by jad's avatar jad

merge_done

parent ef5b9341
......@@ -17,4 +17,15 @@
</node>
<node pkg="drone_project" name="detectVanish" type="image_proc.py">
</node>
<node pkg="drone_project" name="image_processor_stairs_round1" type="stairs_entrance_target.py">
<remap from="/drone_img" to="/bebop/image_raw/compressed"/>
</node>
<node pkg="drone_project" name="drone_vs_stairs_round1" type="go_to_stairs.py">
</node>
<node pkg="drone_project" name="image_analyzer3" type="stairs_target_up.py">
<remap from="/drone_img" to="/bebop/image_raw/compressed"/>
</node>
<node pkg="drone_project" name="drone_vs_stairs_round2" type="get_up_stairs.py">
</node>
</launch>
\ No newline at end of file
......@@ -17,6 +17,11 @@
<remap from="/curr_in" to="/bebop/odom" />
<remap from="/reset_cmd" to="/reset_cmd_z"/>
</node>
<node pkg="drone_project" name="ComputeAlt" type="compute_alt.py">
<remap from="/tar_in" to="/vel_in_alt"/>
<remap from="/vel_out" to="/vel_out_alt"/>
<remap from="/curr_in" to="/bebop/odom" />
</node>
<node pkg="drone_project" name="sendCommand" type="send_cmd.py">
</node>
</launch>
\ No newline at end of file
......@@ -24,9 +24,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_stairs_round1","drone_vs_stairs_round1"],["drone_vs_stairs_round1"]],[["image_processor_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 +43,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 +57,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()
......
import numpy as np
from sklearn.cluster import DBSCAN
import pandas as pd
import tools
def clusterize_parallel(linesArray,eps,min_sample):
max_clusters = 10
normal_vector = linesArray[:,5:7]
try:
db = DBSCAN(eps, min_sample).fit(normal_vector)
labels = db.labels_
n_clusters_ = max(set(labels))
clusters = pd.Series([linesArray[labels == i] for i in xrange(0,n_clusters_+1)])
tab_ab=[]
for cluster in clusters:
a = np.mean(cluster[:,5])
b = np.mean(cluster[:,6])
tab_ab.append([a,b])
return (np.array(tab_ab),clusters, labels)
except Exception as e:
print("Error")
def clusterize_xy(linesArray,eps,min_sample):
#first, we compute the array of midlles of each segment
midXY = tools.findMiddlePointsArray(linesArray)
# we will clusterize on y(x) so we write y first
mid=np.concatenate((np.expand_dims(midXY[:,1],axis=1),np.expand_dims(midXY[:,0],axis=1)), axis =1)
try:
db = DBSCAN(eps, min_sample).fit(mid)
labels = db.labels_
n_clusters_ = max(set(labels))
clusters = pd.Series([linesArray[labels == i] for i in xrange(0,n_clusters_+1)])
return(clusters,labels)
except Exception as e:
print("Error")
def clusterize_offsets(linesArray,eps,min_samples):
vecteur_c = linesArray[:,7]
n_lines = np.size(vecteur_c)
X = vecteur_c.reshape(n_lines,1)
db = DBSCAN(eps, min_samples).fit(X)##
labels = db.labels_ #-1
nb_cluster = max(labels)
i_list = list(np.arange(nb_cluster+1))
gathered_on_C = pd.Series([linesArray[labels == i] for i in i_list])
tab_c=[]
for cluster in gathered_on_C:
c = np.mean(cluster[:,7])
tab_c.append(c)
# we add the well detected lines (label == -1) at the end:
if -1 in set(labels):
well_detected_lines = linesArray[labels == -1]
new_indexes = list(np.arange(nb_cluster+1 , nb_cluster+len(well_detected_lines)+1))
k = nb_cluster + 1
for line in well_detected_lines:
tab_c.append(line[7])
i_list.append(k)
k = k+1
s1 = pd.Series([[line] for line in well_detected_lines],index = new_indexes)
gathered_on_C = gathered_on_C.append(s1)
return (tab_c, i_list, gathered_on_C, labels)
\ No newline at end of file
#!/usr/bin/env python
"""
This node will just forward data from /vel_in to /vel_out
It will get the target velocity from the topic /vel_in
and republishes it to /vel_out
"""
import numpy as np
import rospy
import sys
from projectTools import GenTools
from Regulator import RegulatorClass
from std_msgs.msg import Float32,Int32
class ZCommand(RegulatorClass):
def __init__(self):
super(ZCommand,self).__init__(0.0,0,0)# p=0
#The Z is not like the others,we need to just publish the data to the out node, we will override the read_tar
def read_tar(self,ros_data):
#if(self.activation == 0):
# return
self.cmd = ros_data.data
#self.cmd_publisher.publish(ros_data.data)
def read_val(self,ros_data):
self.cmd = self.dc.computeCommand(self.currVal,self.targVal)
if(self.data_rec == 1):
self.cmd_publisher.publish(self.cmd)
self.data_rec = 0
def main(args):
rospy.init_node('computeLinZ', anonymous=True)
sc = ZCommand()
#rospy.init_node('send_command', anonymous=True)
rospy.spin()
if __name__ == '__main__':
main(sys.argv)
import cv2
import matplotlib.pyplot as plt
import numpy as np
import cluster
############################# OUTILS DE DESSIN ################################
def segments(img, nameString, lines, color,thickness,lsd):
drawn_img=lsd.drawSegments(img,lines)
def draw_line (screen, nameString, line, color, thickness):
H, W = screen.shape[:2]
a=line[5]
b=line[6]
c=line[7]
T=[]
def f(x,y):
return(a*x+b*y+c)
if f(0,0)*f(W,0)<0: #croise le bord haut de la fenetre
x=(-c/a)
y=0
T.append(x,y)
if f(W,0)*f(W,H)<0: #croise le bord droit de la fenetre
x=W
y=(-c-a*W)/b
T.append(x,y)
if f(0,H)*f(W,H)<0: #croise le bord en bas
x=(-c-b*H)/a
y=H
T.append(x,y)
if f(0,0)*f(0,H)<0:
x=0
y=-c/b
T.append(x,y)
cv2.line(screen, (int(T[0]),int(T[1])),(int(T[2]),int(T[3])), color, thickness, lineType=8, shift=0)
def draw_lines(drawn_img, nameString, lines, color, thickness):
for line in lines:
drawn_img = draw_line(drawn_img, nameString, line,color, thickness)
def draw_segments_couleur_angle(drawn_img, lines, tab_ab, labels, clusters_AB,thickness):
i=0
for line in lines:
label_i=labels[i]#+1
color=cluster_color(tab_ab,label_i)
drawn_img = cv2.line(drawn_img, (int(line[0]),int(line[1])),(int(line[2]),int(line[3])), color, thickness, lineType=8, shift=0)
i+=1
def draw_segments_couleur_angle_par_cluster(drawn_img, lines, tab_ab, labels, clusters_AB,thickness):
i=0
for label in labels:#on parcourt les labels
color=cluster_color(tab_ab,label)
lines_to_plot=clusters_AB[label]
for line in lines_to_plot:#on parcourt le cluster
drawn_img = cv2.line(drawn_img, (int(line[0]),int(line[1])),(int(line[2]),int(line[3])), color, thickness, lineType=8, shift=0)
def draw_segments(drawn_img, lines, color,thickness):
i=0
for line in lines:
drawn_img = cv2.line(drawn_img, (int(line[0]),int(line[1])),(int(line[2]),int(line[3])), color, thickness, lineType=8, shift=0)
i+=1
def draw_segment(drawn_img, line, color_tab, thickness):
drawn_img = cv2.line(drawn_img, (int(line[0]),int(line[1])),(int(line[2]),int(line[3])), color_tab, thickness, lineType=8, shift=0)
def points(img, pointsArray, radius = 3, color = 255,thickness=-1):#j'ai supprime l'argument nameString en deuxieme position
drawn_img = img
for x,y in pointsArray:
drawn_img = cv2.circle(img, (int(x),int(y)), radius, color, thickness)
#cv2.circle(img, center, radius, color[, thickness[, lineType[, shift]]])
####################### CLUSTERS #########################################################################
def clusters(vecteur_normalise,labels,desc_string,a=None, b=None):
#utilise plot, cas general
fig, ax = plt.subplots()
ax.scatter(vecteur_normalise[:,0], vecteur_normalise[:,1], c=labels)
if desc_string == "ab":
ax.set_xlabel('a norm')
ax.set_ylabel('b norm')
plt.title('repartion b en fonction de a')
elif desc_string == "c":
ax.set_xlabel('c')
ax.set_ylabel('c')
title = 'repartion de c sur le cluster ab, a=' + str(a) + ' b=' +str(b)
plt.title(title)
else:
print("vous devez quel type de cluster il faut afficher, rentrez ab ou c - en precisant a et b dans ce cas")
plt.show()
def empty_cluster(desc_string="ab", a=None, b=None):
#preciser le type de cluster: "ab"(par defaut) ou "c"
#cree le fond
if desc_string == "ab":
name = "repartion b en fonction de a"
x_name = "a norm"
y_name = "b norm"
if desc_string == "c":
name = "repartion de c sur le cluster ab, a=" + str(a) + " b=" +str(b)
x_name = "c"
y_name = "c"
font = cv2.FONT_HERSHEY_SIMPLEX
fontScale = 1
fontColor = 0
lineType = 2#cv2.LINE_AA
height, width = 200, 200
mid = height//2
if desc_string=="ab":
x_mid = mid
y_mid = mid
if desc_string=="c":
x_mid = mid
y_mid = mid
empty_window(name)
img_plot = empty_white_img (height,width)
#on commence par nommer l'axe y et faire pivoter l'image
cv2.putText(img_plot, y_name, (70,x_mid-20), font, fontScale, fontColor, lineType)
rows,cols,color_type = img_plot.shape
M = cv2.getRotationMatrix2D((cols/2,rows/2),90,1)
img_plot = cv2.warpAffine(img_plot,M,(cols,rows))
# on place les autres elements
mid = height//2
cv2.arrowedLine(img_plot, (x_mid, y_mid), (680, y_mid), 0, thickness=1, line_type=8, shift=0, tipLength=0.1)
cv2.arrowedLine(img_plot, (x_mid, y_mid), (x_mid,680), 0, thickness=1, line_type=8, shift=0, tipLength=0.1)
cv2.putText(img_plot, x_name, (width-200, y_mid+30), font, fontScale, fontColor, lineType) #img,text,coord,font
return name, img_plot
def cluster_plot(vecteur_normalise,labels, image_plot, name, desc_string="ab"):
#remplit le fond avec les donnees
height, width = 200, 200
mid = height//2
if desc_string=="ab":
x_mid = mid-50
y_mid = mid
scale = 80
cluster_plot = image_plot.copy()
if desc_string=="c":
x_mid = mid
y_mid = mid
scale = 800
cluster_plot = image_plot
n = vecteur_normalise.shape[0]
radius = 3
thickness=-1
for i in range (n):
cluster_plot = cv2.circle(cluster_plot,(int(vecteur_normalise[i,0]*scale)+x_mid,int(vecteur_normalise[i,1]*scale)+y_mid), radius, colors(labels[i]+1),thickness)
cv2.imshow(name,cluster_plot)# ou return cluster_plot
def cluster_plot_bis(vecteur_normalise,cluster_AB, tab_ab, labels, image_plot, name, desc_string="ab"):
#remplit le fond avec les donnees
height, width = 200, 200
mid = height//2
if desc_string=="ab":
x_mid = mid-50
y_mid = mid
scale = 80
cluster_plot = image_plot.copy()
if desc_string=="c":
x_mid = mid
y_mid = mid
scale = 80
cluster_plot = image_plot
n = vecteur_normalise.shape[0]
radius = 3
thickness=-1
n=np.max(labels)
i=0
for label_i in labels:
if label_i>-1:
cluster_AB_i=cluster_AB[label_i]
color=cluster_color(tab_ab,label_i)
vecteur_plot=cluster_AB_i[:,5:7]
for vecteur_normalises in vecteur_plot:
cluster_plot = cv2.circle(cluster_plot,(int(vecteur_normalises[0]*scale)+2*x_mid,int(vecteur_normalises[1]*scale)+y_mid), radius, color,thickness)
image_plot=cluster_plot
cv2.imshow(name,cluster_plot)# ou return cluster_plot*
cv2.waitKey(10)
def cluster_ab(vecteur_normalise,labels, image_plot, window_name):
cluster_plot(vecteur_normalise,labels, image_plot, window_name, "ab")
def cluster_ab_bis(vecteur_normalise,cluster_AB, tab_ab,labels, image_plot, window_name):
cluster_plot_bis(vecteur_normalise,cluster_AB, tab_ab, labels, image_plot, window_name, "ab")
def cluster_c(vecteur_normalise,labels, a, b):
vecteur_normalise = np.apply_along_axis(np.divide,0,vecteur_normalise,np.max(np.abs(vecteur_normalise)))#est ce que ca ne modifierait pas le tableau initial?
name, img_plot = empty_cluster("c", a, b)
cluster_plot(vecteur_normalise,labels, img_plot, name, "c")
def couleur():
colors=np.zeros((180,3))
for i in range (60):
colors[i][2]=255-i*4
colors[i+30][1]=4*i
colors[179-i-30][1]=4*i
colors[179-i][0]=255-4*i
return(colors)
def colors_angle(angle):
colors = couleur()
color=tuple((colors[angle,0],colors[angle,1],colors[angle,2]))
return(color)
def cluster_color(tab_ab,i):
a_moy=tab_ab[i,0]
b_moy=tab_ab[i,1]
if abs(a_moy)>0.01:
teta=int((np.arctan(b_moy/a_moy)*180)/(np.pi))
color=colors_angle(teta)
else:
color=colors_angle(1)
return(color)
####################### GESTION DES FENETRES #####################
def empty_white_img(height,width):
blank_image = np.ones((height,width,3), np.float32)*255#initialement np.uint8 #cv2.waitKey(0)np.float32?
return blank_image
def empty_black_img(height,width):
blank_image = np.zeros((height,width,3), np.float32)#initialement np.uint8 #cv2.waitKey(0)np.float32?
return blank_image
def copy_img(img):
drawn_img = cv2.cvtColor(img,cv2.COLOR_GRAY2RGB)#a sortir pour pouvoir afficher plusieur
return drawn_img
def empty_window(nameString):
cv2.namedWindow(nameString, cv2.WINDOW_KEEPRATIO)
def resize_window(nameString, img):
screen_res = 1280.0, 720.0
scale_width = screen_res[0] / img.shape[1]
scale_height = screen_res[1] / img.shape[0]
scale = min(scale_width, scale_height)
window_width = int(img.shape[1] * scale)
window_height = int(img.shape[0] * scale)
cv2.namedWindow(nameString, cv2.WINDOW_NORMAL)
cv2.resizeWindow(nameString, window_width, window_height)
return window_width, window_height
#################### GESTION colors GENERALE #######################
def colors(index):
c = [(0, 0, 0), (255, 0, 0), (0, 255, 0), (0, 0, 255), (255, 255, 0), (0, 255, 255), (255, 0, 255), (120, 120, 120), (120, 50, 0), (50, 120, 0), (0, 50, 120), (120, 0, 50), (0,120,50),(50, 0, 120)]
if index >= len(c):
return c[0]
else:
return c[index]
def colors_table(labels):
return [colors(label + 1) for label in labels]
def different_window(img, nameString, function, argArray, color, thickness):
drawn_img = copy_img(img)
function(drawn_img, argArray, color, thickness)
resize_window(nameString,img)
cv2.imshow(nameString,drawn_img)
\ 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)#initialement 0.1
self.dcang = DroneCommand(1.,0,0.0)#initialement 0.001
self.dcy = DroneCommand(1.0,0.0,0.0)#nerveux
self.dcz = DroneCommand(1.0,0,0)
self.stairs_presence = rospy.Subscriber("/stairs_presence",Int32,self.stairs_detection)#
#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.st_ctr_y = rospy.Subscriber("/stairs_center_y_relative",Float32,self.callback)
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.ycmd_publisher = rospy.Publisher("/vel_out_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#0 enleve pour le test
#self.ready = 0#1#0enleve pour le test
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_centerY = 0.2 # qqch >= 0
self.delta_Y = 0.25 # tiers 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 #turn
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 #, self.dcy.computeCommand(self.diff_angle,0.0)) #shift(centerdiffX)
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.stairs_detected:# and self.ready:
self.diff_angle = ros_data.data - math.pi/2
self.directionOK = abs(self.diff_angle) < self.delta_angle
if not(self.directionOK) :
self.turn()
# self.shift()#sur proposition d'Adrien
else:
self.angcmd = 0.0
def lateralPositionCheck(self, ros_data):
if self.stairs_detected:# and self.ready:
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):
self.zcmd = 0.
self.lowest_st_y = ros_data.data
self.verticalPositionOK = self.lowest_st_y > 0.3
if self.directionOK and self.lateralPositionOK and not(self.verticalPositionOK): #> self.delta_Y:
self.zcmd = 0.35
#si on avance de 25cm on monte de 20cm 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.stairs_detected:# and self.ready:
if self.directionOK and self.lateralPositionOK:
#if self.lateralPositionOK:
self.goforwards()
#if not(self.verticalPositionOK):
# self.zcmd = 0.25
#saturation
self.angcmd = GenTools.setMax(self.angcmd,0.3)
#self.ycmd = GenTools.setMax(self.ycmd,0.08)
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)
else:#a supprimer aprs, c'est juste pour voir #ca ne marche pas dans la vraie vie a mon avis
self.stop()
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,returnResp
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)#-? I=0.5
self.stairs_presence = rospy.Subscriber("/stairs_presence",Int32,self.de_activation)
self.st_ctr_x = rospy.Subscriber("/stairs_center_x_relative",Float32,self.directionCheck)
self.st_ctr_y = rospy.Subscriber("/stairs_center_y_relative",Float32,self.computeStairsCommand)
self.angcmd_publisher = rospy.Publisher("/vel_in_ang",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.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)
def de_activation(self,ros_data):
#if not(self.ready_to_get_up):
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 not(self.ready_to_get_up):
self.center_diff_x = ros_data.data
self.facingstairs = abs(self.center_diff_x) < self.delta_centerX
def computeStairsCommand(self,ros_data):
#if not(self.ready_to_get_up):
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) #turn(centerdiffX) #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)
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
......@@ -13,10 +13,12 @@ class collectData:
self.xcmd_subscriber = rospy.Subscriber("/vel_out_x", Float32, self.read_vel_x)
self.ycmd_subscriber = rospy.Subscriber("/vel_out_y", Float32, self.read_vel_y)
self.zcmd_subscriber = rospy.Subscriber("/vel_out_z", Float32, self.read_vel_z)
self.angcmd_subscriber = rospy.Subscriber("/vel_out_alt", Float32, self.read_vel_alt)
self.cmd_publisher = rospy.Publisher("/cmd_vel",Twist,queue_size = 1)
self.xcmd = 0
self.ycmd = 0
self.zcmd = 0
self.altcmd = 0
def read_vel_x(self,ros_data):
#Just updating values
......@@ -33,11 +35,16 @@ class collectData:
self.zcmd = ros_data.data
self.send_twist()
def read_vel_alt(self,ros_data):
#update and sendData Here
self.altcmd = ros_data.data
self.send_twist()
def send_twist(self):
my_cmd = Twist()
my_cmd.linear.x = self.xcmd
my_cmd.linear.y = self.ycmd
my_cmd.linear.z = 0
my_cmd.linear.z = self.altcmd
my_cmd.angular.x = 0
my_cmd.angular.y = 0
my_cmd.angular.z = self.zcmd
......
#!/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")