<commit_before>#!/usr/bin/env python
###########################################################
#               WARNING: Generated code!                  #
#              **************************                 #
# Manual changes may get lost if file is generated again. #
# Only code inside the [MANUAL] tags will be kept.        #
###########################################################

import roslib; roslib.load_manifest('behavior_search_victims')
from flexbe_core import Behavior, Autonomy, OperatableStateMachine, ConcurrencyContainer, PriorityContainer, Logger
from hector_flexbe_states.move_arm_state import MoveArmState
from flexbe_states.wait_state import WaitState
from behavior_explorationdriveto.explorationdriveto_sm import ExplorationDriveToSM
from hector_flexbe_states.confirm_victim import ConfirmVictim
from hector_flexbe_states.discard_victim import DiscardVictim
from hector_flexbe_states.Decide_If_Victim import DecideIfVictim
from behavior_exploration.exploration_sm import ExplorationSM
from hector_flexbe_states.detect_object import DetectObject
from hector_flexbe_states.move_arm_dyn_state import MoveArmDynState
# Additional imports can be added inside the following tags
# [MANUAL_IMPORT]
from geometry_msgs.msg import PoseStamped
# [/MANUAL_IMPORT]


'''
Created on Thu May 19 2016
@author: <NAME>
'''
class SearchVictimsSM(Behavior):
	'''
	Explore and search victims
	'''


	def __init__(self):
		super(SearchVictimsSM, self).__init__()
		self.name = 'Search Victims'

		# parameters of this behavior

		# references to used behaviors
		self.add_behavior(ExplorationDriveToSM, 'ExplorationDriveTo')
		self.add_behavior(ExplorationSM, 'ExplorationWithDetection/Exploration')

		# Additional initialization code can be added inside the following tags
		# [MANUAL_INIT]
		
		# [/MANUAL_INIT]

		# Behavior comments:



	def create(self):
		# x:30 y:365, x:43 y:428
		_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
		_state_machine.userdata.pose = PoseStamped()
		_state_machine.userdata.joint_config = [0, 0, 0, 0]
		_state_machine.userdata.group_name = 'arm_group'
		_state_machine.userdata.explore_speed = 0.2
		_state_machine.userdata.drive_to_speed = 0.4
		_state_machine.userdata.type = 'hotspot'

		# Additional creation code can be added inside the following tags
		# [MANUAL_CREATE]
		
		# [/MANUAL_CREATE]

		# x:30 y:365, x:130 y:365, x:230 y:365, x:330 y:365, x:430 y:365
		_sm_explorationwithdetection_0 = ConcurrencyContainer(outcomes=['finished', 'failed'], input_keys=['speed'], output_keys=['pose', 'victim'], conditions=[
										('finished', [('Exploration', 'finished')]),
										('failed', [('Exploration', 'failed')]),
										('finished', [('Detect_Object', 'found')])
										])

		with _sm_explorationwithdetection_0:
			# x:40 y:44
			OperatableStateMachine.add('Exploration',
										self.use_behavior(ExplorationSM, 'ExplorationWithDetection/Exploration'),
										transitions={'finished': 'finished', 'failed': 'failed'},
										autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
										remapping={'speed': 'speed'})

			# x:346 y:59
			OperatableStateMachine.add('Detect_Object',
										DetectObject(),
										transitions={'found': 'finished'},
										autonomy={'found': Autonomy.Off},
										remapping={'pose': 'pose', 'victim': 'victim'})



		with _state_machine:
			# x:84 y:49
			OperatableStateMachine.add('SetInitialArmState',
										MoveArmState(),
										transitions={'reached': 'ExplorationWithDetection', 'planning_failed': 'SetInitialArmState', 'control_failed': 'SetInitialArmState'},
										autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.High, 'control_failed': Autonomy.High},
										remapping={'joint_config': 'joint_config', 'group_name': 'group_name'})

			# x:84 y:141
			OperatableStateMachine.add('Wait',
										WaitState(wait_time=5),
										transitions={'done': 'SetInitialArmState'},
										autonomy={'done': Autonomy.Off})

			# x:798 y:180
			OperatableStateMachine.add('ExplorationDriveTo',
										self.use_behavior(ExplorationDriveToSM, 'ExplorationDriveTo'),
										transitions={'finished': 'MoveArmVictim', 'failed': 'Decide_If_Victim'},
										autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
										remapping={'pose': 'pose', 'victim': 'victim', 'speed': 'drive_to_speed'})

			# x:365 y:197
			OperatableStateMachine.add('Confirm_Victim',
										ConfirmVictim(),
										transitions={'confirmed': 'Wait'},
										autonomy={'confirmed': Autonomy.Off},
										remapping={'victim': 'victim'})

			# x:371 y:308
			OperatableStateMachine.add('Discard_Victim',
										DiscardVictim(),
										transitions={'discarded': 'Wait'},
										autonomy={'discarded': Autonomy.Off},
										remapping={'victim': 'victim'})

			# x:605 y:238
			OperatableStateMachine.add('Decide_If_Victim',
										DecideIfVictim(),
										transitions={'confirm': 'Confirm_Victim', 'discard': 'Discard_Victim', 'retry': 'ExplorationDriveTo'},
										autonomy={'confirm': Autonomy.Off, 'discard': Autonomy.Off, 'retry': Autonomy.Off})

			# x:400 y:34
			OperatableStateMachine.add('ExplorationWithDetection',
										_sm_explorationwithdetection_0,
										transitions={'finished': 'ExplorationDriveTo', 'failed': 'failed'},
										autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
										remapping={'speed': 'explore_speed', 'pose': 'pose', 'victim': 'victim'})

			# x:759 y:333
			OperatableStateMachine.add('MoveArmVictim',
										MoveArmDynState(),
										transitions={'reached': 'Decide_If_Victim', 'sampling_failed': 'finished', 'planning_failed': 'finished', 'control_failed': 'finished'},
										autonomy={'reached': Autonomy.Off, 'sampling_failed': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},<commit_msg>[search_victims] Changed autonomy and renamed joint config<commit_after>#!/usr/bin/env python
###########################################################
#               WARNING: Generated code!                  #
#              **************************                 #
# Manual changes may get lost if file is generated again. #
# Only code inside the [MANUAL] tags will be kept.        #
###########################################################

import roslib; roslib.load_manifest('behavior_search_victims')
from flexbe_core import Behavior, Autonomy, OperatableStateMachine, ConcurrencyContainer, PriorityContainer, Logger
from hector_flexbe_states.move_arm_state import MoveArmState
from flexbe_states.wait_state import WaitState
from behavior_explorationdriveto.explorationdriveto_sm import ExplorationDriveToSM
from hector_flexbe_states.confirm_victim import ConfirmVictim
from hector_flexbe_states.discard_victim import DiscardVictim
from hector_flexbe_states.Decide_If_Victim import DecideIfVictim
from behavior_exploration.exploration_sm import ExplorationSM
from hector_flexbe_states.detect_object import DetectObject
from hector_flexbe_states.move_arm_dyn_state import MoveArmDynState
# Additional imports can be added inside the following tags
# [MANUAL_IMPORT]
from geometry_msgs.msg import PoseStamped
# [/MANUAL_IMPORT]


'''
Created on Thu May 19 2016
@author: <NAME>
'''
class SearchVictimsSM(Behavior):
	'''
	Explore and search victims
	'''


	def __init__(self):
		super(SearchVictimsSM, self).__init__()
		self.name = 'Search Victims'

		# parameters of this behavior

		# references to used behaviors
		self.add_behavior(ExplorationDriveToSM, 'ExplorationDriveTo')
		self.add_behavior(ExplorationSM, 'ExplorationWithDetection/Exploration')

		# Additional initialization code can be added inside the following tags
		# [MANUAL_INIT]
		
		# [/MANUAL_INIT]

		# Behavior comments:



	def create(self):
		# x:685 y:553, x:911 y:41
		_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
		_state_machine.userdata.pose = PoseStamped()
		_state_machine.userdata.down_config = [0, 0, 0, 0]
		_state_machine.userdata.group_name = 'arm_group'
		_state_machine.userdata.explore_speed = 0.2
		_state_machine.userdata.drive_to_speed = 0.4
		_state_machine.userdata.type = 'hotspot'

		# Additional creation code can be added inside the following tags
		# [MANUAL_CREATE]
		
		# [/MANUAL_CREATE]

		# x:30 y:365, x:130 y:365, x:230 y:365, x:330 y:365, x:430 y:365
		_sm_explorationwithdetection_0 = ConcurrencyContainer(outcomes=['finished', 'failed'], input_keys=['speed'], output_keys=['pose', 'victim'], conditions=[
										('finished', [('Exploration', 'finished')]),
										('failed', [('Exploration', 'failed')]),
										('finished', [('Detect_Object', 'found')])
										])

		with _sm_explorationwithdetection_0:
			# x:40 y:44
			OperatableStateMachine.add('Exploration',
										self.use_behavior(ExplorationSM, 'ExplorationWithDetection/Exploration'),
										transitions={'finished': 'finished', 'failed': 'failed'},
										autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
										remapping={'speed': 'speed'})

			# x:346 y:59
			OperatableStateMachine.add('Detect_Object',
										DetectObject(),
										transitions={'found': 'finished'},
										autonomy={'found': Autonomy.Off},
										remapping={'pose': 'pose', 'victim': 'victim'})



		with _state_machine:
			# x:84 y:49
			OperatableStateMachine.add('SetInitialArmState',
										MoveArmState(),
										transitions={'reached': 'ExplorationWithDetection', 'planning_failed': 'SetInitialArmState', 'control_failed': 'SetInitialArmState'},
										autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.High, 'control_failed': Autonomy.High},
										remapping={'joint_config': 'down_config', 'group_name': 'group_name'})

			# x:84 y:141
			OperatableStateMachine.add('Wait',
										WaitState(wait_time=5),
										transitions={'done': 'SetInitialArmState'},
										autonomy={'done': Autonomy.Off})

			# x:798 y:180
			OperatableStateMachine.add('ExplorationDriveTo',
										self.use_behavior(ExplorationDriveToSM, 'ExplorationDriveTo'),
										transitions={'finished': 'MoveArmVictim', 'failed': 'Decide_If_Victim'},
										autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
										remapping={'pose': 'pose', 'victim': 'victim', 'speed': 'drive_to_speed'})

			# x:365 y:197
			OperatableStateMachine.add('Confirm_Victim',
										ConfirmVictim(),
										transitions={'confirmed': 'Wait'},
										autonomy={'confirmed': Autonomy.Off},
										remapping={'victim': 'victim'})

			# x:371 y:308
			OperatableStateMachine.add('Discard_Victim',
										DiscardVictim(),
										transitions={'discarded': 'Wait'},
										autonomy={'discarded': Autonomy.Off},
										remapping={'victim': 'victim'})

			# x:605 y:238
			OperatableStateMachine.add('Decide_If_Victim',
										DecideIfVictim(),
										transitions={'confirm': 'Confirm_Victim', 'discard': 'Discard_Victim', 'retry': 'ExplorationDriveTo'},
										autonomy={'confirm': Autonomy.Full, 'discard': Autonomy.Full, 'retry': Autonomy.Full})

			# x:400 y:34
			OperatableStateMachine.add('ExplorationWithDetection',
										_sm_explorationwithdetection_0,
										transitions={'finished': 'ExplorationDriveTo', 'failed': 'failed'},
										autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
										remapping={'speed': 'explore_speed', 'pose': 'pose', 'victim': 'victim'})

			# x:759 y:333
			OperatableStateMachine.add('MoveArmVictim',
										MoveArmDynState(),
										transitions={'reached': 'Decide_If_Victim', 'sampling_failed': 'finished', 'planning_failed': 'finished', 'control_failed': 'finished'},
										autonomy={'reached': Autonomy.Off, 'sampling_failed': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
{'fraction_non_alphanumeric': 0.15124779430299976, 'fraction_numerical': 0.01856986807831275, 'mean_word_length': 4.437185929648241, 'pattern_counts': {'":': 0, '<': 5, '<?xml version=': 0, '>': 5, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5566844', 'n_tokens_mistral': 4772, 'n_tokens_neox': 3619, 'n_words': 701}