def const_synch_tree():
have_coarse_mission = C_HaveCoarseMission()
# we need one here too, to initialize the mission in the first place
# set dont_visit to True so we dont skip the first wp of the plan
# and simply ready the bb to have the waypoint in it
set_next_plan_action = A_SetNextPlanAction(do_not_visit=True)
return Sequence(name="SQ_GotMission",
children=[have_coarse_mission, set_next_plan_action])
def const_execute_mission_tree():
gotowp = A_GotoWaypoint(action_namespace=auv_config.ACTION_NAMESPACE,
goal_tolerance=auv_config.WAYPOINT_TOLERANCE,
goal_tf_frame=auv_config.UTM_LINK)
follow_plan = Sequence(name="SQ-FollowMissionPlan",
children=[
C_HaveCoarseMission(),
C_StartPlanReceived(),
C_PlanIsNotChanged(), gotowp,
A_SetNextPlanAction()
])
return Fallback(name="FB-ExecuteMissionPlan",
children=[C_PlanCompleted(), follow_plan])
def const_execute_mission_tree():
# waypoint type
wp_is_goto = C_CheckWaypointType(
expected_wp_type=imc_enums.MANEUVER_GOTO)
wp_is_sample = C_CheckWaypointType(
expected_wp_type=imc_enums.MANEUVER_SAMPLE)
wp_is = Fallback("FB-IsWaypoint", children=[wp_is_goto, wp_is_sample])
goto_action = A_GotoWaypoint(
action_namespace=auv_config.ACTION_NAMESPACE,
goal_tolerance=auv_config.WAYPOINT_TOLERANCE,
goal_tf_frame=auv_config.UTM_LINK)
goto_maneuver = Sequence(name="SQ-GotoWaypoint",
children=[wp_is, goto_action])
#############################################################################################
# INSPECT
#TODO add an inspection maneuver into bridge and neptus etc.
# wp_is_inspect = C_CheckWaypointType(expected_wp_type = imc_enums.MANEUVER_INSPECT)
#inspection_action = A_GotoWaypoint(action_namespace = auv_config.INSPECTION_ACTION_NAMESPACE,
# goal_tolerance = auv_config.WAYPOINT_TOLERANCE,
# goal_tf_frame = auv_config.UTM_LINK)
# inspection_maneuver = Sequence(name="SQ-InspectWP",
# children=[
# wp_is_inspect,
# inspection_action
# ])
#############################################################################################
# put the known plannable maneuvers in here as each others backups
execute_maneuver = Fallback(name="FB-ExecuteManeuver",
children=[
goto_maneuver,
])
# and then execute them in order
follow_plan = Sequence(name="SQ-FollowMissionPlan",
children=[
C_HaveCoarseMission(),
C_StartPlanReceived(),
C_PlanIsNotChanged(), execute_maneuver,
A_SetNextPlanAction()
])
# until the plan is done
return Fallback(name="FB-ExecuteMissionPlan",
children=[C_PlanCompleted(), follow_plan])
def const_synch_tree():
have_refined_mission = C_HaveRefinedMission()
have_coarse_mission = C_HaveCoarseMission()
refine_mission = A_RefineMission(config.PATH_PLANNER_NAME,
config.PATH_TOPIC)
# we need one here too, to initialize the mission in the first place
# set dont_visit to True so we dont skip the first wp of the plan
set_next_plan_action = A_SetNextPlanAction(do_not_visit=True)
refinement_tree = Sequence(name="SQ_Refinement",
children=[
have_coarse_mission, refine_mission,
set_next_plan_action
])
return Fallback(name='FB_SynchMission',
children=[have_refined_mission, refinement_tree])
def const_execute_mission_tree():
plan_complete = C_PlanCompleted()
# but still wait for operator to tell us to 'go'
start_received = C_StartPlanReceived()
gotowp = A_GotoWaypoint(auv_config.ACTION_NAMESPACE)
# and this will run after every success of the goto action
set_next_plan_action = A_SetNextPlanAction()
plan_is_same = C_PlanIsNotChanged()
# idle = pt.behaviours.Running(name="Idle")
follow_plan = Sequence(name="SQ-FollowMissionPlan",
children=[
start_received, plan_is_same, gotowp,
set_next_plan_action
])
return Fallback(
name="FB-ExecuteMissionPlan",
children=[
plan_complete, follow_plan
# idle
])
def const_safety_tree():
no_abort = C_NoAbortReceived()
altOK = C_AltOK(auv_config.MIN_ALTITUDE,
auv_config.ABSOLUTE_MIN_ALTITUDE)
depthOK = C_DepthOK(auv_config.MAX_DEPTH)
leakOK = C_LeakOK()
# more safety checks will go here
safety_checks = Sequence(name="SQ-SafetyChecks",
blackbox_level=1,
children=[no_abort, altOK, depthOK, leakOK])
surface = Fallback(
name="FB_Surface",
children=[
A_EmergencySurface(auv_config.EMERGENCY_ACTION_NAMESPACE)
# A_EmergencySurfaceByForce(auv_config.EMERGENCY_TOPIC,
# auv_config.VBS_CMD_TOPIC,
# auv_config.RPM_CMD_TOPIC,
# auv_config.LCG_PID_ENABLE_TOPIC,
# auv_config.VBS_PID_ENABLE_TOPIC,
# auv_config.TCG_PID_ENABLE_TOPIC,
# auv_config.YAW_PID_ENABLE_TOPIC,
# auv_config.DEPTH_PID_ENABLE_TOPIC,
# auv_config.VEL_PID_ENABLE_TOPIC)
])
skip_wp = Sequence(
name='SQ-CountEmergenciesAndSkip',
children=[
Counter(n=auv_config.EMERGENCY_TRIALS_BEFORE_GIVING_UP,
name="A_EmergencyCounter",
reset=True),
A_SetNextPlanAction()
])
return Fallback(name='FB_SafetyOK',
children=[safety_checks, skip_wp, surface])
def const_safety_tree():
no_abort = C_NoAbortReceived()
altOK = C_AltOK(auv_config.MIN_ALTITUDE,
auv_config.ABSOLUTE_MIN_ALTITUDE)
depthOK = C_DepthOK(auv_config.MAX_DEPTH)
leakOK = C_LeakOK()
# more safety checks will go here
safety_checks = Sequence(name="SQ-SafetyChecks",
blackbox_level=1,
children=[no_abort, altOK, depthOK, leakOK])
skip_wp = Sequence(
name='SQ-CountEmergenciesAndSkip',
children=[
Counter(n=auv_config.EMERGENCY_TRIALS_BEFORE_GIVING_UP,
name="A_EmergencyCounter",
reset=True),
A_SetNextPlanAction()
])
abort = Sequence(
name="SQ-ABORT",
children=[
A_SimplePublisher(topic=auv_config.EMERGENCY_TOPIC,
message_object=Empty()),
A_EmergencySurface(auv_config.EMERGENCY_ACTION_NAMESPACE)
])
return Fallback(
name='FB_SafetyOK',
children=[
safety_checks, skip_wp, abort
# publish_abort,
# A_EmergencySurface(auv_config.EMERGENCY_ACTION_NAMESPACE)
])