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

some adjs

parent df5bb493
......@@ -6,7 +6,7 @@ from nav_msgs.msg import Odometry
import sys
from projectTools import DroneCommand
from std_msgs.msg import Float32,Int32
import time
class RegulatorClass(object):
def __init__(self,P,I,D):
self.odom_subscriber = rospy.Subscriber("/curr_in",Odometry,self.read_val)
......@@ -21,6 +21,7 @@ class RegulatorClass(object):
self.targVal = 0
self.reset_ack = 0
self.data_rec = 0
self.last_rec = -1
# self.activation = 0
def read_tar(self,ros_data):
print "tar received"
......@@ -28,6 +29,7 @@ class RegulatorClass(object):
# return
self.targVal = ros_data.data
self.data_rec = 1
self.last_rec = time.time()
#self.cmd = self.dc.computeCommand(self.currVal,self.targVal)
#self.cmd_publisher.publish(self.cmd)
print "cmd sent"
......
......@@ -9,13 +9,16 @@ from nav_msgs.msg import Odometry
import sys
from Regulator import RegulatorClass
from std_msgs.msg import Float32
import time
class XCommand(RegulatorClass):
def __init__(self):
#super(XCommand,self).__init__(0.7,0.03,0.6)
super(XCommand,self).__init__(0.8,0.0,0.7)
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):
if(np.absolute(self.last_rec - time.time()) > 0.5):
self.reset_cmd()
twist = ros_data.twist.twist
self.currVal = twist.linear.x
self.deb.publish(0)
......
......@@ -11,6 +11,7 @@ import sys
from projectTools import GenTools
from std_msgs.msg import Float32
from Regulator import RegulatorClass
import time
class YCommand(RegulatorClass):
......@@ -22,6 +23,8 @@ class YCommand(RegulatorClass):
#We need to compute the command first in this method
#if(self.activation == 0):
# return
if(np.absolute(self.last_rec - time.time()) > 0.5):
self.reset_cmd()
twist = ros_data.twist.twist
self.currVal = twist.linear.y
self.targVal = GenTools.setMax(self.targVal,0.3)
......
......@@ -9,6 +9,7 @@ import sys
from projectTools import GenTools
from Regulator import RegulatorClass
from std_msgs.msg import Float32,Int32
import time
class ZCommand(RegulatorClass):
def __init__(self):
......@@ -21,6 +22,8 @@ class ZCommand(RegulatorClass):
self.cmd = ros_data.data
self.data_rec = 1
def read_val(self,ros_data):
if(np.absolute(self.last_rec - time.time()) > 0.5):
self.reset_cmd()
#self.cmd = self.dc.computeCommand(self.currVal,self.targVal)
if(self.data_rec == 1):
self.cmd_publisher.publish(self.cmd)
......
......@@ -15,6 +15,7 @@ from cv_bridge import CvBridge, CvBridgeError
import random
from nav_msgs.msg import Odometry
from activation_class import NodeActivate,returnResp
import time
class EnterDoors(NodeActivate,returnResp):
def __init__(self):
......@@ -43,8 +44,10 @@ class EnterDoors(NodeActivate,returnResp):
self.door_pub = rospy.Publisher("/door_det",Int32,queue_size=1)
self.y_vel = 0
self.act_time = -1
def update_vel(self,ros_data):
if(self.node_active == 0):
self.act_time = time.time()
return
twist = ros_data.twist.twist
self.my_vel = np.absolute(twist.linear.y)
......@@ -57,6 +60,7 @@ class EnterDoors(NodeActivate,returnResp):
def transform_image(self,ros_data):
if(self.node_active == False):
self.act_time = time.time()
return
self.vel_pub_x.publish(0.0)
self.vel_pub_y.publish(self.y_vel)
......@@ -91,14 +95,14 @@ 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):
if(m_var_x > 500 and abs(time.time() - self.act_time) > 1):
#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):
if(self.conf > 3 ):
#Here we can say that, we have a door
#self.door_pub.publish(1)
self.send_conf()
......
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