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 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 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 __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, 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 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 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 __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 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 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 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
