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)
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 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 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 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)
if not(self._topic_2 == None):
receive_2 = FunctionUnit.create_receive(self, self._topic_2, self._msg_type_2, self.receive_cb_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)
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 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 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 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 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))