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 01a51dcc authored by jad's avatar jad

test passed

parent 92d6fc05
......@@ -9,8 +9,6 @@
</node>
<node pkg="drone_project" name="turnAng" type="turn_ang.py">
</node>
<node pkg="drone_project" name="resetCmd" type="reset_cmd_node.py">
</node>
<node pkg="drone_project" name="computeTarX" type="compute_tar_x.py">
</node>
<node pkg="drone_project" name="computeTarY" type="compute_tar_y.py">
......
......@@ -33,8 +33,10 @@ class RegulatorClass(object):
#self.cmd = self.dc.computeCommand(self.currVal,self.targVal)
#self.cmd_publisher.publish(self.cmd)
print "cmd sent"
def reset_cmd(self,ros_data):
def myreset_cmd(self):
self.dc.cmd = 0
self.dc.TotErr = 0
self.dc.oldErr = 0
self.reset_ack = 1
\ No newline at end of file
self.reset_ack = 1
def reset_cmd(self,ros_data):
pass
\ No newline at end of file
No preview for this file type
......@@ -6,14 +6,14 @@ 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_pub = rospy.Publisher("/activations",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)
......@@ -21,23 +21,29 @@ class Sequencer(object):
"""
Init should be a list and not a name
"""
doors_seq_list = ["doors",[["curveMotion"],["curveMotion"]],[["checkDoors","turnAng"],["checkDoors"]]]
init_seq_list = ["init",[ ["resetCmd"],[] ] ]
hallway_seq_list = ["hallways",[["detectVanish","computeTarX","computeTarY","computeTarZ"], [] ] ]
self.doors_seq = Sequence(doors_seq_list)
self.init_seq = Sequence(init_seq_list)
self.hallways_seq = Sequence(hallway_seq_list)
self.doors_seq_list = ["doors",[["curveMotion"],["curveMotion"]],[["checkDoors","turnAng"],["checkDoors"]]]
self.init_seq_list = ["init",[ ["resetCmd"],[] ] ]
self.hallway_seq_list = ["hallways",[["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.rate = rospy.Rate(20) #10Hz
def read_mode(self,ros_data):
self.mode = ros_data.data
self.loop_pub.publish(3)
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)
else:
self.loop_pub.publish(3)
def enter_loop(self,ros_data):
if(self.mode == self.init_seq.get_mode()):
self.doors_seq.reset_seq()
self.hallways_seq.reset_seq()
#self.new_seq.reset_seq()
self.init_seq.seq_func()
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()):
......
import numpy as np
import rospy
import sys
import time
from projectTools import GenTools,DroneCommand
from Regulator import RegulatorClass
......@@ -8,7 +8,7 @@ from std_msgs.msg import Float32,Int32
from nav_msgs.msg import Odometry
from activation_class import NodeActivate,returnResp
class TurnDrone(NodeActivate,returnResp):
class TurnDrone(NodeActivate):
def __init__(self,tar_in):
super(TurnDrone,self).__init__("turnAng")
#self.tar_sub = rospy.Subscriber("/ang_in_z",Float32,self.read_tar)
......
......@@ -4,7 +4,7 @@ from std_msgs.msg import String
from drone_project.msg import actMsg
class returnResp(object):
def __init__(self,name):
self.ret_pub = rospy.Publisher("/return_resp",String,queue_size=1)
self.ret_pub = rospy.Publisher("/return_resp",String,queue_size=10)
self._name = name
def send_conf(self):
self.ret_pub.publish(self._name)
......@@ -12,7 +12,10 @@ class NodeActivate(object):
def __init__(self,my_name):
self._my_name = my_name
self.act_sub = rospy.Subscriber("/activations",actMsg,self.check_act)
self.ret_pub = rospy.Publisher("/return_resp",String,queue_size=10)
self.node_active = 0
def send_conf(self):
self.ret_pub.publish(self._my_name)
def check_act(self,ros_data):
node_name = ros_data.node_name
active = ros_data.activate
......
No preview for this file type
......@@ -18,7 +18,7 @@ class XCommand(RegulatorClass):
self.deb = rospy.Publisher("/deb_x",Float32,queue_size=1)
def read_val(self,ros_data):
if(np.absolute(self.last_rec - time.time()) > 0.5):
self.reset_cmd()
self.myreset_cmd()
twist = ros_data.twist.twist
self.currVal = twist.linear.x
self.deb.publish(0)
......
......@@ -24,7 +24,7 @@ class YCommand(RegulatorClass):
#if(self.activation == 0):
# return
if(np.absolute(self.last_rec - time.time()) > 0.5):
self.reset_cmd()
self.myreset_cmd()
twist = ros_data.twist.twist
self.currVal = twist.linear.y
self.targVal = GenTools.setMax(self.targVal,0.3)
......
......@@ -23,7 +23,7 @@ class ZCommand(RegulatorClass):
self.data_rec = 1
def read_val(self,ros_data):
if(np.absolute(self.last_rec - time.time()) > 0.5):
self.reset_cmd()
self.myreset_cmd()
#self.cmd = self.dc.computeCommand(self.currVal,self.targVal)
if(self.data_rec == 1):
self.cmd_publisher.publish(self.cmd)
......
......@@ -37,10 +37,11 @@ class Sequence:
self.nodes_to_activate = self._seq[self._phase][0]
self.conds_to_wait = self._seq[self._phase][1]
self._verf_conds = [False]*len(self.conds_to_wait)
self.actv_pub = rospy.Publisher("/activations",actMsg,queue_size=1)
self.actv_pub = rospy.Publisher("/activations",actMsg,queue_size=10)
self.resp_sub = rospy.Subscriber("/return_resp",String,self.updateConds)
self.loop_pub = rospy.Publisher("/loop_sep",Int32,queue_size=1)
self.loop_pub = rospy.Publisher("/loop_seq",Int32,queue_size=1)
self.mode_pub = rospy.Publisher("/mode",String,queue_size=1)
self.deb_pub3 = rospy.Publisher("/deb_pub3",String,queue_size=1)
self.rate = rospy.Rate(20) #20Hz
def reset_seq(self):
self._phase = 0
......@@ -52,8 +53,10 @@ class Sequence:
self._verf_conds[ind] = True
def seq_fun(self):
self.nodes_to_activate = self._seq[self._phase][0]
self.conds_to_wait = self._seq[self._phase][1]
initPubl = False
if(self._phase < self._phase_nb):
self.nodes_to_activate = self._seq[self._phase][0]
self.conds_to_wait = self._seq[self._phase][1]
#First we get the nodes we want
if(self._published == False):
self.send_activations()
......@@ -64,12 +67,16 @@ class Sequence:
self._verf_conds = [False]*len(self._seq[self._phase][1])
self.nodes_to_activate = self._seq[self._phase][0]
self.conds_to_wait = self._seq[self._phase][1]
self.loop_pub.publish(1)
else:
#The sequence is over here
self.mode_pub.publish("init")
initPubl = True
if(self._mode != "init"):
self.mode_pub.publish("init")
if(initPubl == False and self._mode != "init"):
self.loop_pub.publish(1)
self.rate.sleep()
def send_activations():
def send_activations(self):
#reset message and send some
self._published = True
msg = actMsg()
......@@ -81,7 +88,9 @@ class Sequence:
msg.activate = True
self.actv_pub.publish(msg)
self.rate.sleep()
def wait_conds():
def wait_conds(self):
ret_val = all(item == True for item in self._verf_conds)
self.deb_pub3.publish(str(ret_val))
return all(item == True for item in self._verf_conds)
def get_mode(self):
return self._mode
......
No preview for this file type
......@@ -25,7 +25,7 @@ class resetCmd(NodeActivate):
self.pubres2.publish(1)
self.pubres3.publish(1)
self.rate.sleep()
self.node_active = 0
self.node_active = False
def main(args):
rospy.init_node('resetCmd', anonymous=True)
sc = resetCmd()
......
......@@ -54,9 +54,9 @@ class EnterDoors(NodeActivate,returnResp):
curr_pose = ros_data.pose.pose
cur_ang_quat = curr_pose.orientation.z
if(cur_ang_quat > 0):
self.y_vel = -0.3
self.y_vel = -0.4
else:
self.y_vel = 0.3
self.y_vel = 0.4
def transform_image(self,ros_data):
if(self.node_active == False):
......@@ -95,7 +95,7 @@ class EnterDoors(NodeActivate,returnResp):
#var_y = np.var(tmp_pixels_y,axis = 1)
m_var_x = np.mean(var_x)
#m_var_y = np.mean(var_y)
if(m_var_x > 500 and abs(time.time() - self.act_time) > 1):
if(m_var_x > 500 and abs(time.time() - self.act_time) > 3):
#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):
......
#!/usr/bin/env python
from TurnAngClass import TurnDrone
import sys
import rospy
def main(args):
rospy.init_node('TurnDrone', anonymous=True)
rospy.init_node('TurnDrone2', anonymous=True)
sc = TurnDrone(90)
#rospy.init_node('send_command', anonymous=True)
rospy.spin()
......
......@@ -14,7 +14,7 @@ from nav_msgs.msg import Odometry
import time
from activation_class import NodeActivate,returnResp
class TurnDrone(NodeActivate,returnResp):
class TurnDrone(NodeActivate):
def __init__(self):
super(TurnDrone,self).__init__("curveMotion")
self.tar_sub = rospy.Subscriber("/ang_in_z",Float32,self.read_tar)
......@@ -60,9 +60,10 @@ class TurnDrone(NodeActivate,returnResp):
#Here we can consider that we have finished the turn
#self.done_pub.publish(1)
self.send_conf()
#pass
def main(args):
rospy.init_node('TurnDrone', anonymous=True)
rospy.init_node('TurnDrone3', anonymous=True)
sc = TurnDrone()
#rospy.init_node('send_command', anonymous=True)
rospy.spin()
......
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