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