def start_detect(self):
FunctionUnit.init_node(self)
#print 'hello 1'
#print self._receive_topic
receive_1 = FunctionUnit.create_receive(self, self._receive_topic, Image, self.receive_1_cb)
#print 'hello 2'
FunctionUnit.spin(self)
def __init__(self, node_name, receive_topic, send_topic, virtual_off= None):
FunctionUnit.__init__(self, node_name)
self._receive_topic = receive_topic
self._send_topic = send_topic
self._virtual = virtual_off
self.br = CvBridge()
self._virtual_send = FunctionUnit.create_send(self, virtual_off, Bool)
def __init__(self, node_name, topic_1 = None, msg_type_1 = None, send_topic_1 = None, topic_2 = None, msg_type_2 = None, send_topic_2 = None):
self._motive_topic_1=node_name+'/activate'
if not(topic_1 == None) and not(msg_type_1 == None):
self._topic_1 = topic_1
self._msg_type_1 = msg_type_1
self._send_topic_1=send_topic_1
self.send_1 = FunctionUnit.create_send(self, self._send_topic_1, self._msg_type_1)
else:
self._topic_1 = None
self._msg_type_1 = None
if not(topic_2 == None) and not(msg_type_2 == None):
self._topic_2 = topic_2
self._msg_type_2 = msg_type_2
self._send_topic_2=send_topic_2
self.send_2 = FunctionUnit.create_send(self, self._send_topic_2, self._msg_type_2)
else:
self._topic_2 = None
#print 'init'
FunctionUnit.__init__(self, node_name)
self.motivational_shared_var = False #'True' means the behavior set is activated
self._action_mode = False
self._last_goal = GoalID()
def __init__(self, node_name, robot_id, behavior_id, switch_topic, heartbeat_topic = None):
if heartbeat_topic == None:
self._heartbeat_topic = 'heart_beat'
#param
self._fast = 20#fast rate, constant for now
self._slow = 3#slow rate,constant for now
self._comm_influence_time=10
self._comm_keep_time=20
self._reset_influence_time=10
self._acquienscence_time_0=20#if other robot is runing the same behavior
self._acquienscence_time_1=60
self._acquienscence_influence_time=5
self._robot_id=robot_id#i
self._behavior_id=behavior_id#j
self._switch_topic=switch_topic
self._broadcast_rate=5
self._random_inc = False
self._node_name=node_name
FunctionUnit.__init__(self, node_name)
self._threshold_of_activation = 100#The initial value of threshold_of_activation is 100.
self._sensory_feedback = 0#If the sensory information is valid, the sensory feedback is true, the value is 1 or 0.
self._comm_recieved = [[0 for col in range(1)] for row in range(10)]#_comm_received,row=k,col=t
self._activity_suppression = 1
self._impatience = self._fast
self._impatience_reset = 1#if this robot is running an other behavior,reset=0
self._acquiescence = 1#if this behavior has run too long, acquiescence=0
self._m = 0#final score
self._time = 0
self._active=0#bool, active=0 else=1
self._active_time = 0#when this behavior is active
def __init__(self, node_name, receive_topic, send_topic, virtual_off=None):
FunctionUnit.__init__(self, node_name)
self._receive_topic = receive_topic
self._send_topic = send_topic
self._virtual = virtual_off
self.br = CvBridge()
self._virtual_send = FunctionUnit.create_send(self, virtual_off, Bool)
def start_detect(self):
FunctionUnit.init_node(self)
#print 'hello 1'
#print self._receive_topic
receive_1 = FunctionUnit.create_receive(self, self._receive_topic,
Image, self.receive_1_cb)
#print 'hello 2'
FunctionUnit.spin(self)
def run(self):
FunctionUnit.init_node(self)
receive_3 = FunctionUnit.create_receive(
self, self._motive_topic_1, Bool,
self.motivational_shared_var_update)
self.set_switch()
FunctionUnit.spin(self)
pass
def add_action(self, sub_action_name, pub_action_name):
#self._action=action_client
self._action_mode = True
self._recieve_action_cancel = FunctionUnit.create_receive(
self, sub_action_name + '/goal', MoveBaseActionGoal,
self.receive_cb_goal)
self._send_cancel = FunctionUnit.create_send(
self, pub_action_name + '/cancel', GoalID)
def __init__(self, node_name, pose_0, pose_1, pose_2, pose_3, topic, wait_time):
FunctionUnit.__init__(self, node_name)
#self._node_name = node_name
self._pose_0 = pose_0
self._pose_1 = pose_1
self._pose_2 = pose_2
self._pose_3 = pose_3
self._topic = topic
self._wait_time = wait_time
def __init__(self, node_name):
self._input_list = []
self._output_list = []
self._cb_list = []
self._shared_list = []
FunctionUnit.__init__(self, node_name)
self._action_mode = False
self._last_goal = GoalID()
def __init__(self, node_name, pose_0, pose_1, pose_2, pose_3, topic,
wait_time):
FunctionUnit.__init__(self, node_name)
#self._node_name = node_name
self._pose_0 = pose_0
self._pose_1 = pose_1
self._pose_2 = pose_2
self._pose_3 = pose_3
self._topic = topic
self._wait_time = wait_time
def add_pair(self, input_topic, topic_type, output_topic):
self._output_list.append(
FunctionUnit.create_send(self, output_topic, topic_type))
self._cb_list.append(
SwitchCallback(self._output_list[self._list_count],
self._shared_list))
self._input_list.append(
FunctionUnit.create_receive(self, input_topic, topic_type,
self._cb_list[self._list_count]))
self._list_count += 1
def start_switch(self):
FunctionUnit.init_node(self)
#print 'run'
if not(self._topic_1 == None):
#receive_1 = FunctionUnit.create_receive(self, self._topic_1, self._msg_type_1, self.receive_cb_1)
self.add_pair(self._topic_1,self._msg_type_1,self._send_topic_1)
if not(self._topic_2 == None):
#receive_2 = FunctionUnit.create_receive(self, self._topic_2, self._msg_type_2, self.receive_cb_2)
self.add_pair(self._topic_2,self._msg_type_2,self._send_topic_2)
receive_3 = FunctionUnit.create_receive(self, self._motive_topic_1, Bool, self.motivational_shared_var_update)
FunctionUnit.spin(self)
def __init__(self, node_name, receive_topic_follower, receive_topic_intruder, send_topic):
FunctionUnit.__init__(self, node_name)
self._receive_topic_follower = receive_topic_follower
self._receive_topic_intruder = receive_topic_intruder
self.follower_pose_x = 0.0
self.follower_pose_y = 0.0
self.intruder_pose_x = 0.0
self.intruder_pose_y = 0.0
self.follow_distance = 0.5 # The follower will be away from the intruder according to this distance
self.linear_velocity = 0.3
self.angular_velocity = 0.5
self.send = FunctionUnit.create_send(self, send_topic, Twist)
def __init__(self, node_name):
self._input_list = []
self._output_list = []
self._cb_list = []
self._shared_list = []
self._shared_list.append(False)
self._list_count = 0
self._motive_topic_1 = 'activate'
FunctionUnit.__init__(self, node_name)
self._action_mode = False
self._last_goal = GoalID()
def __init__(self, node_name):
self._input_list=[]
self._output_list=[]
self._cb_list=[]
self._shared_list=[]
self._shared_list.append(False)
self._list_count = 0
self._motive_topic_1='activate'
FunctionUnit.__init__(self, node_name)
self._action_mode = False
self._last_goal = GoalID()
def start_switch(self):
FunctionUnit.init_node(self)
#print 'run'
if not (self._topic_1 == None):
#receive_1 = FunctionUnit.create_receive(self, self._topic_1, self._msg_type_1, self.receive_cb_1)
self.add_pair(self._topic_1, self._msg_type_1, self._send_topic_1)
if not (self._topic_2 == None):
#receive_2 = FunctionUnit.create_receive(self, self._topic_2, self._msg_type_2, self.receive_cb_2)
self.add_pair(self._topic_2, self._msg_type_2, self._send_topic_2)
receive_3 = FunctionUnit.create_receive(
self, self._motive_topic_1, Bool,
self.motivational_shared_var_update)
FunctionUnit.spin(self)
def run(self):
FunctionUnit.init_node(self)
thread.start_new_thread(self.timer,())
heartbeat_receive = FunctionUnit.create_receive(self, self._heartbeat_topic, Heartbeat, self.heartbeat_on_received)
self._heartbeat_send = FunctionUnit.create_send(self, 'heart_beat', Heartbeat)
self._switch_send = FunctionUnit.create_send(self, self._switch_topic, Bool)
#sensory_receive = FunctionUnit.create_receive(self, self._node_name+"/sensory_feedback", Bool, self.sensory_on_received)
FunctionUnit.spin(self)
def __init__(self, node_name, robot_id, behavior_id, switch_topic, filename = None,heartbeat_topic = None):
if heartbeat_topic == None:
self._heartbeat_topic = 'heart_beat'
#param
self._fast = 20#fast rate, constant for now
self._slow = 3#slow rate,constant for now
self._comm_influence_time=10
self._comm_keep_time=20
self._reset_influence_time=10
self._acquienscence_time_0=20#if other robot is runing the same behavior
self._acquienscence_time_1=60
self._acquienscence_influence_time=5
self._robot_id=robot_id#i
self._behavior_id=behavior_id#j
self._switch_topic=switch_topic
self._broadcast_rate=5
self._random_inc = False
if(filename !=None):
self._debug_mode = True
else:
self._debug_mode = False
if(self._debug_mode == True):
print "Debug"
self._f = file(filename,'w+')
self._f.close()
self._filename=filename
self._node_name=node_name
FunctionUnit.__init__(self, node_name)
self._threshold_of_activation = 100#The initial value of threshold_of_activation is 100.
self._sensory_feedback = 0#If the sensory information is valid, the sensory feedback is true, the value is 1 or 0.
self._comm_recieved = [[0 for col in range(1)] for row in range(10)]#_comm_received,row=k,col=t
self._activity_suppression = 1
self._impatience = self._fast
self._impatience_reset = 1#if this robot is running an other behavior,reset=0
self._acquiescence = 1#if this behavior has run too long, acquiescence=0
self._m = 0#final score
self._time = 0
self._active=0#bool, active=0 else=1
self._active_time = 0#when this behavior is active
def run(self):
FunctionUnit.init_node(self)
self.user()
self._robot_id=rospy.get_param('~robot_id',0)
self._behavior_id = rospy.get_param('~behavior_id',0)
thread.start_new_thread(self.timer,())
heartbeat_receive = FunctionUnit.create_receive(self, self._heartbeat_topic, Heartbeat, self.heartbeat_on_received)
self._heartbeat_send = FunctionUnit.create_send(self, '/heart_beat', Heartbeat)
self._switch_send = FunctionUnit.create_send(self, self._switch_topic, Bool)
#sensory_receive = FunctionUnit.create_receive(self, self._node_name+"/sensory_feedback", Bool, self.sensory_on_received)
FunctionUnit.spin(self)
def start_follow(self):
FunctionUnit.init_node(self)
#print 'hello 1'
#print self._receive_topic
receive_1 = FunctionUnit.create_receive(self, self._receive_topic_follower, Odometry, self.receive_1_cb)
receive_2 = FunctionUnit.create_receive(self, self._receive_topic_intruder, Odometry, self.receive_2_cb)
#print 'hello 2'
FunctionUnit.spin(self)
def receive_1_cb(self, msg):
#print 'message received'
#print msg
im = self.br.imgmsg_to_cv2(msg, desired_encoding='passthrough')
for i in range(0, im.shape[0]):
for j in range(0, im.shape[1]):
#if not (im[i, j, 0] == 255 and im[i, j, 1] == 0 and im[i, j, 2] == 0):
if im[i, j, 0] == 255 and im[i, j, 1] == 0 and im[i, j, 2] == 0:
#print 'Detect an intruder!'
msg_sended = Bool()
msg_sended.data = True
send = FunctionUnit.create_send(self, self._send_topic, Bool)
send.send(msg_sended)
virtual_msg = Bool()
virtual_msg.data = False
self._virtual_send.send(virtual_msg)
def receive_1_cb(self, msg):
#print 'message received'
#print msg
im = self.br.imgmsg_to_cv2(msg, desired_encoding='passthrough')
for i in range(0, im.shape[0]):
for j in range(0, im.shape[1]):
#if not (im[i, j, 0] == 255 and im[i, j, 1] == 0 and im[i, j, 2] == 0):
if im[i, j, 0] == 255 and im[i, j, 1] == 0 and im[i, j,
2] == 0:
#print 'Detect an intruder!'
msg_sended = Bool()
msg_sended.data = True
send = FunctionUnit.create_send(self, self._send_topic,
Bool)
send.send(msg_sended)
virtual_msg = Bool()
virtual_msg.data = False
self._virtual_send.send(virtual_msg)
def run(self):
FunctionUnit.init_node(self)
self._num = rospy.get_param('~num', 1)
for i in range(0, self._num): #bs_id
self._shared_list.append([])
for j in range(0, self._num): #r_id
self._shared_list[i].append(False)
receive_3 = FunctionUnit.create_receive(
self, '/heart_beat', Bool, self.motivational_shared_var_update)
self.set_type(Twist)
for i in range(0, self._num):
self._output_list.append(
FunctionUnit.create_send(self, 'output_' + str(i), self._type))
for i in range(0, self._num):
self._cb_list.append(
MultiSwitchCallback(self._output_list, self._shared_list,
self._num, i))
self._input_list.append(
FunctionUnit.create_receive(self, 'input_' + str(i),
self._type, self._cb_list[i]))
FunctionUnit.spin(self)
pass
def user(self): #user defined inc(fast)
self._fast_receive = FunctionUnit.create_receive(
self, 'inc_input', Twist, self.fast_on_received)
self._fast = 0
def start_patrol(self):
FunctionUnit.init_node(self)
self._client = actionlib.SimpleActionClient(
self._topic, micros_mars_task_alloc.msg.MoveBaseAction
) #use 'super' to call the method of his father class.
#FunctionUnit.init() #use class name to call the father method
#print 'Patroller 0 starts!'
#client.wait_for_server()
#Several goals to be sent to the action server.
goal_0 = micros_mars_task_alloc.msg.MoveBaseGoal()
goal_0.target_pose.header.frame_id = 'map'
goal_0.target_pose.pose.position.x = self._pose_0[0]
goal_0.target_pose.pose.position.y = self._pose_0[1]
goal_0.target_pose.pose.position.z = 0.0
goal_0.target_pose.pose.orientation.x = 0.0
goal_0.target_pose.pose.orientation.y = 0.0
goal_0.target_pose.pose.orientation.z = self._pose_0[2]
goal_0.target_pose.pose.orientation.w = self._pose_0[3]
goal_1 = micros_mars_task_alloc.msg.MoveBaseGoal()
goal_1.target_pose.header.frame_id = 'map'
goal_1.target_pose.pose.position.x = self._pose_1[0]
goal_1.target_pose.pose.position.y = self._pose_1[1]
goal_1.target_pose.pose.position.z = 0.0
goal_1.target_pose.pose.orientation.x = 0.0
goal_1.target_pose.pose.orientation.y = 0.0
goal_1.target_pose.pose.orientation.z = self._pose_1[2]
goal_1.target_pose.pose.orientation.w = self._pose_1[3]
goal_2 = micros_mars_task_alloc.msg.MoveBaseGoal()
goal_2.target_pose.header.frame_id = 'map'
goal_2.target_pose.pose.position.x = self._pose_2[0]
goal_2.target_pose.pose.position.y = self._pose_2[1]
goal_2.target_pose.pose.position.z = 0.0
goal_2.target_pose.pose.orientation.x = 0.0
goal_2.target_pose.pose.orientation.y = 0.0
goal_2.target_pose.pose.orientation.z = self._pose_2[2]
goal_2.target_pose.pose.orientation.w = self._pose_2[3]
goal_3 = micros_mars_task_alloc.msg.MoveBaseGoal()
goal_3.target_pose.header.frame_id = 'map'
goal_3.target_pose.pose.position.x = self._pose_3[0]
goal_3.target_pose.pose.position.y = self._pose_3[1]
goal_3.target_pose.pose.position.z = 0.0
goal_3.target_pose.pose.orientation.x = 0.0
goal_3.target_pose.pose.orientation.y = 0.0
goal_3.target_pose.pose.orientation.z = self._pose_3[2]
goal_3.target_pose.pose.orientation.w = self._pose_3[3]
while not rospy.is_shutdown():
self._client.send_goal(goal_0)
self._client.wait_for_result(
rospy.Duration.from_sec(self._wait_time))
self._client.send_goal(goal_1)
self._client.wait_for_result(
rospy.Duration.from_sec(self._wait_time))
self._client.send_goal(goal_2)
self._client.wait_for_result(
rospy.Duration.from_sec(self._wait_time))
self._client.send_goal(goal_3)
self._client.cancel_all_goals()
self._client.wait_for_result(
rospy.Duration.from_sec(self._wait_time))
def set_sensor(self,sensor_topic):
sensory_receive = FunctionUnit.create_receive(self, sensor_topic, Bool, self.sensory_on_received)
def start_patrol(self):
FunctionUnit.init_node(self)
self._client = actionlib.SimpleActionClient(self._topic, micros_mars_task_alloc.msg.MoveBaseAction)#use 'super' to call the method of his father class.
#FunctionUnit.init() #use class name to call the father method
#print 'Patroller 0 starts!'
#client.wait_for_server()
#Several goals to be sent to the action server.
goal_0 = micros_mars_task_alloc.msg.MoveBaseGoal()
goal_0.target_pose.header.frame_id = 'map'
goal_0.target_pose.pose.position.x = self._pose_0[0]
goal_0.target_pose.pose.position.y = self._pose_0[1]
goal_0.target_pose.pose.position.z = 0.0
goal_0.target_pose.pose.orientation.x = 0.0
goal_0.target_pose.pose.orientation.y = 0.0
goal_0.target_pose.pose.orientation.z = self._pose_0[2]
goal_0.target_pose.pose.orientation.w = self._pose_0[3]
goal_1 = micros_mars_task_alloc.msg.MoveBaseGoal()
goal_1.target_pose.header.frame_id = 'map'
goal_1.target_pose.pose.position.x = self._pose_1[0]
goal_1.target_pose.pose.position.y = self._pose_1[1]
goal_1.target_pose.pose.position.z = 0.0
goal_1.target_pose.pose.orientation.x = 0.0
goal_1.target_pose.pose.orientation.y = 0.0
goal_1.target_pose.pose.orientation.z = self._pose_1[2]
goal_1.target_pose.pose.orientation.w = self._pose_1[3]
goal_2 = micros_mars_task_alloc.msg.MoveBaseGoal()
goal_2.target_pose.header.frame_id = 'map'
goal_2.target_pose.pose.position.x = self._pose_2[0]
goal_2.target_pose.pose.position.y = self._pose_2[1]
goal_2.target_pose.pose.position.z = 0.0
goal_2.target_pose.pose.orientation.x = 0.0
goal_2.target_pose.pose.orientation.y = 0.0
goal_2.target_pose.pose.orientation.z = self._pose_2[2]
goal_2.target_pose.pose.orientation.w = self._pose_2[3]
goal_3 = micros_mars_task_alloc.msg.MoveBaseGoal()
goal_3.target_pose.header.frame_id = 'map'
goal_3.target_pose.pose.position.x = self._pose_3[0]
goal_3.target_pose.pose.position.y = self._pose_3[1]
goal_3.target_pose.pose.position.z = 0.0
goal_3.target_pose.pose.orientation.x = 0.0
goal_3.target_pose.pose.orientation.y = 0.0
goal_3.target_pose.pose.orientation.z = self._pose_3[2]
goal_3.target_pose.pose.orientation.w = self._pose_3[3]
while not rospy.is_shutdown():
self._client.send_goal(goal_0)
self._client.wait_for_result(rospy.Duration.from_sec(self._wait_time))
self._client.send_goal(goal_1)
self._client.wait_for_result(rospy.Duration.from_sec(self._wait_time))
self._client.send_goal(goal_2)
self._client.wait_for_result(rospy.Duration.from_sec(self._wait_time))
self._client.send_goal(goal_3)
self._client.cancel_all_goals()
self._client.wait_for_result(rospy.Duration.from_sec(self._wait_time))
def add_pair(self,input_topic,topic_type,output_topic):
self._output_list.append(FunctionUnit.create_send(self, output_topic, topic_type))
self._cb_list.append(SwitchCallback(self._output_list[self._list_count],self._shared_list))
self._input_list.append(FunctionUnit.create_receive(self, input_topic, topic_type, self._cb_list[self._list_count]))
self._list_count += 1
def run(self):
FunctionUnit.init_node(self)
receive_3 = FunctionUnit.create_receive(self, self._motive_topic_1, Bool, self.motivational_shared_var_update)
self.set_switch()
FunctionUnit.spin(self)
pass
def add_action(self,sub_action_name,pub_action_name):
#self._action=action_client
self._action_mode=True
self._recieve_action_cancel = FunctionUnit.create_receive(self, sub_action_name + '/goal', MoveBaseActionGoal, self.receive_cb_goal)
self._send_cancel= FunctionUnit.create_send(self, pub_action_name+'/cancel', GoalID)
def user(self):#user defined inc(fast)
self._fast_receive = FunctionUnit.create_receive(self, 'inc_input', Twist, self.fast_on_received)
self._fast = 0
