def create(self):
control_goto_reference_topic = "control_manager/reference"
control_mpc_tracker_diagnostic_topic = "control_manager/mpc_tracker/diagnostics"
# x:683 y:90, x:283 y:190
_state_machine = OperatableStateMachine(outcomes=['reached', 'failed'],
input_keys=['goal'])
_state_machine.userdata.goal = mrs_msgs.srv.ReferenceStampedSrvRequest(
)
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:74 y:74
OperatableStateMachine.add(
'Goto_reference',
ServiceGoToReferenceState(
service_topic=control_goto_reference_topic),
transitions={
'successed': 'Wait_Till_Reaching_Goal',
'failed': 'failed'
},
autonomy={
'successed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'goal': 'goal'})
# x:381 y:74
OperatableStateMachine.add(
'Wait_Till_Reaching_Goal',
WaitForMsgState(topic=control_mpc_tracker_diagnostic_topic,
wait_time=-1,
function=self.mpc_tracker_diagnostics_cb,
output_keys=[],
output_function=None),
transitions={
'successed': 'reached',
'failed': 'failed'
},
autonomy={
'successed': Autonomy.Off,
'failed': Autonomy.Off
})
return _state_machine
def create(self):
control_goto_altitude_topic = "control_manager/goto_altitude"
control_mpc_tracker_diagnostic_topic = "control_manager/mpc_tracker/diagnostics"
# x:683 y:90, x:283 y:190
_state_machine = OperatableStateMachine(outcomes=['reached', 'failed'],
input_keys=['goal'])
_state_machine.userdata.goal = 3
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:97 y:74
OperatableStateMachine.add(
'Call_GoTo_Altitude',
ServiceGoToAltitudeState(
service_topic=control_goto_altitude_topic),
transitions={
'successed': 'Wait_Till_Reaching_Goal',
'failed': 'failed'
},
autonomy={
'successed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'goal': 'goal'})
# x:381 y:74
OperatableStateMachine.add(
'Wait_Till_Reaching_Goal',
WaitForMsgState(topic=control_mpc_tracker_diagnostic_topic,
wait_time=-1,
function=self.mpc_tracker_diagnostics_cb),
transitions={
'successed': 'reached',
'failed': 'failed'
},
autonomy={
'successed': Autonomy.Off,
'failed': Autonomy.Off
})
return _state_machine
def create(self):
controller_motor_on_topic = "control_manager/motors"
takeoff_topic = "uav_manager/takeoff"
wait_time_after_setting = 0.5
wait_time_for_takeoff = 15
control_diagnostic_topic = "control_manager/diagnostics"
e_land_topic = "control_manager/eland"
# x:971 y:180, x:588 y:387
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['simulation'])
_state_machine.userdata.simulation = True
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:166 y:79
OperatableStateMachine.add('TakeOff',
ServiceTriggerState(service_topic=takeoff_topic, state_name="ControlManager"),
transitions={'finished': 'Wait_For_Successful_TakeOff', 'failed': 'Emergency_Landing'},
autonomy={'finished': Autonomy.Off, 'failed': Autonomy.Off})
# x:424 y:142
OperatableStateMachine.add('Wait_For_Successful_TakeOff',
WaitForMsgState(topic=control_diagnostic_topic, wait_time=wait_time_for_takeoff, function=self.mrs_takeoff_cb, input_keys=[], output_keys=[], output_function=None),
transitions={'successed': 'finished', 'failed': 'Emergency_Landing'},
autonomy={'successed': Autonomy.Off, 'failed': Autonomy.Off})
# x:196 y:271
OperatableStateMachine.add('Emergency_Landing',
ServiceTriggerState(service_topic=e_land_topic, state_name="ControlManager"),
transitions={'finished': 'failed', 'failed': 'failed'},
autonomy={'finished': Autonomy.Off, 'failed': Autonomy.Off})
return _state_machine
def create(self):
wait_time_after_flying = 1
wall_following_desired_yaw = -math.pi / 2
# x:139 y:774, x:281 y:333
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.simulation = True
_state_machine.userdata.flying_altitude = 5
_state_machine.userdata.waiting_spot = mrs_msgs.srv.ReferenceStampedSrvRequest(
)
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
param_prefix_namespace = "flexbe_behavior_launcher/"
flying_altitude_list = rospy.get_param(param_prefix_namespace +
'main/flying_altitudes')
uav_name_list = rospy.get_param(param_prefix_namespace +
'main/robot_name_list')
robot_name = rospy.get_param(param_prefix_namespace + 'robot_name')
waiting_spots = rospy.get_param(param_prefix_namespace +
'main/waiting_spots')
wall_following_desired_speed = rospy.get_param(
param_prefix_namespace + 'main/wall_following_desired_speed')
wall_following_desired_wall_distance = rospy.get_param(
param_prefix_namespace +
'main/wall_following_desired_wall_distance')
_state_machine.userdata.robot_name = robot_name
Logger.loginfo("wall_following_desired_speed: %s" %
wall_following_desired_speed)
Logger.loginfo("wall_following_desired_wall_distance: %s" %
wall_following_desired_wall_distance)
window_position_estimation_reset_topic = rospy.get_param(
param_prefix_namespace +
'main/window_position_estimation_reset_topic')
window_position_estimation_reset_topic = "/" + robot_name + "/" + window_position_estimation_reset_topic
Logger.loginfo("window_position_estimation_reset_topic: %s" %
window_position_estimation_reset_topic)
window_position_estimation_closest_window_topic = rospy.get_param(
param_prefix_namespace +
'main/window_position_estimation_closest_window_topic')
window_position_estimation_closest_window_topic = "/" + robot_name + "/" + window_position_estimation_closest_window_topic
Logger.loginfo("window_position_estimation_closest_window_topic: %s" %
window_position_estimation_closest_window_topic)
land_home_service_topic = rospy.get_param(
param_prefix_namespace + 'main/land_home_service_topic')
land_home_service_topic = "/" + robot_name + "/" + land_home_service_topic
Logger.loginfo("land_home_service_topic: %s" % land_home_service_topic)
e_land_topic = rospy.get_param(param_prefix_namespace +
'main/e_land_topic')
e_land_topic = "/" + robot_name + "/" + e_land_topic
Logger.loginfo("e_land_topic: %s" % e_land_topic)
fire_detection_topic = rospy.get_param(param_prefix_namespace +
'main/fire_detection_topic')
fire_detection_topic = "/" + robot_name + "/" + fire_detection_topic
Logger.loginfo("fire_detection_topic: %s" % fire_detection_topic)
window_flyer_action_server_name = rospy.get_param(
param_prefix_namespace + 'main/window_flyer_action_server_name')
window_flyer_action_server_name = "/" + robot_name + "/" + window_flyer_action_server_name
Logger.loginfo("window_flyer_action_server_name: %s" %
window_flyer_action_server_name)
wall_following_action_server_name = rospy.get_param(
param_prefix_namespace + 'main/wall_following_action_server_name')
wall_following_action_server_name = "/" + robot_name + "/" + wall_following_action_server_name
Logger.loginfo("wall_following_action_server_name: %s" %
wall_following_action_server_name)
Logger.loginfo("fire_ch_sm.robot_name: %s" % robot_name)
flying_altitude = 3
waiting_spot = mrs_msgs.srv.ReferenceStampedSrvRequest()
waiting_spot.header.frame_id = robot_name + "/rtk_origin"
for i in range(len(uav_name_list)):
if uav_name_list[i] == robot_name:
flying_altitude = flying_altitude_list[i]
waiting_spot.reference.position.x = waiting_spots[i * 3]
waiting_spot.reference.position.y = waiting_spots[i * 3 + 1]
waiting_spot.reference.position.z = flying_altitude
waiting_spot.reference.yaw = waiting_spots[i * 3 + 2]
break
_state_machine.userdata.flying_altitude = flying_altitude
_state_machine.userdata.waiting_spot = waiting_spot
Logger.loginfo("fire_ch_sm.flying_altitude: %s" %
_state_machine.userdata.flying_altitude)
Logger.loginfo("fire_ch_sm.waiting_spot: %s" %
_state_machine.userdata.waiting_spot)
# [/MANUAL_CREATE]
# x:30 y:480, x:130 y:480, x:230 y:480, x:734 y:480, x:430 y:480, x:530 y:483, x:630 y:480, x:335 y:480, x:830 y:480
_sm_detect_window_inside_0 = ConcurrencyContainer(
outcomes=['preempted', 'no_detection', 'error', 'window_detected'],
input_keys=['window_id'],
output_keys=['window_id'],
conditions=[
('preempted', [('Wall_Follow_From_Inside', 'preempted')]),
('no_detection', [('Wall_Follow_From_Inside', 'successed')]),
('error', [('Wall_Follow_From_Inside', 'aborted')]),
('window_detected', [('Wait_For_Window_Estimation',
'successed')]),
('error', [('Wait_For_Window_Estimation', 'failed')])
])
with _sm_detect_window_inside_0:
# x:156 y:151
OperatableStateMachine.add(
'Wall_Follow_From_Inside',
WallFollowingActionState(
action_server_name=wall_following_action_server_name,
desired_yaw=wall_following_desired_yaw,
desired_speed=wall_following_desired_speed,
desired_wall_distance=wall_following_desired_wall_distance
),
transitions={
'successed': 'no_detection',
'aborted': 'error',
'preempted': 'preempted'
},
autonomy={
'successed': Autonomy.Off,
'aborted': Autonomy.Off,
'preempted': Autonomy.Off
})
# x:537 y:145
OperatableStateMachine.add(
'Wait_For_Window_Estimation',
WaitForMsgState(
topic=window_position_estimation_closest_window_topic,
wait_time=-1,
function=lambda x: True,
output_keys=['window_id'],
output_function=self.window_detection_cb),
transitions={
'successed': 'window_detected',
'failed': 'error'
},
autonomy={
'successed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'window_id': 'window_id'})
# x:133 y:390, x:683 y:390, x:233 y:390, x:333 y:390, x:533 y:390, x:530 y:550, x:630 y:550, x:730 y:550, x:830 y:550
_sm_detect_fire_inside_1 = ConcurrencyContainer(
outcomes=['preempted', 'fire_detected', 'no_detection', 'error'],
conditions=[
('error', [('Wall_Follow_From_Inside', 'aborted')]),
('no_detection', [('Wall_Follow_From_Inside', 'successed')]),
('preempted', [('Wall_Follow_From_Inside', 'preempted')]),
('error', [('Wait_For_Fire_Detection', 'failed')]),
('fire_detected', [('Wait_For_Fire_Detection', 'successed')])
])
with _sm_detect_fire_inside_1:
# x:198 y:174
OperatableStateMachine.add(
'Wall_Follow_From_Inside',
WallFollowingActionState(
action_server_name=wall_following_action_server_name,
desired_yaw=wall_following_desired_yaw,
desired_speed=wall_following_desired_speed,
desired_wall_distance=wall_following_desired_wall_distance
),
transitions={
'successed': 'no_detection',
'aborted': 'error',
'preempted': 'preempted'
},
autonomy={
'successed': Autonomy.Off,
'aborted': Autonomy.Off,
'preempted': Autonomy.Off
})
# x:579 y:174
OperatableStateMachine.add('Wait_For_Fire_Detection',
WaitForMsgState(
topic=fire_detection_topic,
wait_time=-1,
function=lambda x: True,
output_keys=[],
output_function=None),
transitions={
'successed': 'fire_detected',
'failed': 'error'
},
autonomy={
'successed': Autonomy.Off,
'failed': Autonomy.Off
})
with _state_machine:
# x:76 y:71
OperatableStateMachine.add('Prepare_UAV',
self.use_behavior(
Prepare_UAVSM, 'Prepare_UAV'),
transitions={
'finished': 'GotoAltitude',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'simulation': 'simulation'})
# x:326 y:71
OperatableStateMachine.add('GotoAltitude',
self.use_behavior(
GotoAltitudeSM, 'GotoAltitude'),
transitions={
'reached': 'Goto_Waiting_Spot',
'failed': 'Emergency_Landing'
},
autonomy={
'reached': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'goal': 'flying_altitude'})
# x:971 y:268
OperatableStateMachine.add(
'Flythrough_Window_Inside',
WindowFlyerActionState(
action_server_name=window_flyer_action_server_name,
yaw_offset=3.14),
transitions={
'successed': 'Detect_Fire_Inside',
'window_not_found': 'Emergency_Landing',
'already_flythrough': 'Emergency_Landing',
'server_not_initialized': 'Emergency_Landing',
'error': 'Emergency_Landing',
'preempted': 'Emergency_Landing'
},
autonomy={
'successed': Autonomy.Off,
'window_not_found': Autonomy.Off,
'already_flythrough': Autonomy.Off,
'server_not_initialized': Autonomy.Off,
'error': Autonomy.Off,
'preempted': Autonomy.Off
},
remapping={
'window_id': 'window_id',
'window_position': 'window_position'
})
# x:774 y:74
OperatableStateMachine.add(
'Reset_Window_Estimation',
ServiceTriggerState(
service_topic=window_position_estimation_reset_topic,
state_name="WindowEstimation"),
transitions={
'finished': 'Wait',
'failed': 'Emergency_Landing'
},
autonomy={
'finished': Autonomy.Off,
'failed': Autonomy.Off
})
# x:965 y:178
OperatableStateMachine.add(
'Wait_For_Window_Estimation',
WaitForMsgState(
topic=window_position_estimation_closest_window_topic,
wait_time=5,
function=lambda x: True,
output_keys=['window_id'],
output_function=self.window_detection_cb),
transitions={
'successed': 'Flythrough_Window_Inside',
'failed': 'Call_Land_Home'
},
autonomy={
'successed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'window_id': 'window_id'})
# x:1008 y:74
OperatableStateMachine.add(
'Wait',
WaitState(wait_time=wait_time_after_flying),
transitions={'done': 'Wait_For_Window_Estimation'},
autonomy={'done': Autonomy.Off})
# x:954 y:536
OperatableStateMachine.add(
'Wait_For_Window_Estimation_2',
WaitForMsgState(
topic=window_position_estimation_closest_window_topic,
wait_time=5,
function=lambda x: True,
output_keys=['window_id'],
output_function=self.window_detection_cb),
transitions={
'successed': 'Flythrough_Window_Outside',
'failed': 'Reset_Window_Estimation_2'
},
autonomy={
'successed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'window_id': 'window_id'})
# x:663 y:755
OperatableStateMachine.add(
'Flythrough_Window_Outside',
WindowFlyerActionState(
action_server_name=window_flyer_action_server_name,
yaw_offset=0),
transitions={
'successed': 'Call_Land_Home',
'window_not_found': 'Emergency_Landing',
'already_flythrough': 'Emergency_Landing',
'server_not_initialized': 'Emergency_Landing',
'error': 'Emergency_Landing',
'preempted': 'Emergency_Landing'
},
autonomy={
'successed': Autonomy.Off,
'window_not_found': Autonomy.Off,
'already_flythrough': Autonomy.Off,
'server_not_initialized': Autonomy.Off,
'error': Autonomy.Off,
'preempted': Autonomy.Off
},
remapping={
'window_id': 'window_id',
'window_position': 'window_position'
})
# x:386 y:754
OperatableStateMachine.add(
'Call_Land_Home',
ServiceTriggerState(service_topic=land_home_service_topic,
state_name="ControlManager"),
transitions={
'finished': 'finished',
'failed': 'Call_Land_Home'
},
autonomy={
'finished': Autonomy.Off,
'failed': Autonomy.Off
})
# x:519 y:329
OperatableStateMachine.add('Emergency_Landing',
ServiceTriggerState(
service_topic=e_land_topic,
state_name="ControlManager"),
transitions={
'finished': 'failed',
'failed': 'Emergency_Landing'
},
autonomy={
'finished': Autonomy.Off,
'failed': Autonomy.Off
})
# x:977 y:343
OperatableStateMachine.add('Detect_Fire_Inside',
_sm_detect_fire_inside_1,
transitions={
'preempted': 'Emergency_Landing',
'fire_detected':
'Goto_ClosestWindow2',
'no_detection': 'Emergency_Landing',
'error': 'Emergency_Landing'
},
autonomy={
'preempted': Autonomy.Inherit,
'fire_detected': Autonomy.Inherit,
'no_detection': Autonomy.Inherit,
'error': Autonomy.Inherit
})
# x:555 y:69
OperatableStateMachine.add(
'Goto_Waiting_Spot',
self.use_behavior(goto_referenceSM, 'Goto_Waiting_Spot'),
transitions={
'reached': 'Reset_Window_Estimation',
'failed': 'Emergency_Landing'
},
autonomy={
'reached': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'goal': 'waiting_spot'})
# x:967 y:443
OperatableStateMachine.add(
'Goto_ClosestWindow2',
self.use_behavior(goto_referenceSM, 'Goto_ClosestWindow2'),
transitions={
'reached': 'Wait_For_Window_Estimation_2',
'failed': 'Emergency_Landing'
},
autonomy={
'reached': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'goal': 'window_position'})
# x:962 y:625
OperatableStateMachine.add(
'Reset_Window_Estimation_2',
ServiceTriggerState(
service_topic=window_position_estimation_reset_topic,
state_name="WindowEstimation"),
transitions={
'finished': 'Detect_Window_Inside',
'failed': 'Emergency_Landing'
},
autonomy={
'finished': Autonomy.Off,
'failed': Autonomy.Off
})
# x:965 y:718
OperatableStateMachine.add('Detect_Window_Inside',
_sm_detect_window_inside_0,
transitions={
'preempted':
'Emergency_Landing',
'no_detection':
'Emergency_Landing',
'error':
'Emergency_Landing',
'window_detected':
'Flythrough_Window_Outside'
},
autonomy={
'preempted': Autonomy.Inherit,
'no_detection': Autonomy.Inherit,
'error': Autonomy.Inherit,
'window_detected': Autonomy.Inherit
},
remapping={'window_id': 'window_id'})
return _state_machine
def create(self):
# x:192 y:728, x:263 y:340
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.simulation = True
_state_machine.userdata.flying_altitude = 5
_state_machine.userdata.orbit_goal = ""
_state_machine.userdata.window_id = 0
_state_machine.userdata.lidar_flier_speed_orbit = 0
_state_machine.userdata.lidar_flier_speed_goto = 0
_state_machine.userdata.lidar_flier_clockwise = False
_state_machine.userdata.lidar_flier_stick_distance = False
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
param_prefix_namespace = "flexbe_behavior_launcher/"
flying_altitude_list = rospy.get_param(param_prefix_namespace +
'main/flying_altitudes')
uav_name_list = rospy.get_param(param_prefix_namespace +
'main/robot_name_list')
robot_name = rospy.get_param(param_prefix_namespace + 'robot_name')
lidar_flier_speed_goto = rospy.get_param(
param_prefix_namespace + 'lidar_flier/lidar_flier_speed_goto')
lidar_flier_speed_orbit = rospy.get_param(
param_prefix_namespace + 'lidar_flier/lidar_flier_speed_orbit')
lidar_flier_clockwise = rospy.get_param(
param_prefix_namespace + 'lidar_flier/lidar_flier_clockwise')
lidar_flier_stick_distance = rospy.get_param(
param_prefix_namespace + 'lidar_flier/lidar_flier_stick_distance')
window_position_estimation_reset_topic = rospy.get_param(
param_prefix_namespace +
'main/window_position_estimation_reset_topic')
window_position_estimation_reset_topic = "/" + robot_name + "/" + window_position_estimation_reset_topic
Logger.loginfo("window_position_estimation_reset_topic: %s" %
window_position_estimation_reset_topic)
window_position_estimation_closest_window_topic = rospy.get_param(
param_prefix_namespace +
'main/window_position_estimation_closest_window_topic')
window_position_estimation_closest_window_topic = "/" + robot_name + "/" + window_position_estimation_closest_window_topic
Logger.loginfo("window_position_estimation_closest_window_topic: %s" %
window_position_estimation_closest_window_topic)
land_service_topic = rospy.get_param(param_prefix_namespace +
'main/land_service_topic')
land_service_topic = "/" + robot_name + "/" + land_service_topic
Logger.loginfo("land_service_topic: %s" % land_service_topic)
e_land_service_topic = rospy.get_param(param_prefix_namespace +
'main/e_land_service_topic')
e_land_service_topic = "/" + robot_name + "/" + e_land_service_topic
Logger.loginfo("e_land_service_topic: %s" % e_land_service_topic)
fire_detection_topic = rospy.get_param(param_prefix_namespace +
'main/fire_detection_topic')
fire_detection_topic = "/" + robot_name + "/" + fire_detection_topic
Logger.loginfo("fire_detection_topic: %s" % fire_detection_topic)
window_flyer_action_server_name = rospy.get_param(
param_prefix_namespace + 'main/window_flyer_action_server_name')
window_flyer_action_server_name = "/" + robot_name + "/" + window_flyer_action_server_name
Logger.loginfo("window_flyer_action_server_name: %s" %
window_flyer_action_server_name)
lidar_flier_action_server_name = rospy.get_param(
param_prefix_namespace + 'main/lidar_flier_action_server_name')
lidar_flier_action_server_name = "/" + robot_name + "/" + lidar_flier_action_server_name
Logger.loginfo("lidar_flier_action_server_name: %s" %
lidar_flier_action_server_name)
fire_extinguish_action_server_name = rospy.get_param(
param_prefix_namespace + 'main/fire_extinguish_action_server_name')
fire_extinguish_action_server_name = "/" + robot_name + "/" + fire_extinguish_action_server_name
Logger.loginfo("fire_extinguish_action_server_name: %s" %
fire_extinguish_action_server_name)
control_manager_diagnostics_topic = rospy.get_param(
param_prefix_namespace + 'main/control_manager_diagnostics_topic')
control_manager_diagnostics_topic = "/" + robot_name + "/" + control_manager_diagnostics_topic
Logger.loginfo("control_manager_diagnostics_topic: %s" %
control_manager_diagnostics_topic)
control_manager_cmd_topic = rospy.get_param(
param_prefix_namespace + 'main/control_manager_cmd_topic')
control_manager_cmd_topic = "/" + robot_name + "/" + control_manager_cmd_topic
Logger.loginfo("control_manager_cmd_topic: %s" %
control_manager_cmd_topic)
goto_reference_service_topic = rospy.get_param(
param_prefix_namespace + 'main/goto_reference_service_topic')
goto_reference_service_topic = "/" + robot_name + "/" + goto_reference_service_topic
Logger.loginfo("goto_reference_service_topic: %s" %
goto_reference_service_topic)
goto_altitude_service_topic = rospy.get_param(
param_prefix_namespace + 'main/goto_altitude_service_topic')
goto_altitude_service_topic = "/" + robot_name + "/" + goto_altitude_service_topic
Logger.loginfo("goto_altitude_service_topic: %s" %
goto_altitude_service_topic)
wait_for_start_service_topic = rospy.get_param(
param_prefix_namespace + 'main/wait_for_start_service_topic')
wait_for_start_service_topic = "/" + robot_name + "/" + wait_for_start_service_topic
Logger.loginfo("wait_for_start_service_topic: %s" %
wait_for_start_service_topic)
# confirmation_service_topic = rospy.get_param(param_prefix_namespace + 'main/confirmation_service_topic')
# confirmation_service_topic = "/" + robot_name + "/" + confirmation_service_topic
# Logger.loginfo("confirmation_service_topic: %s" % confirmation_service_topic)
hector_reset_service_topic = rospy.get_param(
param_prefix_namespace + 'main/hector_reset_service_topic')
hector_reset_service_topic = "/" + robot_name + "/" + hector_reset_service_topic
Logger.loginfo("hector_reset_service_topic: %s" %
hector_reset_service_topic)
odometry_switch_service_topic = rospy.get_param(
param_prefix_namespace + 'main/odometry_switch_service_topic')
odometry_switch_service_topic = "/" + robot_name + "/" + odometry_switch_service_topic
Logger.loginfo("odometry_switch_service_topic: %s" %
odometry_switch_service_topic)
flying_altitude = 3
for i in range(len(uav_name_list)):
if uav_name_list[i] == robot_name:
flying_altitude = flying_altitude_list[i]
break
orbit_goal = lidar_flier.msg.lfGoal()
orbit_goal.altitude = flying_altitude
orbit_goal.orbit = True
orbit_goal.speed = lidar_flier_speed_orbit
orbit_goal.clockwise = lidar_flier_clockwise
orbit_goal.stick_distance = lidar_flier_stick_distance
_state_machine.userdata.flying_altitude = flying_altitude
_state_machine.userdata.orbit_goal = orbit_goal
_state_machine.userdata.lidar_flier_speed_goto = lidar_flier_speed_goto
_state_machine.userdata.lidar_flier_speed_orbit = lidar_flier_speed_orbit
_state_machine.userdata.lidar_flier_clockwise = lidar_flier_clockwise
_state_machine.userdata.lidar_flier_stick_distance = lidar_flier_stick_distance
Logger.loginfo("fire_ch_sm.flying_altitude: %s" %
_state_machine.userdata.flying_altitude)
Logger.loginfo("fire_ch_sm.orbit_goal: %s" %
_state_machine.userdata.orbit_goal)
Logger.loginfo("fire_ch_sm.lidar_flier_speed_goto: %s" %
_state_machine.userdata.lidar_flier_speed_goto)
Logger.loginfo("fire_ch_sm.lidar_flier_speed_orbit: %s" %
_state_machine.userdata.lidar_flier_speed_orbit)
Logger.loginfo("fire_ch_sm.lidar_flier_stick_distance: %s" %
_state_machine.userdata.lidar_flier_stick_distance)
Logger.loginfo("fire_ch_sm.lidar_flier_clockwise: %s" %
_state_machine.userdata.lidar_flier_clockwise)
# [/MANUAL_CREATE]
# x:30 y:490, x:644 y:487, x:230 y:490, x:330 y:490, x:430 y:490, x:35 y:612, x:97 y:605, x:168 y:600, x:1205 y:397, x:883 y:498, x:1030 y:490, x:1130 y:490, x:1230 y:490
_sm_lookforwindow_0 = ConcurrencyContainer(
outcomes=['error', 'detection', 'no_detection', 'preemted'],
input_keys=['orbit_goal', 'window_id'],
output_keys=['window_id'],
conditions=[
('no_detection', [('OrbitBuilding', 'successed')]),
('error', [('OrbitBuilding', 'no_valid_points_in_scan')]),
('error', [('OrbitBuilding', 'stop_requested')]),
('error', [('OrbitBuilding', 'error')]),
('preemted', [('OrbitBuilding', 'preempted')]),
('error', [('OrbitBuilding', 'server_not_initialized')]),
('error', [('InfiniteWait', 'done')]),
('detection', [('WaitForDetection', 'successed')]),
('error', [('WaitForDetection', 'failed')])
])
with _sm_lookforwindow_0:
# x:434 y:172
OperatableStateMachine.add('InfiniteWait',
WaitState(wait_time=100000),
transitions={'done': 'error'},
autonomy={'done': Autonomy.Off})
# x:155 y:164
OperatableStateMachine.add(
'OrbitBuilding',
LidarFlierActionState(
action_server_name=lidar_flier_action_server_name),
transitions={
'successed': 'no_detection',
'no_valid_points_in_scan': 'error',
'stop_requested': 'error',
'server_not_initialized': 'error',
'error': 'error',
'preempted': 'preemted'
},
autonomy={
'successed': Autonomy.Off,
'no_valid_points_in_scan': Autonomy.Off,
'stop_requested': Autonomy.Off,
'server_not_initialized': Autonomy.Off,
'error': Autonomy.Off,
'preempted': Autonomy.Off
},
remapping={'goal': 'orbit_goal'})
# x:735 y:179
OperatableStateMachine.add(
'WaitForDetection',
WaitForMsgState(
topic=window_position_estimation_closest_window_topic,
wait_time=-1,
function=self.callback_true,
input_keys=["window_id"],
output_keys=["window_id"],
output_function=self.window_detection_cb),
transitions={
'successed': 'detection',
'failed': 'error'
},
autonomy={
'successed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'window_id': 'window_id'})
with _state_machine:
# x:55 y:69
OperatableStateMachine.add(
'WaitForStart',
ServiceWaitForStart(
service_topic=wait_for_start_service_topic),
transitions={'received': 'GotoAltitude'},
autonomy={'received': Autonomy.Off},
remapping={'start_value': 'start_value'})
# x:705 y:70
OperatableStateMachine.add(
'ResetWindowEstimation',
ServiceTriggerState(
service_topic=window_position_estimation_reset_topic,
state_name="WindowEstimation"),
transitions={
'finished': 'LookForWindow',
'failed': 'EmergencyLanding'
},
autonomy={
'finished': Autonomy.Off,
'failed': Autonomy.Off
})
# x:519 y:329
OperatableStateMachine.add('EmergencyLanding',
ServiceTriggerState(
service_topic=e_land_service_topic,
state_name="ControlManager"),
transitions={
'finished': 'failed',
'failed': 'EmergencyLanding'
},
autonomy={
'finished': Autonomy.Off,
'failed': Autonomy.Off
})
# x:251 y:67
OperatableStateMachine.add(
'GotoAltitude',
ServiceGoToAltitudeState(
service_topic=goto_altitude_service_topic,
control_manager_diagnostics_topic=
control_manager_diagnostics_topic),
transitions={
'successed': 'SaveHomePosition',
'failed': 'EmergencyLanding'
},
autonomy={
'successed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'goal': 'flying_altitude'})
# x:495 y:68
OperatableStateMachine.add(
'SaveHomePosition',
WaitForMsgState(
topic=control_manager_cmd_topic,
wait_time=5,
function=self.callback_true,
input_keys=["lidar_flier_speed_goto"],
output_keys=["home_position"],
output_function=self.home_position_output_function),
transitions={
'successed': 'ResetWindowEstimation',
'failed': 'EmergencyLanding'
},
autonomy={
'successed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'lidar_flier_speed_goto': 'lidar_flier_speed_goto',
'home_position': 'home_position'
})
# x:1190 y:65
OperatableStateMachine.add(
'FlythroughWindowInsideApproach',
WindowFlyerActionState(
action_server_name=window_flyer_action_server_name,
just_approach=True,
yaw_offset=3.14),
transitions={
'successed': 'ResetHector',
'window_not_found': 'EmergencyLanding',
'already_flythrough': 'EmergencyLanding',
'server_not_initialized': 'EmergencyLanding',
'error': 'EmergencyLanding',
'preempted': 'EmergencyLanding'
},
autonomy={
'successed': Autonomy.Off,
'window_not_found': Autonomy.Off,
'already_flythrough': Autonomy.Off,
'server_not_initialized': Autonomy.Off,
'error': Autonomy.Off,
'preempted': Autonomy.Off
},
remapping={
'window_id': 'window_id',
'window_position': 'window_position'
})
# x:1009 y:65
OperatableStateMachine.add('LookForWindow',
_sm_lookforwindow_0,
transitions={
'error': 'EmergencyLanding',
'detection':
'FlythroughWindowInsideApproach',
'no_detection': 'EmergencyLanding',
'preemted': 'EmergencyLanding'
},
autonomy={
'error': Autonomy.Inherit,
'detection': Autonomy.Inherit,
'no_detection': Autonomy.Inherit,
'preemted': Autonomy.Inherit
},
remapping={
'orbit_goal': 'orbit_goal',
'window_id': 'window_id'
})
# x:1419 y:387
OperatableStateMachine.add(
'FlythroughWindowOutsideApproach',
WindowFlyerActionState(
action_server_name=window_flyer_action_server_name,
just_approach=True,
yaw_offset=3.14),
transitions={
'successed': 'FlythroughWindowOutside',
'window_not_found': 'EmergencyLanding',
'already_flythrough': 'EmergencyLanding',
'server_not_initialized': 'EmergencyLanding',
'error': 'EmergencyLanding',
'preempted': 'EmergencyLanding'
},
autonomy={
'successed': Autonomy.Off,
'window_not_found': Autonomy.Off,
'already_flythrough': Autonomy.Off,
'server_not_initialized': Autonomy.Off,
'error': Autonomy.Off,
'preempted': Autonomy.Off
},
remapping={
'window_id': 'window_id',
'window_position': 'window_position'
})
# x:671 y:739
OperatableStateMachine.add(
'GoToHome',
LidarFlierActionState(
action_server_name=lidar_flier_action_server_name),
transitions={
'successed': 'Land',
'no_valid_points_in_scan': 'EmergencyLanding',
'stop_requested': 'EmergencyLanding',
'server_not_initialized': 'EmergencyLanding',
'error': 'EmergencyLanding',
'preempted': 'EmergencyLanding'
},
autonomy={
'successed': Autonomy.Off,
'no_valid_points_in_scan': Autonomy.Off,
'stop_requested': Autonomy.Off,
'server_not_initialized': Autonomy.Off,
'error': Autonomy.Off,
'preempted': Autonomy.Off
},
remapping={'goal': 'home_position'})
# x:390 y:734
OperatableStateMachine.add('Land',
ServiceTriggerState(
service_topic=land_service_topic,
state_name="ControlManager"),
transitions={
'finished': 'finished',
'failed': 'Land'
},
autonomy={
'finished': Autonomy.Off,
'failed': Autonomy.Off
})
# x:1467 y:64
OperatableStateMachine.add(
'ResetHector',
ServiceTriggerState(service_topic=hector_reset_service_topic,
state_name="ResetHector"),
transitions={
'finished': 'WaitForTwoSecond',
'failed': 'EmergencyLanding'
},
autonomy={
'finished': Autonomy.Off,
'failed': Autonomy.Off
})
# x:1627 y:58
OperatableStateMachine.add(
'WaitForTwoSecond',
WaitState(wait_time=2),
transitions={'done': 'SwitchOdometryToHector'},
autonomy={'done': Autonomy.Off})
# x:1444 y:159
OperatableStateMachine.add(
'SwitchOdometryToHector',
ServiceStringState(service_topic=odometry_switch_service_topic,
msg_text="HECTOR",
state_name="Odometry"),
transitions={
'finished': 'WaitForTwoSecond2',
'failed': 'EmergencyLanding'
},
autonomy={
'finished': Autonomy.Off,
'failed': Autonomy.Off
})
# x:1625 y:158
OperatableStateMachine.add(
'WaitForTwoSecond2',
WaitState(wait_time=2),
transitions={'done': 'FlythroughWindowInside'},
autonomy={'done': Autonomy.Off})
# x:999 y:736
OperatableStateMachine.add(
'SwitchOdometryToGPS',
ServiceStringState(service_topic=odometry_switch_service_topic,
msg_text="GPS",
state_name="Odometry"),
transitions={
'finished': 'GoToHome',
'failed': 'EmergencyLanding'
},
autonomy={
'finished': Autonomy.Off,
'failed': Autonomy.Off
})
# x:1455 y:250
OperatableStateMachine.add(
'FlythroughWindowInside',
WindowFlyerActionState(
action_server_name=window_flyer_action_server_name,
just_approach=False,
yaw_offset=3.14),
transitions={
'successed': 'FlythroughWindowOutsideApproach',
'window_not_found': 'EmergencyLanding',
'already_flythrough': 'EmergencyLanding',
'server_not_initialized': 'EmergencyLanding',
'error': 'EmergencyLanding',
'preempted': 'EmergencyLanding'
},
autonomy={
'successed': Autonomy.Off,
'window_not_found': Autonomy.Off,
'already_flythrough': Autonomy.Off,
'server_not_initialized': Autonomy.Off,
'error': Autonomy.Off,
'preempted': Autonomy.Off
},
remapping={
'window_id': 'window_id',
'window_position': 'window_position'
})
# x:1448 y:487
OperatableStateMachine.add(
'FlythroughWindowOutside',
WindowFlyerActionState(
action_server_name=window_flyer_action_server_name,
just_approach=False,
yaw_offset=3.14),
transitions={
'successed': 'SwitchOdometryToGPS',
'window_not_found': 'EmergencyLanding',
'already_flythrough': 'EmergencyLanding',
'server_not_initialized': 'EmergencyLanding',
'error': 'EmergencyLanding',
'preempted': 'EmergencyLanding'
},
autonomy={
'successed': Autonomy.Off,
'window_not_found': Autonomy.Off,
'already_flythrough': Autonomy.Off,
'server_not_initialized': Autonomy.Off,
'error': Autonomy.Off,
'preempted': Autonomy.Off
},
remapping={
'window_id': 'window_id',
'window_position': 'window_position'
})
return _state_machine
def create(self):
# x:1333 y:90, x:1333 y:340
_state_machine = OperatableStateMachine(
outcomes=['mission_successful', 'missing_failure'])
_state_machine.userdata.fire_extinguish_position = lidar_flier.msg.lfGoal(
)
_state_machine.userdata.lidar_flier_speed_goto = 1
_state_machine.userdata.fire_position = fire_detect.msg.firemanGoal()
_state_machine.userdata.orbit_goal = lidar_flier.msg.lfGoal()
_state_machine.userdata.flying_altitude = 5
_state_machine.userdata.is_mission_successful = False
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
param_prefix_namespace = "flexbe_behavior_launcher/"
flying_altitude_list = rospy.get_param(param_prefix_namespace +
'main/flying_altitudes')
uav_name_list = rospy.get_param(param_prefix_namespace +
'main/robot_name_list')
robot_name = rospy.get_param(param_prefix_namespace + 'robot_name')
lidar_flier_speed_goto = rospy.get_param(
param_prefix_namespace + 'lidar_flier/lidar_flier_speed_goto')
lidar_flier_speed_orbit = rospy.get_param(
param_prefix_namespace + 'lidar_flier/lidar_flier_speed_orbit')
lidar_flier_clockwise = rospy.get_param(
param_prefix_namespace + 'lidar_flier/lidar_flier_clockwise')
lidar_flier_stick_distance = rospy.get_param(
param_prefix_namespace + 'lidar_flier/lidar_flier_stick_distance')
window_position_estimation_reset_topic = rospy.get_param(
param_prefix_namespace +
'main/window_position_estimation_reset_topic')
window_position_estimation_reset_topic = "/" + robot_name + "/" + window_position_estimation_reset_topic
Logger.loginfo("window_position_estimation_reset_topic: %s" %
window_position_estimation_reset_topic)
window_position_estimation_closest_window_topic = rospy.get_param(
param_prefix_namespace +
'main/window_position_estimation_closest_window_topic')
window_position_estimation_closest_window_topic = "/" + robot_name + "/" + window_position_estimation_closest_window_topic
Logger.loginfo("window_position_estimation_closest_window_topic: %s" %
window_position_estimation_closest_window_topic)
land_service_topic = rospy.get_param(param_prefix_namespace +
'main/land_service_topic')
land_service_topic = "/" + robot_name + "/" + land_service_topic
Logger.loginfo("land_service_topic: %s" % land_service_topic)
e_land_service_topic = rospy.get_param(param_prefix_namespace +
'main/e_land_service_topic')
e_land_service_topic = "/" + robot_name + "/" + e_land_service_topic
Logger.loginfo("e_land_service_topic: %s" % e_land_service_topic)
fire_detection_topic = rospy.get_param(param_prefix_namespace +
'main/fire_detection_topic')
fire_detection_topic = "/" + robot_name + "/" + fire_detection_topic
Logger.loginfo("fire_detection_topic: %s" % fire_detection_topic)
window_flyer_action_server_name = rospy.get_param(
param_prefix_namespace + 'main/window_flyer_action_server_name')
window_flyer_action_server_name = "/" + robot_name + "/" + window_flyer_action_server_name
Logger.loginfo("window_flyer_action_server_name: %s" %
window_flyer_action_server_name)
lidar_flier_action_server_name = rospy.get_param(
param_prefix_namespace + 'main/lidar_flier_action_server_name')
lidar_flier_action_server_name = "/" + robot_name + "/" + lidar_flier_action_server_name
Logger.loginfo("lidar_flier_action_server_name: %s" %
lidar_flier_action_server_name)
fire_extinguish_action_server_name = rospy.get_param(
param_prefix_namespace + 'main/fire_extinguish_action_server_name')
fire_extinguish_action_server_name = "/" + robot_name + "/" + fire_extinguish_action_server_name
Logger.loginfo("fire_extinguish_action_server_name: %s" %
fire_extinguish_action_server_name)
control_manager_diagnostics_topic = rospy.get_param(
param_prefix_namespace + 'main/control_manager_diagnostics_topic')
control_manager_diagnostics_topic = "/" + robot_name + "/" + control_manager_diagnostics_topic
Logger.loginfo("control_manager_diagnostics_topic: %s" %
control_manager_diagnostics_topic)
wait_for_start_service_topic = rospy.get_param(
param_prefix_namespace + 'main/wait_for_start_service_topic')
wait_for_start_service_topic = "/" + robot_name + "/" + wait_for_start_service_topic
Logger.loginfo("wait_for_start_service_topic: %s" %
wait_for_start_service_topic)
control_manager_cmd_topic = rospy.get_param(
param_prefix_namespace + 'main/control_manager_cmd_topic')
control_manager_cmd_topic = "/" + robot_name + "/" + control_manager_cmd_topic
Logger.loginfo("control_manager_cmd_topic: %s" %
control_manager_cmd_topic)
goto_altitude_service_topic = rospy.get_param(
param_prefix_namespace + 'main/goto_altitude_service_topic')
goto_altitude_service_topic = "/" + robot_name + "/" + goto_altitude_service_topic
Logger.loginfo("goto_altitude_service_topic: %s" %
goto_altitude_service_topic)
constraint_switch_service_topic = rospy.get_param(
param_prefix_namespace + 'main/constraint_switch_service_topic')
constraint_switch_service_topic = "/" + robot_name + "/" + constraint_switch_service_topic
Logger.loginfo("constraint_switch_service_topic: %s" %
constraint_switch_service_topic)
control_manager_switch_controller_service_topic = rospy.get_param(
param_prefix_namespace +
'main/control_manager_switch_controller_service_topic')
control_manager_switch_controller_service_topic = "/" + robot_name + "/" + control_manager_switch_controller_service_topic
Logger.loginfo("control_manager_switch_controller_service_topic: %s" %
control_manager_switch_controller_service_topic)
outside_flying_controller = rospy.get_param(
param_prefix_namespace + 'main/outside_flying_controller')
outside_flying_controller = outside_flying_controller
Logger.loginfo("outside_flying_controller: %s" %
outside_flying_controller)
inside_flying_controller = rospy.get_param(
param_prefix_namespace + 'main/inside_flying_controller')
inside_flying_controller = inside_flying_controller
Logger.loginfo("inside_flying_controller: %s" %
inside_flying_controller)
extinguishing_controller = rospy.get_param(
param_prefix_namespace + 'main/extinguishing_controller')
extinguishing_controller = extinguishing_controller
Logger.loginfo("extinguishing_controller: %s" %
extinguishing_controller)
outside_flying_constraints = rospy.get_param(
param_prefix_namespace + 'main/outside_flying_constraints')
outside_flying_constraints = outside_flying_constraints
Logger.loginfo("outside_flying_constraints: %s" %
outside_flying_constraints)
inside_flying_constraints = rospy.get_param(
param_prefix_namespace + 'main/inside_flying_constraints')
inside_flying_constraints = inside_flying_constraints
Logger.loginfo("inside_flying_constraints: %s" %
inside_flying_constraints)
extinguishing_constraints = rospy.get_param(
param_prefix_namespace + 'main/extinguishing_constraints')
extinguishing_constraints = extinguishing_constraints
Logger.loginfo("extinguishing_constraints: %s" %
extinguishing_constraints)
flying_altitude = 3
for i in range(len(uav_name_list)):
if uav_name_list[i] == robot_name:
flying_altitude = flying_altitude_list[i]
break
orbit_goal = lidar_flier.msg.lfGoal()
orbit_goal.altitude = flying_altitude
orbit_goal.orbit = True
orbit_goal.frame_id = str(robot_name + "/gps_origin")
orbit_goal.speed = lidar_flier_speed_orbit
orbit_goal.stick_distance = lidar_flier_stick_distance
orbit_goal.clockwise = lidar_flier_clockwise
_state_machine.userdata.flying_altitude = flying_altitude
_state_machine.userdata.orbit_goal = orbit_goal
_state_machine.userdata.lidar_flier_speed_goto = lidar_flier_speed_goto
Logger.loginfo("fire_ch_sm.flying_altitude: %s" %
_state_machine.userdata.flying_altitude)
Logger.loginfo("fire_ch_sm.orbit_goal: %s" %
_state_machine.userdata.orbit_goal)
Logger.loginfo("fire_ch_sm.lidar_flier_speed_goto: %s" %
_state_machine.userdata.lidar_flier_speed_goto)
# [/MANUAL_CREATE]
with _state_machine:
# x:92 y:74
OperatableStateMachine.add(
'WaitForStart',
ServiceWaitForStart(
service_topic=wait_for_start_service_topic),
transitions={
'received_one': 'GotoAltitude',
'received_two': 'Land',
'received_three': 'Land',
'error': 'Land'
},
autonomy={
'received_one': Autonomy.Off,
'received_two': Autonomy.Off,
'received_three': Autonomy.Off,
'error': Autonomy.Off
},
remapping={'start_value': 'start_value'})
# x:328 y:74
OperatableStateMachine.add(
'GotoAltitude',
ServiceGoToAltitudeState(
service_topic=goto_altitude_service_topic,
control_manager_diagnostics_topic=
control_manager_diagnostics_topic),
transitions={
'successed': 'SaveHomePosition',
'failed': 'Land'
},
autonomy={
'successed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'goal': 'flying_altitude'})
# x:641 y:74
OperatableStateMachine.add(
'SaveHomePosition',
WaitForMsgState(
topic=control_manager_cmd_topic,
wait_time=5,
function=self.callback_true,
input_keys=["lidar_flier_speed_goto"],
output_keys=["home_position"],
output_function=self.home_position_output_function),
transitions={
'successed': 'fire_challenge_outside',
'failed': 'Land'
},
autonomy={
'successed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'lidar_flier_speed_goto': 'lidar_flier_speed_goto',
'home_position': 'home_position'
})
# x:669 y:224
OperatableStateMachine.add(
'LidarFlierGotoHomePosition',
LidarFlierActionState(
action_server_name=lidar_flier_action_server_name),
transitions={
'successed': 'Land',
'no_valid_points_in_scan': 'Land',
'stop_requested': 'Land',
'server_not_initialized': 'Land',
'error': 'Land',
'preempted': 'Land'
},
autonomy={
'successed': Autonomy.Off,
'no_valid_points_in_scan': Autonomy.Off,
'stop_requested': Autonomy.Off,
'server_not_initialized': Autonomy.Off,
'error': Autonomy.Off,
'preempted': Autonomy.Off
},
remapping={'goal': 'home_position'})
# x:442 y:374
OperatableStateMachine.add('Land',
ServiceTriggerState(
service_topic=land_service_topic,
state_name="ControlManager"),
transitions={
'finished': 'WasMissionSuccessful',
'failed': 'Land'
},
autonomy={
'finished': Autonomy.Off,
'failed': Autonomy.Off
})
# x:869 y:71
OperatableStateMachine.add('fire_challenge_outside',
self.use_behavior(
fire_challenge_outsideSM,
'fire_challenge_outside'),
transitions={
'no_detection':
'LidarFlierGotoHomePosition',
'water_depleted':
'SetMissionSuccess',
'mission_part_failure':
'LidarFlierGotoHomePosition',
'preempted':
'LidarFlierGotoHomePosition',
'target_lost':
'LidarFlierGotoHomePosition'
},
autonomy={
'no_detection': Autonomy.Inherit,
'water_depleted': Autonomy.Inherit,
'mission_part_failure':
Autonomy.Inherit,
'preempted': Autonomy.Inherit,
'target_lost': Autonomy.Inherit
})
# x:981 y:374
OperatableStateMachine.add(
'WasMissionSuccessful',
CheckConditionState(predicate=lambda x: x),
transitions={
'true': 'mission_successful',
'false': 'missing_failure'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={'input_value': 'is_mission_successful'})
# x:983 y:224
OperatableStateMachine.add(
'SetMissionSuccess',
SetVariableToTrueState(key=['is_mission_successful'],
function=self.set_success_variable),
transitions={'done': 'LidarFlierGotoHomePosition'},
autonomy={'done': Autonomy.Off},
remapping={'is_mission_successful': 'is_mission_successful'})
return _state_machine
def create(self):
# x:214 y:238, x:1435 y:649, x:153 y:589, x:383 y:502, x:1263 y:790
_state_machine = OperatableStateMachine(outcomes=[
'no_detection', 'water_depleted', 'mission_part_failure',
'preempted', 'target_lost'
])
_state_machine.userdata.flying_altitude = 5
_state_machine.userdata.orbit_goal = lidar_flier.msg.lfGoal()
_state_machine.userdata.fire_position = fire_detect.msg.firemanGoal()
_state_machine.userdata.fire_extinguish_position = lidar_flier.msg.lfGoal(
)
_state_machine.userdata.is_preempted = False
_state_machine.userdata.is_water_depleted = False
_state_machine.userdata.is_target_lost = False
_state_machine.userdata.lidar_flier_speed_goto = 0.5
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
param_prefix_namespace = "flexbe_behavior_launcher/"
flying_altitude_list = rospy.get_param(param_prefix_namespace +
'main/flying_altitudes')
uav_name_list = rospy.get_param(param_prefix_namespace +
'main/robot_name_list')
robot_name = rospy.get_param(param_prefix_namespace + 'robot_name')
lidar_flier_speed_goto = rospy.get_param(
param_prefix_namespace + 'lidar_flier/lidar_flier_speed_goto')
lidar_flier_speed_orbit = rospy.get_param(
param_prefix_namespace + 'lidar_flier/lidar_flier_speed_orbit')
lidar_flier_clockwise = rospy.get_param(
param_prefix_namespace + 'lidar_flier/lidar_flier_clockwise')
lidar_flier_stick_distance = rospy.get_param(
param_prefix_namespace + 'lidar_flier/lidar_flier_stick_distance')
fire_detection_topic = rospy.get_param(param_prefix_namespace +
'main/fire_detection_topic')
fire_detection_topic = "/" + robot_name + "/" + fire_detection_topic
Logger.loginfo("fire_detection_topic: %s" % fire_detection_topic)
lidar_flier_action_server_name = rospy.get_param(
param_prefix_namespace + 'main/lidar_flier_action_server_name')
lidar_flier_action_server_name = "/" + robot_name + "/" + lidar_flier_action_server_name
Logger.loginfo("lidar_flier_action_server_name: %s" %
lidar_flier_action_server_name)
fire_extinguish_action_server_name = rospy.get_param(
param_prefix_namespace + 'main/fire_extinguish_action_server_name')
fire_extinguish_action_server_name = "/" + robot_name + "/" + fire_extinguish_action_server_name
Logger.loginfo("fire_extinguish_action_server_name: %s" %
fire_extinguish_action_server_name)
control_manager_diagnostics_topic = rospy.get_param(
param_prefix_namespace + 'main/control_manager_diagnostics_topic')
control_manager_diagnostics_topic = "/" + robot_name + "/" + control_manager_diagnostics_topic
Logger.loginfo("control_manager_diagnostics_topic: %s" %
control_manager_diagnostics_topic)
control_manager_cmd_topic = rospy.get_param(
param_prefix_namespace + 'main/control_manager_cmd_topic')
control_manager_cmd_topic = "/" + robot_name + "/" + control_manager_cmd_topic
Logger.loginfo("control_manager_cmd_topic: %s" %
control_manager_cmd_topic)
constraint_switch_service_topic = rospy.get_param(
param_prefix_namespace + 'main/constraint_switch_service_topic')
constraint_switch_service_topic = "/" + robot_name + "/" + constraint_switch_service_topic
Logger.loginfo("constraint_switch_service_topic: %s" %
constraint_switch_service_topic)
control_manager_switch_controller_service_topic = rospy.get_param(
param_prefix_namespace +
'main/control_manager_switch_controller_service_topic')
control_manager_switch_controller_service_topic = "/" + robot_name + "/" + control_manager_switch_controller_service_topic
Logger.loginfo("control_manager_switch_controller_service_topic: %s" %
control_manager_switch_controller_service_topic)
outside_flying_controller = rospy.get_param(
param_prefix_namespace + 'main/outside_flying_controller')
outside_flying_controller = outside_flying_controller
Logger.loginfo("outside_flying_controller: %s" %
outside_flying_controller)
extinguishing_controller = rospy.get_param(
param_prefix_namespace + 'main/extinguishing_controller')
extinguishing_controller = extinguishing_controller
Logger.loginfo("extinguishing_controller: %s" %
extinguishing_controller)
outside_flying_constraints = rospy.get_param(
param_prefix_namespace + 'main/outside_flying_constraints')
outside_flying_constraints = outside_flying_constraints
Logger.loginfo("outside_flying_constraints: %s" %
outside_flying_constraints)
extinguishing_constraints = rospy.get_param(
param_prefix_namespace + 'main/extinguishing_constraints')
extinguishing_constraints = extinguishing_constraints
Logger.loginfo("extinguishing_constraints: %s" %
extinguishing_constraints)
flying_altitude = 3
for i in range(len(uav_name_list)):
if uav_name_list[i] == robot_name:
flying_altitude = flying_altitude_list[i]
break
orbit_goal = lidar_flier.msg.lfGoal()
orbit_goal.altitude = flying_altitude
orbit_goal.orbit = True
orbit_goal.frame_id = str(robot_name + "/gps_origin")
orbit_goal.speed = lidar_flier_speed_orbit
orbit_goal.stick_distance = lidar_flier_stick_distance
orbit_goal.clockwise = lidar_flier_clockwise
_state_machine.userdata.flying_altitude = flying_altitude
_state_machine.userdata.orbit_goal = orbit_goal
_state_machine.userdata.lidar_flier_speed_goto = lidar_flier_speed_goto
Logger.loginfo("fire_ch_sm.flying_altitude: %s" %
_state_machine.userdata.flying_altitude)
Logger.loginfo("fire_ch_sm.orbit_goal: %s" %
_state_machine.userdata.orbit_goal)
Logger.loginfo("fire_ch_sm.lidar_flier_speed_goto: %s" %
_state_machine.userdata.lidar_flier_speed_goto)
# [/MANUAL_CREATE]
# x:30 y:490, x:394 y:528, x:276 y:624, x:711 y:486, x:30 y:576, x:530 y:490, x:630 y:490, x:894 y:493, x:614 y:408, x:778 y:489, x:591 y:614, x:687 y:612, x:1230 y:490
_sm_lookforfire_0 = ConcurrencyContainer(
outcomes=['error', 'preempted', 'no_detection', 'fire_detected'],
input_keys=[
'orbit_goal', 'fire_position', 'lidar_flier_speed_goto',
'fire_extinguish_position'
],
output_keys=['fire_position', 'fire_extinguish_position'],
conditions=[
('error', [('LidarFlierOrbit', 'error')]),
('preempted', [('LidarFlierOrbit', 'preempted')]),
('error', [('LidarFlierOrbit', 'server_not_initialized')]),
('error', [('LidarFlierOrbit', 'stop_requested')]),
('error', [('LidarFlierOrbit', 'no_valid_points_in_scan')]),
('no_detection', [('LidarFlierOrbit', 'successed')]),
('fire_detected', [('WaitForDetection', 'successed')]),
('error', [('WaitForDetection', 'failed')]),
('error', [('InfiniteWait', 'done')])
])
with _sm_lookforfire_0:
# x:537 y:175
OperatableStateMachine.add('InfiniteWait',
WaitState(wait_time=1000000),
transitions={'done': 'error'},
autonomy={'done': Autonomy.Off})
# x:118 y:146
OperatableStateMachine.add(
'LidarFlierOrbit',
LidarFlierActionState(
action_server_name=lidar_flier_action_server_name),
transitions={
'successed': 'no_detection',
'no_valid_points_in_scan': 'error',
'stop_requested': 'error',
'server_not_initialized': 'error',
'error': 'error',
'preempted': 'preempted'
},
autonomy={
'successed': Autonomy.Off,
'no_valid_points_in_scan': Autonomy.Off,
'stop_requested': Autonomy.Off,
'server_not_initialized': Autonomy.Off,
'error': Autonomy.Off,
'preempted': Autonomy.Off
},
remapping={'goal': 'orbit_goal'})
# x:828 y:141
OperatableStateMachine.add(
'WaitForDetection',
WaitForMsgState(
topic=fire_detection_topic,
wait_time=-1,
function=self.callback_true,
input_keys=['lidar_flier_speed_goto'],
output_keys=['fire_extinguish_position', 'fire_position'],
output_function=self.fire_detect_output_function),
transitions={
'successed': 'fire_detected',
'failed': 'error'
},
autonomy={
'successed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'lidar_flier_speed_goto': 'lidar_flier_speed_goto',
'fire_extinguish_position': 'fire_extinguish_position',
'fire_position': 'fire_position'
})
with _state_machine:
# x:322 y:26
OperatableStateMachine.add('LookForFire',
_sm_lookforfire_0,
transitions={
'error':
'mission_part_failure',
'preempted':
'preempted',
'no_detection':
'no_detection',
'fire_detected':
'LidarFlierGotoExtinguishPosition'
},
autonomy={
'error': Autonomy.Inherit,
'preempted': Autonomy.Inherit,
'no_detection': Autonomy.Inherit,
'fire_detected': Autonomy.Inherit
},
remapping={
'orbit_goal':
'orbit_goal',
'fire_position':
'fire_position',
'lidar_flier_speed_goto':
'lidar_flier_speed_goto',
'fire_extinguish_position':
'fire_extinguish_position'
})
# x:856 y:209
OperatableStateMachine.add(
'FireExtinguish',
FireExtinguishActionState(
action_server_name=fire_extinguish_action_server_name),
transitions={
'successed': 'SetExtinguishingAsSuccessful',
'lost_target': 'SetTargetAsLost',
'error': 'SwitchControlletForFlyingOutside',
'preempted': 'SetPreempted'
},
autonomy={
'successed': Autonomy.Off,
'lost_target': Autonomy.Off,
'error': Autonomy.Off,
'preempted': Autonomy.Off
},
remapping={
'goal': 'fire_position',
'is_tank_depleted': 'is_tank_depleted'
})
# x:543 y:27
OperatableStateMachine.add(
'LidarFlierGotoExtinguishPosition',
LidarFlierActionState(
action_server_name=lidar_flier_action_server_name),
transitions={
'successed': 'SwitchControllerForExtinguishing',
'no_valid_points_in_scan': 'mission_part_failure',
'stop_requested': 'mission_part_failure',
'server_not_initialized': 'mission_part_failure',
'error': 'mission_part_failure',
'preempted': 'preempted'
},
autonomy={
'successed': Autonomy.Off,
'no_valid_points_in_scan': Autonomy.Off,
'stop_requested': Autonomy.Off,
'server_not_initialized': Autonomy.Off,
'error': Autonomy.Off,
'preempted': Autonomy.Off
},
remapping={'goal': 'fire_extinguish_position'})
# x:848 y:29
OperatableStateMachine.add(
'SwitchControllerForExtinguishing',
ServiceStringState(
service_topic=
control_manager_switch_controller_service_topic,
msg_text=extinguishing_controller,
state_name="ControlManager"),
transitions={
'finished': 'SwitchConstraintsForExtinguishing',
'failed': 'SwitchControllerForExtinguishing'
},
autonomy={
'finished': Autonomy.Off,
'failed': Autonomy.Off
})
# x:838 y:416
OperatableStateMachine.add(
'SwitchControlletForFlyingOutside',
ServiceStringState(
service_topic=
control_manager_switch_controller_service_topic,
msg_text=outside_flying_controller,
state_name="ControlManager"),
transitions={
'finished': 'SwitchConstraintsForOutsideFlying',
'failed': 'SwitchControlletForFlyingOutside'
},
autonomy={
'finished': Autonomy.Off,
'failed': Autonomy.Off
})
# x:837 y:128
OperatableStateMachine.add(
'SwitchConstraintsForExtinguishing',
ServiceStringState(
service_topic=constraint_switch_service_topic,
msg_text=extinguishing_constraints,
state_name="ControlManager"),
transitions={
'finished': 'FireExtinguish',
'failed': 'SwitchConstraintsForExtinguishing'
},
autonomy={
'finished': Autonomy.Off,
'failed': Autonomy.Off
})
# x:832 y:520
OperatableStateMachine.add(
'SwitchConstraintsForOutsideFlying',
ServiceStringState(
service_topic=constraint_switch_service_topic,
msg_text=outside_flying_constraints,
state_name="ControlManager"),
transitions={
'finished': 'WasExtinguishingSuccessful',
'failed': 'SwitchConstraintsForOutsideFlying'
},
autonomy={
'finished': Autonomy.Off,
'failed': Autonomy.Off
})
# x:873 y:717
OperatableStateMachine.add(
'WasPreempted',
CheckConditionState(predicate=lambda x: x == True),
transitions={
'true': 'preempted',
'false': 'WasTargetLost'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={'input_value': 'is_preempted'})
# x:854 y:622
OperatableStateMachine.add(
'WasExtinguishingSuccessful',
CheckConditionState(predicate=lambda x: x == True),
transitions={
'true': 'water_depleted',
'false': 'WasPreempted'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={'input_value': 'is_water_depleted'})
# x:873 y:802
OperatableStateMachine.add(
'WasTargetLost',
CheckConditionState(predicate=lambda x: x == True),
transitions={
'true': 'target_lost',
'false': 'mission_part_failure'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={'input_value': 'is_target_lost'})
# x:1253 y:309
OperatableStateMachine.add(
'SetPreempted',
SetVariableToTrueState(key=['is_preempted'],
function=self.set_preempted_variable),
transitions={'done': 'SwitchControlletForFlyingOutside'},
autonomy={'done': Autonomy.Off},
remapping={'is_preempted': 'is_preempted'})
# x:675 y:310
OperatableStateMachine.add(
'SetExtinguishingAsSuccessful',
SetVariableToTrueState(
key=['is_water_depleted'],
function=self.set_water_depleted_variable),
transitions={'done': 'SwitchControlletForFlyingOutside'},
autonomy={'done': Autonomy.Off},
remapping={'is_water_depleted': 'is_water_depleted'})
# x:979 y:310
OperatableStateMachine.add(
'SetTargetAsLost',
SetVariableToTrueState(key=['is_target_lost'],
function=self.set_target_lost_variable),
transitions={'done': 'SwitchControlletForFlyingOutside'},
autonomy={'done': Autonomy.Off},
remapping={'is_target_lost': 'is_target_lost'})
return _state_machine
def create(self):
controller_motor_on_topic = "control_manager/motors"
takeoff_topic = "uav_manager/takeoff"
wait_time_after_setting = 0.5
wait_time_for_takeoff = 10
control_diagnostic_topic = "control_manager/diagnostics"
e_land_topic = "control_manager/eland"
# x:1309 y:290, x:588 y:387
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['simulation'])
_state_machine.userdata.simulation = True
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:41 y:224
OperatableStateMachine.add(
'Set_Motors_ON',
ServiceSetBoolState(service_topic=controller_motor_on_topic,
request=True,
state_name="ControlManager"),
transitions={
'finished': 'Wait_After_Motors_ON',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Off,
'failed': Autonomy.Off
})
# x:397 y:174
OperatableStateMachine.add('Arming',
MavrosArmState(request=True),
transitions={
'finished': 'Set_Offboard',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Off,
'failed': Autonomy.Off
})
# x:539 y:24
OperatableStateMachine.add('Set_Offboard',
MavrosSetModeState(request="offboard"),
transitions={
'finished':
'Wait_After_Switching_To_Offboard',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Off,
'failed': Autonomy.Off
})
# x:240 y:24
OperatableStateMachine.add(
'Check_If_To_Arm',
CheckConditionState(
predicate=lambda simulation: simulation == True),
transitions={
'true': 'Arming',
'false': 'Set_Offboard'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={'input_value': 'simulation'})
# x:793 y:174
OperatableStateMachine.add(
'TakeOff',
ServiceTriggerState(service_topic=takeoff_topic,
state_name="ControlManager"),
transitions={
'finished': 'Wait_For_Successful_TakeOff',
'failed': 'Emergency_Landing'
},
autonomy={
'finished': Autonomy.Off,
'failed': Autonomy.Off
})
# x:133 y:124
OperatableStateMachine.add(
'Wait_After_Motors_ON',
WaitState(wait_time=wait_time_after_setting),
transitions={'done': 'Check_If_To_Arm'},
autonomy={'done': Autonomy.Off})
# x:754 y:24
OperatableStateMachine.add(
'Wait_After_Switching_To_Offboard',
WaitState(wait_time=wait_time_after_setting),
transitions={'done': 'TakeOff'},
autonomy={'done': Autonomy.Off})
# x:1016 y:274
OperatableStateMachine.add('Wait_For_Successful_TakeOff',
WaitForMsgState(
topic=control_diagnostic_topic,
wait_time=wait_time_for_takeoff,
function=self.mrs_takeoff_cb,
output_keys=[],
output_function=None),
transitions={
'successed': 'finished',
'failed': 'Emergency_Landing'
},
autonomy={
'successed': Autonomy.Off,
'failed': Autonomy.Off
})
# x:788 y:324
OperatableStateMachine.add('Emergency_Landing',
ServiceTriggerState(
service_topic=e_land_topic,
state_name="ControlManager"),
transitions={
'finished': 'failed',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Off,
'failed': Autonomy.Off
})
return _state_machine