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 0441e3a1 authored by jad's avatar jad

Seq Doors done

parent 9cb34866
<launch>
<node pkg="drone_project" name="Main" type="Main.py">
</node>
<node pkg="drone_project" name="Sequencer" type="Sequencer.py">
</node>
<node pkg="drone_project" name="optFlow" type="testDenseOpticalFlow.py">
</node>
<node pkg="drone_project" name="curveMotion" type="turn_to_ang.py">
</node>
<node pkg="drone_project" name="turnAng" type="turn_ang.py">
</node>
</launch>
\ No newline at end of file
#!/usr/bin/env python
import rospy
from std_msgs.msg import Int32,Float32
from std_msgs.msg import Int32,Float32,String
import sys
from PyQt4 import QtGui,QtCore
......@@ -10,14 +10,7 @@ class Window(QtGui.QMainWindow):
super(Window,self).__init__()
self.setGeometry(50,50,500,300)
self.setWindowTitle("Command Test")
self.pub = rospy.Publisher("/mode", Int32, queue_size=1)
self.pub.publish(0)
self.pubx = rospy.Publisher("/in_vel_x", Float32, queue_size=1)
self.puby = rospy.Publisher("/in_vel_y", Float32, queue_size=1)
rospy.init_node("Project_Master", anonymous=True)
self.xval = 0.2
self.yval = 0
self.mode_pub = rospy.Publisher("/mode",String,queue_size=1)
self.home()
def home(self):
btn = QtGui.QPushButton("Quit",self)
......@@ -30,40 +23,30 @@ class Window(QtGui.QMainWindow):
btn.resize(100,70)
btn.move(10,10)
btn = QtGui.QPushButton("Constant V",self)
btn.clicked.connect(self.cnstnt_v)
btn = QtGui.QPushButton("Stairs",self)
btn.clicked.connect(self.Stairs)
btn.resize(100,70)
btn.move(120,10)
btn = QtGui.QPushButton("Freeze",self)
btn.clicked.connect(self.freeze_drone)
btn = QtGui.QPushButton("Doors",self)
btn.clicked.connect(self.Doors)
btn.resize(100,70)
btn.move(230,10)
self.show()
def hallway_nav(self):
#In this Mode the drone will navigate in the hallways
self.pubx.publish(self.xval)
cmd = 1;
self.pub.publish(cmd)
def cnstnt_v(self):
#In this mode the drone will move at a givem velocity ,
#this velocity is predefined here in the code and it will be sent to /in_vel_x and /in_vel_y
self.pubx.publish(self.xval)
self.puby.publish(self.yval)
cmd = 2;
self.pub.publish(cmd)
def freeze_drone(self):
#In this mode the drone will receive a twist of zero and will stay stable in the position
cmd = 0;
self.pub.publish(cmd)
self.mode_pub.publish("hallway")
def Stairs(self):
self.mode_pub.publish("stairs")
def Doors(self):
self.mode_pub.publish("door")
def close_app(self):
print "Closing"
sys.exit()
def run():
rospy.init_node('Main', anonymous=True)
app = QtGui.QApplication(sys.argv)
GUI = Window()
sys.exit(app.exec_())
run()
\ 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
"""
import rospy
from std_msgs.msg import String,Int32,Float32
from projectTools import Sequence
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_pub = rospy.Publisher("/activations",String,queue_size=1)
self.turn_done_sub = rospy.Subscriber("/turn_done",Int32,self.turn_done_callback)
self.det_doors_sub = rospy.Subscriber("/door_det",Int32,self.door_det_callback)
self.mode = "init"
self.doors_seq = Sequence("doors")
self.stairs_seq = Sequence("stairs")
self.hallway_seq = Sequence("hallway")
self.rate = rospy.Rate(20) #10Hz
def turn_done_callback(self,ros_data):
self.doors_seq.set_phase(1)
self.doors_seq.set_published(False)
def door_det_callback(self,ros_data):
self.doors_seq.set_phase(2)
self.doors_seq.set_published(False)
def read_mode(self,ros_data):
self.mode = ros_data.data
if(self.mode == "init"):
#we need to reset the command and hover the drone
pass
if(self.mode == self.doors_seq.get_mode):
self.doors_seq_func(self.doors_seq)
pass
if(self.mode == self.stairs_seq.get_mode):
self.stairs_seq_func()
pass
if(self.mode == self.hallway_seq.get_mode):
self.hallway_seq_func()
pass
def doors_seq_func(self):
if(self.doors_seq.get_phase() == 0 and self.doors_seq.get_publ() == False):
self.actv_pub.publish("reset")
self.rate.sleep()
self.actv_pub.publish("curveMotion_1")
self.rate.sleep()
self.doors_seq.set_published(True)
if(self.doors_seq.get_phase() == 1 and self.doors_seq.get_publ() == False):
self.actv_pub.publish("reset")
self.rate.sleep()
self.actv_pub.publish("checkDoors_1")
self.actv_pub.publish("turnAng_1")
self.rate.sleep()
self.doors_seq.set_published(True)
if(self.phase == 2):
self.actv_pub.publish("reset")
self.rate.sleep()
self.doors_seq.reset_seq()
self.mode_pub.publish('init')
\ No newline at end of file
#!/usr/bin/env python
import rospy
from std_msgs.msg import String
class NodeActivate(object):
def __init__(self,my_name):
self._my_name = my_name
self.act_sub = rospy.Subscriber("/activations",String,self.check_act)
self.node_active = 0
def check_act(self,ros_data):
msg = ros_data.data
if(msg == reset):
self.node_active = 0
else:
msg,act = msg.split("_")
if(msg == self._my_name):
self.node_active = act
\ No newline at end of file
#!/usr/bin/env python
"""
This node will publish the desired velocity along the x axis on the topic /vel_in_x
"""
import numpy as np
import rospy
import sys
from projectTools import GenTools,DroneCommand
from Regulator import RegulatorClass
from std_msgs.msg import Float32,Int32
from nav_msgs.msg import Odometry
from activation_class import NodeActivate
class ComputeXTar(NodeActivate):
def __init__(self):
super(ComputeXTar,self).__init__("compute_tar_x")
cmd_reset = rospy.Publisher("/reset_cmd_x",Int32,queue_size=1)
self.sync_sub = rospy.Subscriber("/bebop/odom",Odometry,self.sync_cmd)
self.cmd_publisher = rospy.Publisher("/vel_in_x",Float32,queue_size=1)
self.targX = 0.3
def sync_cmd(self,ros_data):
if(self.node_active == 0):
return
self.cmd_publisher.publish(self.targX)
def main(args):
rospy.init_node('computeTarX', anonymous=True)
sc = ComputeXTar()
#rospy.init_node('send_command', anonymous=True)
rospy.spin()
if __name__ == '__main__':
main(sys.argv)
......@@ -10,15 +10,20 @@ import sys
from projectTools import GenTools,DroneCommand
from Regulator import RegulatorClass
from std_msgs.msg import Float32,Int32
from activation_class import NodeActivate
class ComputeYTar:
class ComputeYTar(NodeActivate):
def __init__(self):
super(ComputeYTar,self).__init__("compute_tar_y")
cmd_reset = rospy.Publisher("/reset_cmd_y",Int32,queue_size=1)
self.sDiff_subscriber = rospy.Subscriber("/sDiffs",Float32,self.read_sDiffs)
self.cmd_publisher = rospy.Publisher("/vel_in_y",Float32,queue_size=1)
self.act_publisher = rospy.Publisher("/activation_y",Int32,queue_size=1)
self.targY = 0
#self.act_publisher.publish(0)
def read_sDiffs(self,ros_data):
if(self.node_active == 0):
return
#read the centroid and send the data
self.targY = ros_data.data
#The value of the velocity is proportional to the value of the difference of the values
......
......@@ -10,9 +10,12 @@ import sys
from projectTools import GenTools,DroneCommand
from Regulator import RegulatorClass
from std_msgs.msg import Float32,Int32
from activation_class import NodeActivate
class ComputeZTar:
class ComputeZTar(NodeActivate):
def __init__(self):
super(ComputeZTar,self).__init__("compute_tar_z")
cmd_reset = rospy.Publisher("/reset_cmd_z",Int32,queue_size=1)
self.dc = DroneCommand(0.007,0,0)
self.centroid_subscriber = rospy.Subscriber("/centroids",Float32,self.read_centroid)
self.cmd_publisher = rospy.Publisher("/vel_in_z",Float32,queue_size=1)
......@@ -23,6 +26,8 @@ class ComputeZTar:
self.act_publisher.publish(1)
def read_centroid(self,ros_data):
#read the centroid and send the data
if(self.node_active == 0):
return
self.centroid = ros_data.data
self.cmd = self.dc.computeCommand(self.centroid,self.TargCentroid)
#Set the max value to 0.3 in absolute value
......
import rospy
from std_msgs.msg import Float32,Int32
......@@ -20,9 +20,11 @@ import math
from cv_bridge import CvBridge, CvBridgeError
from itertools import combinations, chain
import datetime
from activation_class import NodeActivate
class image_convert:
class image_convert(NodeActivate):
def __init__(self):
super(image_convert,self).__init__("detect_vanish")
self.pub = rospy.Publisher("/centroids", Float32, queue_size=1)
self.pubDiff = rospy.Publisher('/sDiffs',Float32,queue_size=1)
self.pubForBag = rospy.Publisher('/imgForBag/front/compressed', CompressedImage, queue_size=1)
......@@ -194,6 +196,8 @@ class image_convert:
def callback(self,ros_data):
#msg = ""
if(self.node_active == 0):
return
cv2_img = self.br.imgmsg_to_cv2(ros_data)
#np_arr = np.fromstring(ros_data.data, np.uint8)
#image_np = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
......@@ -211,7 +215,7 @@ def main(args):
rospy.init_node('image_feature', anonymous=True)
ic = image_convert()
rospy.spin()
ic.saveData()
#ic.saveData()
cv2.destroyAllWindows()
......
......@@ -18,6 +18,24 @@ class DroneCommand:
Derr = 0
self.cmd = self._P*err + self.TotErr*self._I + self._D*Derr
return self.cmd
class Sequence:
def __init__(self,mode='init'):
self._mode = mode
self._phase = 0
self._published=False
def get_phase(self):
return self._phase
def get_mode(self):
return self._mode
def get_publ(self):
return self._published
def set_phase(self,val):
self._phase = val
def set_published(self,val):
self._published = val
def reset_seq(self):
self._published = False
self._phase = 0
class GenTools:
def __init__(self):
pass
......
......@@ -14,9 +14,11 @@ import math
from cv_bridge import CvBridge, CvBridgeError
import random
from nav_msgs.msg import Odometry
from activation_class import NodeActivate
class EnterDoors:
class EnterDoors(NodeActivate):
def __init__(self):
super(EnterDoors,self).__init__("checkDoors")
self.br = CvBridge()
self.prvs = None
self.next = None
......@@ -28,16 +30,7 @@ class EnterDoors:
self.thresh = 200
self.dat_bin = None
self.my_vel = 0
self.res_pub_x = rospy.Publisher("/reset_cmd_x",Int32,queue_size=1)
self.res_pub_y = rospy.Publisher("/reset_cmd_y",Int32,queue_size=1)
self.res_pub_z = rospy.Publisher("/reset_cmd_z",Int32,queue_size=1)
rate = rospy.Rate(10)
for i in range(5):
self.res_pub_x.publish(1)
self.res_pub_y.publish(1)
self.res_pub_z.publish(1)
rate.sleep()
self.conf = 0
self.img_read = rospy.Subscriber("/bebop/image_raw/compressed",CompressedImage,self.transform_image)
self.img_pub = rospy.Publisher("/image_graph",Image,queue_size=1)
......@@ -47,19 +40,26 @@ class EnterDoors:
self.vel_pub_x = rospy.Publisher("/vel_in_x",Float32,queue_size=1)
self.vel_pub_y = rospy.Publisher("/vel_in_y",Float32,queue_size=1)
self.vel_pub_z = rospy.Publisher("/vel_in_z",Float32,queue_size=1)
print "TEST TEST"
self.door_pub = rospy.Publisher("/door_det",Int32,queue_size=1)
self.y_vel = 0
def update_vel(self,ros_data):
if(self.node_active == 0):
return
twist = ros_data.twist.twist
self.my_vel = np.absolute(twist.linear.y)
curr_pose = ros_data.pose.pose
cur_ang_quat = curr_pose.orientation.z
if(cur_ang_quat > 0):
self.y_vel = -0.3
else:
self.y_vel = 0.3
def transform_image(self,ros_data):
if(self.node_active == 0):
return
self.vel_pub_x.publish(0)
self.vel_pub_y.publish(0.3)
self.vel_pub_z.publish(0)
self.vel_pub_y.publish(self.y_vel)
cv2_image = self.br.compressed_imgmsg_to_cv2(ros_data)
cv2_image = cv2.resize(cv2_image, (320,240), interpolation = cv2.INTER_AREA)
cv2_roi = cv2_image[80:160,:]
......@@ -94,6 +94,14 @@ class EnterDoors:
if(m_var_x > 500):
#print "Door detected between "+str(left) + " and "+str(right)
cv2.circle(self.plot_image,(int((left+right)/2),100),3,(0,0,0),-1)
if(left < 20 and right > 300):
self.conf = self.conf + 1
else:
self.conf = 0
if(self.conf > 3):
#Here we can say that, we have a door
self.door_pub.publish(1)
def draw_and_dispaly(self):
flow = cv2.calcOpticalFlowFarneback(self.prvs,self.next, None, 0.5, 2, 15, 3, 5, 1.2, 0)
u = flow[...,0]
......
#!/usr/bin/env python
"""
This node has an input the target yaw angle and will send the corresponding velocity over the Z axis
The input node is the topic /desired_yaw and the output is the /vel_in_z after activating and reseting the Z
"""
import numpy as np
import rospy
import sys
from projectTools import GenTools,DroneCommand
from Regulator import RegulatorClass
from std_msgs.msg import Float32,Int32
from nav_msgs.msg import Odometry
from activation_class import NodeActivate
class TurnDrone(NodeActivate):
def __init__(self):
super(TurnDrone,self).__init__("turnAng")
self.tar_sub = rospy.Subscriber("/ang_in_z",Float32,self.read_tar)
self.cur_sub = rospy.Subscriber("/bebop/odom",Odometry,self.read_cur)
self.cmd_pub_z = rospy.Publisher("/vel_in_z",Float32,queue_size=1)
self.tar_ang = 90.0/180*np.pi
self.cur_ang = 0
self.dc = DroneCommand(0.5,0,0)
self.start = time.time()
self.end = time.time()
def read_tar(self,ros_data):
self.tar_ang = ros_data.data/180*np.pi
def read_cur(self,ros_data):
if(self.node_active == 0):
return
curr_pose = ros_data.pose.pose
cur_ang_quat = curr_pose.orientation.z
self.cur_ang = 2*np.arcsin(cur_ang_quat)
cmd = self.dc.computeCommand(self.cur_ang,self.tar_ang)
cmd = GenTools.setMax(cmd,0.3)
self.cmd_pub_z.publish(cmd)
def main(args):
rospy.init_node('TurnDrone', anonymous=True)
sc = TurnDrone()
#rospy.init_node('send_command', anonymous=True)
rospy.spin()
if __name__ == '__main__':
main(sys.argv)
......@@ -12,22 +12,27 @@ from Regulator import RegulatorClass
from std_msgs.msg import Float32,Int32
from nav_msgs.msg import Odometry
import time
from activation_class import NodeActivate
class TurnDrone:
class TurnDrone(NodeActivate):
def __init__(self):
super(TurnDrone,self).__init__("curveMotion")
self.tar_sub = rospy.Subscriber("/ang_in_z",Float32,self.read_tar)
self.cur_sub = rospy.Subscriber("/bebop/odom",Odometry,self.read_cur)
self.cmd_pub_z = rospy.Publisher("/vel_in_z",Float32,queue_size=1)
self.cmd_pub_x = rospy.Publisher("/vel_in_x",Float32,queue_size=1)
self.cmd_pub_y = rospy.Publisher("/vel_in_y",Float32,queue_size=1)
self.done_pub = rospy.Publisher("/turn_done",Int32,queue_size=1)
self.tar_ang = 90.0/180*np.pi
self.cur_ang = 0
self.dc = DroneCommand(0.5,0,0)
self.start = time.time()
self.end = time.time()
def read_tar(self,ros_data):
pass
self.tar_ang = ros_data.data/180*np.pi
def read_cur(self,ros_data):
if(self.node_active == 0):
return
curr_pose = ros_data.pose.pose
cur_ang_quat = curr_pose.orientation.z
if(self.end - self.start < 2):
......@@ -42,6 +47,9 @@ class TurnDrone:
self.cmd_pub_z.publish(cmd)
self.cmd_pub_x.publish(-0.1)
self.cmd_pub_y.publish(0.0)
if(np.absolute(self.cur_ang - self.tar_ang)< 0.1):
#Here we can consider that we have finished the turn
self.done_pub.publish(1)
def main(args):
rospy.init_node('TurnDrone', anonymous=True)
......
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