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 658c5f59 authored by jad's avatar jad

seq door tested

parent 8f825e32
<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="Main" type="Main.py">
</node>
<node pkg="drone_project" name="optFlow" type="testDenseOpticalFlow.py">
</node>
<node pkg="drone_project" name="curveMotion" type="turn_to_ang.py">
<node pkg="drone_project" name="curveMotion" type="turn_to_angle.py">
</node>
<node pkg="drone_project" name="turnAng" type="turn_ang.py">
</node>
<node pkg="drone_project" name="resetCmd" type="reset_cmd_node.py">
</node>
</launch>
\ No newline at end of file
......@@ -6,6 +6,7 @@ and will choose the nodes to activate sequentially
import rospy
from std_msgs.msg import String,Int32,Float32
from projectTools import Sequence
import sys
class Sequencer(object):
def __init__(self):
......@@ -35,9 +36,10 @@ class Sequencer(object):
self.doors_seq.set_published(False)
def read_mode(self,ros_data):
self.mode = ros_data.data
self.loop_pub.publish(1)
self.loop_pub.publish(3)
def enter_loop(self,ros_data):
if(self.mode == "init"):
self.doors_seq.reset_seq()
self.reset_seq_func()
pass
if(self.mode == self.doors_seq.get_mode()):
......@@ -50,6 +52,7 @@ class Sequencer(object):
self.hallway_seq_func()
pass
def reset_seq_func(self):
self.actv_pub.publish("reset")
self.actv_pub.publish("resetCmd_1")
def stairs_seq_func(self):
pass
......@@ -75,12 +78,17 @@ class Sequencer(object):
self.rate.sleep()
self.doors_seq.set_published(True)
if(self.mode != "init"):
self.loop_pub.publish(1)
self.loop_pub.publish(2)
self.rate.sleep()
if(self.phase == 2):
if(self.doors_seq.get_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
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
......@@ -9,7 +9,7 @@ class NodeActivate(object):
self.node_active = 0
def check_act(self,ros_data):
msg = ros_data.data
if(msg == reset):
if(msg == 'reset'):
self.node_active = 0
else:
msg,act = msg.split("_")
......
......@@ -12,14 +12,15 @@ from std_msgs.msg import Float32
class XCommand(RegulatorClass):
def __init__(self):
super(XCommand,self).__init__(0.7,0.03,0.6)
#super(XCommand,self).__init__(0.7,0.03,0.6)
super(XCommand,self).__init__(0.8,0.0,0.7)
self.deb = rospy.Publisher("/deb_x",Float32,queue_size=1)
def read_val(self,ros_data):
twist = ros_data.twist.twist
self.currVal = twist.linear.x
self.deb.publish(0)
self.cmd = self.dc.computeCommand(self.currVal,self.targVal)
if(self.reset_ack == 1 and self.data_rec == 1):
if(self.data_rec == 1):
self.cmd_publisher.publish(self.cmd)
self.data_rec = 0
print "Val Read"
......
......@@ -28,7 +28,7 @@ class YCommand(RegulatorClass):
#ycmd = self.dc.computeCommand(self.currVal,self.targVal)
#self.cmd_publisher.publish(ycmd)
self.cmd = self.dc.computeCommand(self.currVal,self.targVal)
if(self.reset_ack == 1 and self.data_rec == 1):
if(self.data_rec == 1):
self.cmd_publisher.publish(self.cmd)
self.data_rec = 0
......
......@@ -17,10 +17,12 @@ class ZCommand(RegulatorClass):
def read_tar(self,ros_data):
#if(self.activation == 0):
# return
self.cmd_publisher.publish(ros_data.data)
#self.cmd_publisher.publish(ros_data.data)
self.cmd = ros_data.data
self.data_rec = 1
def read_val(self,ros_data):
self.cmd = self.dc.computeCommand(self.currVal,self.targVal)
if(self.reset_ack == 1 and self.data_rec == 1):
#self.cmd = self.dc.computeCommand(self.currVal,self.targVal)
if(self.data_rec == 1):
self.cmd_publisher.publish(self.cmd)
self.data_rec = 0
......
......@@ -23,9 +23,9 @@ def talker():
rate.sleep()
start = time.time()
end = time.time()
while end - start < 1:
while end - start < 8:
end = time.time()
velx = 0.2
velx = 0.3
vely = 0.0
velz = 0.0
......@@ -36,17 +36,6 @@ def talker():
rate.sleep()
start = time.time()
end = time.time()
while end - start < 4:
end = time.time()
velx = 0.0
vely = 0.0
velz = -0.4
pubvel1.publish(velx)
pubvel2.publish(vely)
pubvel3.publish(velz)
rate.sleep()
if __name__ == '__main__':
try:
talker()
......
No preview for this file type
......@@ -20,9 +20,16 @@ class resetCmd(NodeActivate):
def reset_cmd_func(self,ros_data):
if(self.node_active == 0):
return
for i in range(10) :
for i in range(5) :
self.pubres1.publish(1)
self.pubres2.publish(1)
self.pubres3.publish(1)
self.rate.sleep()
self.node_active = 0
\ No newline at end of file
self.node_active = 0
def main(args):
rospy.init_node('resetCmd', anonymous=True)
sc = resetCmd()
#rospy.init_node('send_command', anonymous=True)
rospy.spin()
if __name__ == '__main__':
main(sys.argv)
\ No newline at end of file
......@@ -58,7 +58,7 @@ class EnterDoors(NodeActivate):
def transform_image(self,ros_data):
if(self.node_active == 0):
return
self.vel_pub_x.publish(0)
self.vel_pub_x.publish(0.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)
......
......@@ -7,6 +7,7 @@ The input node is the topic /desired_yaw and the output is the /vel_in_z after a
import numpy as np
import rospy
import sys
import time
from projectTools import GenTools,DroneCommand
from Regulator import RegulatorClass
from std_msgs.msg import Float32,Int32
......
......@@ -26,27 +26,36 @@ class TurnDrone(NodeActivate):
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()
self.start = -1
self.end = -1
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):
self.start = -1
self.end = -1
return
if(self.start == -1):
self.start = time.time()
self.end = time.time()
curr_pose = ros_data.pose.pose
cur_ang_quat = curr_pose.orientation.z
if(self.end - self.start < 2):
self.end = time.time()
self.cmd_pub_z.publish(0.0)
self.cmd_pub_x.publish(0.2)
self.cmd_pub_x.publish(0.3)
self.cmd_pub_y.publish(0.0)
else:
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)
self.cmd_pub_x.publish(-0.1)
self.cmd_pub_y.publish(0.0)
if(np.absolute(self.cur_ang) > 80):
self.cmd_pub_x.publish(0.2)
self.cmd_pub_y.publish(-0.3)
else:
self.cmd_pub_x.publish(0.0)
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)
......
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