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 !

...
 
Commits (15)
......@@ -5,8 +5,9 @@ This ROS package is for a semi-autonomous drone pilot for the Parrot Bebop 2 usi
## Quick Intro
This project has many parts,The first one is moving the drone at a constant velocity.
The Second part is built on the first one , is used move the drone inside a hallway.
The third part, is used to detect openned doors.
The Second part is built on the first one , is used to perform indoors navigation(hallways, stairs and doors crossing).
For more details, check the documentation file ```repport.pdf```(French Documentation).
## Use This Package
First Clone this repo in your catkin workspace
......@@ -17,28 +18,54 @@ Build your workspace
```
$ catkin build
```
## Moving Drone
## Drone Control
In this project we've built a module, that allows you to command the drone velocity.
To do this you need to launch the VelLaunch.launch
```
$ roslaunch drone_project VelLaunch.launch
```
First start with sending a reset command to this modules by sending any integer to the topics ```/reset_cmd_x```,```/reset_cmd_y``` and ```/reset_cmd_z```.
And then publish to ```/vel_in_x``` , ``` /vel_in_y``` and ```/vel_in_z``` the velocities you want the drone to move with.
And now you can publish to ```/vel_in_x``` , ``` /vel_in_y``` to command linear x and y respectively,```/vel_in_z``` to command angular z,and finally ```/vel_in_alt``` to command the altitude.
The frequency of sending data to the drone is equal to 5Hz.
### PID parameters
To change PID parameters, open the corresponding ```compute``` file, like ```compute_x.py``` for the X axis for example,
```python
super(XCommand,self).__init__(P,I,D)
```
### Example Moving at constant Velocity:
The ``` move_tester.py ``` file is an example of moving with a constant velocity.
## Using The Interface
To run the other parts of the project, you need to launch the file ```Sequencer.launch```.
### Doors and Hallways
To run the indoors navigation modules , you just need to launch the file ```Sequencer.launch```.
```
$ roslaunch drone_project Sequencer.launch
```
You will get an interface, that will let you choose the mode you want, you can click on Doors, to find opened doors and pass through them, or Hallway to launch the autonomous navigation in the Hallways.
You will get an interface, that will let you choose the mode you want, you can click on Doors, to find opened doors and pass through them, Hallway to launch the autonomous navigation in the Hallways.
### Stairs
Be careful, the stairs module is still quite unstable in real-life tests because of the unstability of the odometry measures in stairs but you can test its logic on the unity simulation scene.
to test it, first download the unity simulation scene ```unityProjectStairs.zip```,and unzip it.
After loading the scene,
```
$ roslaunch drone_project SequencerUnity.launch
```
then click ```play``` to run he simulation and select ```stairs``` on the interface.
## Customize this project
......@@ -46,7 +73,7 @@ You will get an interface, that will let you choose the mode you want, you can c
Each node in this project have a unique name, used to activate it.
Some of the nodes also have the ability to send back confirmation after reaching their target .
They also have the ability to send back confirmation after reaching a target .
Like for example the node responsible of searching for openned doors, it will send a confirmation when a door is detected.
......@@ -64,6 +91,9 @@ self.new_seq = Sequencer(new_seq_list)
Where ```mode_name``` is the string that when we receive on the ```/mode``` topic , the sequence will run.
Where ```node_1``` and ```node_2``` are the names of nodes that will be activated in the phase 1 of this sequence, and ```cond_1``` and ```cond_2``` are the names of nodes confirmations we are waiting for to continue to the next step.
### NB:
Having ```[]``` as condition means always True, and having ```[""]``` means always False.
### Running the sequence
To run the sequence, you need to add 2 lines of code to the ```Sequencer.py``` class .
......@@ -107,19 +137,12 @@ class myNewNode(NodeActivate)
super(myNewNode,self).__init__("newNodeName")
#Your Code
```
This means that your node has the name ```newNodeName``` and this is the name that will be used to activate it seperatly if you want (Check section)
This means that your node has the name ```newNodeName``` and this is the name that will be used to activate it seperatly if you want(Later).
and to check if the node is activated or not, the variable ```self.activate``` is true when node activated and false otherwise.
As mentionned above, the node may need to send confirmation after reaching the target, to do so, you have to inherit from the class ```returnResp```.
As mentionned above, the nodes may need to send confirmation after reaching the target, to do so, use the function ```send_conf()```.
```python
from activation_class import NodeActivate,returnResp
class myNewNode(NodeActivate,returnResp)
def __init__(self):
super(myNewNode,self).__init__("newNodeName")
```
And to send the confirmation use the following function,
```python
self.send_conf()
```
......
<launch>
<include file="$(find drone_project)/launch/VelLaunch.launch"/>
<node pkg="drone_project" name="Sequencer" type="Sequencer.py">
</node>
<node pkg="drone_project" name="Main" type="Main.py">
......@@ -17,4 +19,15 @@
</node>
<node pkg="drone_project" name="detectVanish" type="image_proc.py">
</node>
<node pkg="drone_project" name="image_processor_drone_vs_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_processor_drone_vs_stairs_round2" 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
<launch>
<include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch"/>
<include file="$(find drone_project)/launch/VelLaunchUnity.launch"/>
<node pkg="drone_project" name="Sequencer" type="Sequencer.py">
</node>
<node pkg="drone_project" name="Main" type="Main.py">
</node>
<node pkg="drone_project" name="image_processor_drone_vs_stairs_round1" type="stairs_entrance_target.py">
<remap from="/drone_img" to="/drone/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_processor_drone_vs_stairs_round2" type="stairs_target_up.py">
<remap from="/drone_img" to="/drone/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
<launch>
<node pkg="drone_project" name="sendCommand" type="send_cmd.py">
<remap from="/cmd_vel" to="/drone/cmd_vel"/>
<!-- PAS D'ODOMETRIE DANS LA SIMULATION: -->
<remap from="/vel_out_x" to="/vel_in_x"/>
<remap from="/vel_out_y" to="/vel_in_y"/>
<remap from="/vel_out_z" to="/vel_in_z"/>
<remap from="/vel_out_alt" to="/vel_in_alt"/>
</node>
</launch>
\ No newline at end of file
#!/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()
......
class MySequencer(Sequencer):
def __init__(self):
super(...)
self.new_mode('hallway',
[[('slide', True), 'detect'], # Phase 1
[Wait(['slide', 'detect'], [('detect_door', True), ('Forward', False)]), # Phase 2 : attendre que slide et detect aient fini. Wait(cond, seq)
...,
....)
self.new_mode('stairs',
[[....]])
...
"""
new_mode(command_name, seq)
seq : cmd_list
cmd : activ_list # activésen même temps
activ : (nom, bool)
| Wait(cond_list, avtiv_list)
cond_list : liste de noms.
"""
def main(args):
rospy.init_node('Sequencer', anonymous=True)
sc = Sequencer()
sc.run()
#rospy.init_node('send_command', anonymous=True)
rospy.spin()
if __name__ == '__main__':
main(sys.argv)
"----------------------"
sequence = { 'hallway' : {....}
'stairs' : {....}
}
seq = Sequenceur(sequence) # et ça part....
#!/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
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
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)
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
......@@ -29,7 +29,7 @@ class image_convert(NodeActivate):
self.pubDiff = rospy.Publisher('/sDiffs',Float32,queue_size=1)
self.pubForBag = rospy.Publisher('/imgForBag/front/compressed', CompressedImage, queue_size=1)
#self.subscriber = rospy.Subscriber("/bebop/image_raw",Image, self.callback)
self.subscriber = rospy.Subscriber("/bebop/image_raw",Image, self.callback)
self.subscriber = rospy.Subscriber("/bebop/image_raw/compressed",CompressedImage, self.callback)
self.lsd = cv2.createLineSegmentDetector(0)
self.real_centr = np.array([160,120])
self.old_centr = np.array([160,120])
......@@ -198,7 +198,7 @@ class image_convert(NodeActivate):
#msg = ""
if(self.node_active == False):
return
cv2_img = self.br.imgmsg_to_cv2(ros_data)
cv2_img = self.br.compressed_imgmsg_to_cv2(ros_data)
#np_arr = np.fromstring(ros_data.data, np.uint8)
#image_np = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
image,cx,sDiff = self.getVanishingPoint(cv2_img)
......@@ -210,7 +210,6 @@ class image_convert(NodeActivate):
cmprsmsg = self.br.cv2_to_compressed_imgmsg(image)
self.pubForBag.publish(cmprsmsg)
cv2.waitKey(10)
def main(args):
rospy.init_node('image_feature', anonymous=True)
ic = image_convert()
......
#!/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
lowest_st_nb = 4
lowest_stairs_lines = tools.findLowerStairs(stairs_lines, lowest_st_nb)
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.
......@@ -46,7 +46,7 @@ class EnterDoors(NodeActivate,returnResp):
self.y_vel = 0
self.act_time = -1
def update_vel(self,ros_data):
if(self.node_active == 0):
if(self.node_active == False):
self.act_time = time.time()
return
twist = ros_data.twist.twist
......
This diff is collapsed.