Commit 6f6a0649 authored by jad's avatar jad

test new seq

parent e8db3c18
......@@ -14,6 +14,7 @@ find_package(catkin REQUIRED COMPONENTS
rospy
sensor_msgs
std_msgs
message_generation
)
## System dependencies are found with CMake's conventions
......@@ -50,11 +51,10 @@ find_package(catkin REQUIRED COMPONENTS
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
add_message_files(
FILES
actMsg.msg
)
## Generate services in the 'srv' folder
# add_service_files(
......@@ -71,10 +71,10 @@ find_package(catkin REQUIRED COMPONENTS
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# geometry_msgs# nav_msgs# sensor_msgs# std_msgs
# )
generate_messages(
DEPENDENCIES
geometry_msgs nav_msgs sensor_msgs std_msgs
)
################################################
## Declare ROS dynamic reconfigure parameters ##
......@@ -108,7 +108,7 @@ find_package(catkin REQUIRED COMPONENTS
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES drone_project
# CATKIN_DEPENDS cv_bridge geometry_msgs nav_msgs rospy sensor_msgs std_msgs
CATKIN_DEPENDS cv_bridge geometry_msgs nav_msgs rospy sensor_msgs std_msgs message_runtime
# DEPENDS system_lib
)
......
string node_name
bool activate
......@@ -7,7 +7,7 @@
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="jad@todo.todo">jad</maintainer>
<maintainer email="jadabdulrahman.obeid@supelec.fr">jad</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
......@@ -37,13 +37,13 @@
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<build_depend>message_generation</build_depend>
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<exec_depend>message_runtime</exec_depend>
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
......
......@@ -16,84 +16,28 @@ class Sequencer(object):
self.loop_pub = rospy.Publisher("/loop_seq",Int32,queue_size=1)
self.loop_sub = rospy.Subscriber("/loop_seq",Int32,self.enter_loop)
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.init_seq = Sequence("init")
"""
Init should be a list and not a name
"""
doors_seq_list = ["doors",[["curveMotion"],["curveMotion"]],[["checkDoors","turnAng"],["checkDoors"]]]
init_seq_list = ["init",[ ["resetCmd"],[] ] ]
self.doors_seq = Sequence(doors_seq_list)
self.init_seq = Sequence(init_seq_list)
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
self.loop_pub.publish(3)
def enter_loop(self,ros_data):
if(self.mode == "init"):
if(self.mode == self.init_seq.get_mode()):
self.doors_seq.reset_seq()
self.hallway_seq.reset_seq()
self.reset_seq_func()
pass
#self.hallway_seq.reset_seq()
self.init_seq.seq_func()
if(self.mode == self.doors_seq.get_mode()):
self.doors_seq_func()
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 reset_seq_func(self):
self.actv_pub.publish("reset")
self.actv_pub.publish("resetCmd_1")
def stairs_seq_func(self):
pass
def hallway_seq_func(self):
if(hallway_seq.get_publ() == False):
self.actv_pub.publish("reset")
self.rate.sleep()
self.actv_pub.publish("detectVanish_1")
self.actv_pub.publish("computeTarX_1")
self.actv_pub.publish("computeTarY_1")
self.actv_pub.publish("computeTarZ_1")
self.rate.sleep()
self.doors_seq.set_published(True)
def doors_seq_func(self):
if(self.doors_seq.get_phase() == 0 ):
if (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.mode != "init"):
self.loop_pub.publish(1)
self.rate.sleep()
if(self.doors_seq.get_phase() == 1):
if(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.mode != "init"):
self.loop_pub.publish(2)
self.rate.sleep()
if(self.doors_seq.get_phase() == 2):
self.actv_pub.publish("reset")
self.rate.sleep()
self.mode_pub.publish('init')
self.doors_seq.seq_fun()
def main(args):
rospy.init_node('Sequencer', anonymous=True)
sc = Sequencer()
......
#!/usr/bin/env python
import rospy
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._name = name
def send_conf(self):
self.ret_pub.publish(self._name)
class NodeActivate(object):
def __init__(self,my_name):
self._my_name = my_name
self.act_sub = rospy.Subscriber("/activations",String,self.check_act)
self.act_sub = rospy.Subscriber("/activations",actMsg,self.check_act)
self.node_active = 0
def check_act(self,ros_data):
msg = ros_data.data
if(msg == 'reset'):
self.node_active = 0
node_name = ros_data.node_name
active = ros_data.activate
if(node_name == 'reset'):
self.node_active = False
else:
msg,act = msg.split("_")
if(msg == self._my_name):
self.node_active = act
\ No newline at end of file
if(node_name == self._my_name):
self.node_active = active
\ No newline at end of file
......@@ -20,7 +20,7 @@ import math
from cv_bridge import CvBridge, CvBridgeError
from itertools import combinations, chain
import datetime
from activation_class import NodeActivate
from activation_class import NodeActivate,returnResp
class image_convert(NodeActivate):
def __init__(self):
......
#!/usr/bin/env python
import rospy
from std_msgs.msg import String,Int32
from drone_project.msg import actMsg
class DroneCommand:
def __init__(self,P,I,D):
self._P = P
......@@ -18,24 +23,70 @@ 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
def __init__(self,seqList):
self._seqList = seqList
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
self._mode = seqList[0]
self._seq = seqList[1:]
self._phase_nb = len(self._seq)
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.resp_sub = rospy.Subscriber("/return_resp",String,self.updateConds)
self.loop_pub = rospy.Publisher("/loop_sep",Int32,queue_size=1)
self.mode_pub = rospy.Publisher("/mode",String,queue_size=1)
self.rate = rospy.Rate(20) #20Hz
def reset_seq(self):
self._published = False
self._phase = 0
self._published = False
def updateConds(self,ros_data):
received = ros_data.data
ind = self.conds_to_wait.index(received)
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]
#First we get the nodes we want
if(self._published == False):
self.send_activations()
if(self.wait_conds()):
self._phase = self._phase + 1
if(self._phase < self._phase_nb):
self._published = False
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")
def send_activations():
#reset message and send some
self._published = True
msg = actMsg()
msg.node_name = "reset"
msg.activate = True
self.actv_pub.publish(msg)
for i in range(len(self.nodes_to_activate)):
msg.node_name = self.nodes_to_activate[i]
msg.activate = True
self.actv_pub.publish(msg)
self.rate.sleep()
def wait_conds():
return all(item == True for item in self._verf_conds)
def get_mode(self):
return self._mode
##########################################################################################################
class GenTools:
def __init__(self):
pass
......
......@@ -18,9 +18,9 @@ class resetCmd(NodeActivate):
self.rate = rospy.Rate(10)
def reset_cmd_func(self,ros_data):
if(self.node_active == 0):
if(self.node_active == False):
return
for i in range(5) :
for i in range(5):
self.pubres1.publish(1)
self.pubres2.publish(1)
self.pubres3.publish(1)
......
......@@ -14,9 +14,9 @@ import math
from cv_bridge import CvBridge, CvBridgeError
import random
from nav_msgs.msg import Odometry
from activation_class import NodeActivate
from activation_class import NodeActivate,returnResp
class EnterDoors(NodeActivate):
class EnterDoors(NodeActivate,returnResp):
def __init__(self):
super(EnterDoors,self).__init__("checkDoors")
self.br = CvBridge()
......@@ -56,7 +56,7 @@ class EnterDoors(NodeActivate):
self.y_vel = 0.3
def transform_image(self,ros_data):
if(self.node_active == 0):
if(self.node_active == False):
return
self.vel_pub_x.publish(0.0)
self.vel_pub_y.publish(self.y_vel)
......@@ -100,7 +100,8 @@ class EnterDoors(NodeActivate):
self.conf = 0
if(self.conf > 3):
#Here we can say that, we have a door
self.door_pub.publish(1)
#self.door_pub.publish(1)
self.send_conf()
def draw_and_dispaly(self):
flow = cv2.calcOpticalFlowFarneback(self.prvs,self.next, None, 0.5, 2, 15, 3, 5, 1.2, 0)
......
......@@ -12,9 +12,9 @@ from Regulator import RegulatorClass
from std_msgs.msg import Float32,Int32
from nav_msgs.msg import Odometry
import time
from activation_class import NodeActivate
from activation_class import NodeActivate,returnResp
class TurnDrone(NodeActivate):
class TurnDrone(NodeActivate,returnResp):
def __init__(self):
super(TurnDrone,self).__init__("curveMotion")
self.tar_sub = rospy.Subscriber("/ang_in_z",Float32,self.read_tar)
......@@ -22,7 +22,7 @@ class TurnDrone(NodeActivate):
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.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)
......@@ -31,7 +31,7 @@ class TurnDrone(NodeActivate):
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):
if(self.node_active == False):
self.start = -1
self.end = -1
return
......@@ -58,7 +58,8 @@ class TurnDrone(NodeActivate):
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)
#self.done_pub.publish(1)
self.send_conf()
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