def const_buoy_localisation_tree():
# buoys key = english, value = array indicies
sides = ['Right', 'Back', 'Left', 'Back']
# set plans
localistion_tree = [
Fallback(
'FB-{}BuoyLocalised'.format(side),
children=[
C_Buoys_Localised(
name='C_{}Buoys_Localised'.format(side),
side=side.lower(),
localisation_tol=auv_config.BUOY_LOCALISATION_TOL,
bb_enum='{}_buoys_localised'.format(
side.lower()) # NOTE: dumb
),
A_Set_Buoy_Localisation_Plan(
name='A_Set{}BuoyPlan'.format(side),
side=side.lower(),
n_rows=auv_config.BUOY_LOCALISATION_N_ROWS,
row_sep=auv_config.BUOY_LOCALISTION_ROW_SEP,
row_overshoot=auv_config.
BUOY_LOCALISATION_ROW_OVERSHOOT,
# should just get this from the buoy poses themselves...
buoy_link=auv_config.LOCAL_LINK,
clockwise=auv_config.BUOY_LOCALISATION_CLOCKWISE,
velocity=auv_config.BUOY_LOCALISATION_VELOCITY,
latlon_utm_serv=auv_config.LATLONTOUTM_SERVICE,
utm_link=auv_config.UTM_LINK)
]) for side in sides
]
localistion_tree = Sequence('SQ-LocalisationTree',
children=localistion_tree)
# NOTE: this is dumb
# this is until we can check to covariance
# matricies of the buoys
decision_tree = Sequence('SQ-LocalisationDecisions',
children=[
C_PlanCompleted(),
C_Buoy_Localisation_Plan_Set(),
A_Set_Buoy_Decisions()
])
localistion_tree = Fallback('FB-LocalisationTree',
children=[
decision_tree,
C_Buoy_Localisation_Plan_Set(),
localistion_tree
])
'''
Look at `py-tree-tree-watcher` to understand what this
structure does.
'''
return localistion_tree
def const_finalize_mission():
publish_complete = A_SimplePublisher(
topic=auv_config.MISSION_COMPLETE_TOPIC, message_object=Empty())
set_finalized = pt.blackboard.SetBlackboardVariable(
variable_name=bb_enums.MISSION_FINALIZED,
variable_value=True,
name='A_SetMissionFinalized->True')
unset_plan_is_go = pt.blackboard.SetBlackboardVariable(
variable_name=bb_enums.PLAN_IS_GO,
variable_value=False,
name='A_SetPlanIsGo->False')
#surface on plan completion
#planned_surface = A_GotoWaypoint(action_namespace = auv_config.PLANNED_SURFACE_ACTION_NAMESPACE,
# goal_tolerance = auv_config.WAYPOINT_TOLERANCE,
# goal_tf_frame = auv_config.UTM_LINK)
#is_submerged = C_AtDVLDepth(0.5)
return Sequence(
name="SQ-FinalizeMission",
children=[
C_HaveCoarseMission(),
C_PlanIsNotChanged(),
C_PlanCompleted(),
#is_submerged,
#planned_surface,
publish_complete,
unset_plan_is_go,
set_finalized
])
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_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_sampling_tree():
# sample
need_to_sample = C_NeedToSample()
sample_action = A_Sample(action_namespace=auv_config.SAMPLE_NAMESPACE,
timeout=auv_config.SAMPLE_TIMEOUT)
sample_maneuver = Sequence(name="SQ-SampleWaypoint",
children=[need_to_sample, sample_action])
return sample_maneuver
def const_execute_mission_tree():
# GOTO
goto_action = A_GotoWaypoint(
action_namespace=auv_config.ACTION_NAMESPACE,
goal_tolerance=auv_config.WAYPOINT_TOLERANCE,
goal_tf_frame=auv_config.UTM_LINK)
wp_is_goto = C_CheckWaypointType(
expected_wp_type=imc_enums.MANEUVER_GOTO)
goto_maneuver = Sequence(name="SQ-GotoWaypoint",
children=[wp_is_goto, goto_action])
def const_leader_follower():
return Sequence(
name="SQ_LeaderFollower",
children=[
C_LeaderFollowerEnabled(config.ENABLE_LEADER_FOLLOWER),
C_LeaderExists(config.BASE_LINK, config.LEADER_LINK),
C_LeaderIsFarEnough(config.BASE_LINK, config.LEADER_LINK,
config.MIN_DISTANCE_TO_LEADER),
A_FollowLeader(config.FOLLOW_ACTION_NAMESPACE,
config.LEADER_LINK)
])
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_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_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)
])
def const_finalize_mission():
publish_complete = A_SimplePublisher(
topic=auv_config.MISSION_COMPLETE_TOPIC, message_object=Empty())
set_finalized = pt.blackboard.SetBlackboardVariable(
variable_name=bb_enums.MISSION_FINALIZED,
variable_value=True,
name='A_SetMissionFinalized')
return Sequence(name="SQ-FinalizeMission",
children=[
C_HaveCoarseMission(),
C_StartPlanReceived(),
C_PlanIsNotChanged(),
C_PlanCompleted(), publish_complete, set_finalized
])
def const_neptus_tree():
update_neptus = Sequence(
name="SQ-UpdateNeptus",
children=[
A_UpdateNeptusEstimatedState(
auv_config.ESTIMATED_STATE_TOPIC),
A_UpdateNeptusPlanControlState(
auv_config.PLAN_CONTROL_STATE_TOPIC),
A_UpdateNeptusVehicleState(auv_config.VEHICLE_STATE_TOPIC),
A_UpdateNeptusPlanDB(auv_config.PLANDB_TOPIC,
auv_config.UTM_LINK,
auv_config.LOCAL_LINK),
A_UpdateNeptusPlanControl(auv_config.PLAN_CONTROL_TOPIC),
A_VizPublishPlan(auv_config.PLAN_VIZ_TOPIC)
])
return update_neptus
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_dvl_tree():
switch_on = Sequence(name="SQ_SwitchOnDVL",
children=[
C_AtDVLDepth(auv_config.DVL_RUNNING_DEPTH),
A_SetDVLRunning(
auv_config.START_STOP_DVL_NAMESPACE, True,
auv_config.DVL_COOLDOWN)
])
switch_off = Fallback(name="FB_SwitchOffDVL",
children=[
switch_on,
A_SetDVLRunning(
auv_config.START_STOP_DVL_NAMESPACE,
False, auv_config.DVL_COOLDOWN)
])
return switch_off
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_tree(auv_config):
"""
construct the entire tree.
the structure of the code reflects the structure of the tree itself.
sub-trees are constructed in inner functions.
auv_config is in scope of all these inner functions.
auv_config is a simple data object with a bunch of UPPERCASE fields in it.
"""
# slightly hacky way to keep track of 'runnable' actions
# such actions should add their names to this list in their init
# just for Neptus vehicle state for now
bb = pt.blackboard.Blackboard()
bb.set(bb_enums.MANEUVER_ACTIONS, [])
# just for clarity when looking at the bb in the field
bb.set(bb_enums.MISSION_FINALIZED, False)
def const_data_ingestion_tree():
read_abort = ptr.subscribers.EventToBlackboard(
name="A_ReadAbort",
topic_name=auv_config.ABORT_TOPIC,
variable_name=bb_enums.ABORT)
# ReadTopic
# name,
# topic_name,
# topic_type,
# blackboard_variables,
# max_period = None,
# allow_silence = True
read_alt = ReadTopic(
name="A_ReadAlt",
topic_name=auv_config.ALTITUDE_TOPIC,
topic_type=DVL,
blackboard_variables={bb_enums.ALTITUDE: 'altitude'})
read_leak = ReadTopic(name="A_ReadLeak",
topic_name=auv_config.LEAK_TOPIC,
topic_type=Leak,
blackboard_variables={bb_enums.LEAK: 'value'})
read_detection = ReadTopic(
name="A_ReadCameraDetection",
topic_name=auv_config.CAMERA_DETECTION_TOPIC,
topic_type=PointStamped,
# read the entire message into the bb
blackboard_variables={bb_enums.POI_POINT_STAMPED: None})
read_latlon = ReadTopic(name="A_ReadLatlon",
topic_name=auv_config.LATLON_TOPIC,
topic_type=GeoPoint,
blackboard_variables={
bb_enums.CURRENT_LATITUDE: 'latitude',
bb_enums.CURRENT_LONGITUDE: 'longitude'
})
read_sample_obs = ReadTopic(
name="A_ReadSampleObs",
topic_name=auv_config.SAMPLE_OBS,
topic_type=Pose,
blackboard_variables={
# blackboard variable name: attribute of msg
bb_enums.SAMPLE_POS:
'position'
})
read_buoys = A_ReadBuoys(auv_config.BUOY_MARKERS,
auv_config.USE_BUOY_PLAN,
auv_config.LOCAL_LINK, auv_config.UTM_LINK,
auv_config.LATLONTOUTM_SERVICE)
def const_neptus_tree():
update_neptus = Sequence(
name="SQ-UpdateNeptus",
children=[
A_UpdateNeptusEstimatedState(
auv_config.ESTIMATED_STATE_TOPIC),
A_UpdateNeptusPlanControlState(
auv_config.PLAN_CONTROL_STATE_TOPIC),
A_UpdateNeptusVehicleState(auv_config.VEHICLE_STATE_TOPIC),
A_UpdateNeptusPlanDB(
auv_config.PLANDB_TOPIC, auv_config.UTM_LINK,
auv_config.LOCAL_LINK, auv_config.LATLONTOUTM_SERVICE,
auv_config.LATLONTOUTM_SERVICE_ALTERNATIVE),
A_UpdateNeptusPlanControl(auv_config.PLAN_CONTROL_TOPIC),
A_VizPublishPlan(auv_config.PLAN_VIZ_TOPIC)
])
return update_neptus
update_tf = A_UpdateTF(auv_config.UTM_LINK, auv_config.BASE_LINK)
neptus_tree = const_neptus_tree()
publish_heartbeat = A_SimplePublisher(topic=auv_config.HEARTBEAT_TOPIC,
message_object=Empty())
return Sequence(
name="SQ-DataIngestion",
# dont show all the things inside here
blackbox_level=1,
children=[
read_abort, read_leak, read_alt, read_detection, read_latlon,
read_buoys, update_tf, neptus_tree, publish_heartbeat
])
def const_dvl_tree():
switch_on = Sequence(name="SQ_SwitchOnDVL",
children=[
C_AtDVLDepth(auv_config.DVL_RUNNING_DEPTH),
A_SetDVLRunning(
auv_config.START_STOP_DVL_NAMESPACE, True,
auv_config.DVL_COOLDOWN)
])
switch_off = Fallback(name="FB_SwitchOffDVL",
children=[
switch_on,
A_SetDVLRunning(
auv_config.START_STOP_DVL_NAMESPACE,
False, auv_config.DVL_COOLDOWN)
])
return switch_off
def const_safety_tree():
no_abort = C_NoAbortReceived()
altOK = C_AltOK()
depthOK = C_DepthOK()
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])
def const_leader_follower():
return Sequence(
name="SQ_LeaderFollower",
children=[
C_LeaderFollowerEnabled(config.ENABLE_LEADER_FOLLOWER),
C_LeaderExists(config.BASE_LINK, config.LEADER_LINK),
C_LeaderIsFarEnough(config.BASE_LINK, config.LEADER_LINK,
config.MIN_DISTANCE_TO_LEADER),
A_FollowLeader(config.FOLLOW_ACTION_NAMESPACE,
config.LEADER_LINK)
])
def const_autonomous_updates():
poi_tree = Fallback(name="FB_Poi",
children=[
C_NoNewPOIDetected(common_globals.POI_DIST),
A_UpdateMissonForPOI(
auv_config.UTM_LINK, auv_config.LOCAL_LINK,
auv_config.POI_DETECTOR_LINK,
auv_config.LATLONTOUTM_SERVICE)
])
return Fallback(name="FB_AutonomousUpdates",
children=[C_AutonomyDisabled(), poi_tree])
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_buoy_localisation_tree():
# buoys key = english, value = array indicies
sides = ['Right', 'Back', 'Left', 'Back']
# set plans
localistion_tree = [
Fallback(
'FB-{}BuoyLocalised'.format(side),
children=[
C_Buoys_Localised(
name='C_{}Buoys_Localised'.format(side),
side=side.lower(),
localisation_tol=auv_config.BUOY_LOCALISATION_TOL,
bb_enum='{}_buoys_localised'.format(
side.lower()) # NOTE: dumb
),
A_Set_Buoy_Localisation_Plan(
name='A_Set{}BuoyPlan'.format(side),
side=side.lower(),
n_rows=auv_config.BUOY_LOCALISATION_N_ROWS,
row_sep=auv_config.BUOY_LOCALISTION_ROW_SEP,
row_overshoot=auv_config.
BUOY_LOCALISATION_ROW_OVERSHOOT,
# should just get this from the buoy poses themselves...
buoy_link=auv_config.LOCAL_LINK,
clockwise=auv_config.BUOY_LOCALISATION_CLOCKWISE,
velocity=auv_config.BUOY_LOCALISATION_VELOCITY,
latlon_utm_serv=auv_config.LATLONTOUTM_SERVICE,
utm_link=auv_config.UTM_LINK)
]) for side in sides
]
localistion_tree = Sequence('SQ-LocalisationTree',
children=localistion_tree)
# NOTE: this is dumb
# this is until we can check to covariance
# matricies of the buoys
decision_tree = Sequence('SQ-LocalisationDecisions',
children=[
C_PlanCompleted(),
C_Buoy_Localisation_Plan_Set(),
A_Set_Buoy_Decisions()
])
localistion_tree = Fallback('FB-LocalisationTree',
children=[
decision_tree,
C_Buoy_Localisation_Plan_Set(),
localistion_tree
])
'''
Look at `py-tree-tree-watcher` to understand what this
structure does.
'''
return localistion_tree
def const_wall_plan_tree():
tree = Fallback(
'FB-WallPlanSet',
children=[
CheckBlackboardVariableValue(bb_enums.WALL_PLAN_SET, True,
'C_WallPlanSet'),
A_Set_Wall_Plan(
'A_SetWallPlan', auv_config.WALL_SURVEY_ROW_SEP,
auv_config.WALL_SURVEY_DEPTH, auv_config.LOCAL_LINK,
auv_config.UTM_LINK, auv_config.WALL_SURVEY_VELOCITY,
auv_config.LATLONTOUTM_SERVICE,
auv_config.LATLONTOUTM_SERVICE_ALTERNATIVE,
auv_config.WALL_SURVEY_X0_OVERSHOOT,
auv_config.WALL_SURVEY_X1_OVERSHOOT,
auv_config.WALL_SURVEY_X0_LINEUP,
auv_config.WALL_SURVEY_X1_LINEUP,
auv_config.WALL_SURVEY_FIRST_LINEUP,
auv_config.WALL_SURVEY_STARBOARD)
])
return tree
def const_execute_mission_tree():
# GOTO
goto_action = A_GotoWaypoint(
action_namespace=auv_config.ACTION_NAMESPACE,
goal_tolerance=auv_config.WAYPOINT_TOLERANCE,
goal_tf_frame=auv_config.UTM_LINK)
wp_is_goto = C_CheckWaypointType(
expected_wp_type=imc_enums.MANEUVER_GOTO)
goto_maneuver = Sequence(name="SQ-GotoWaypoint",
children=[wp_is_goto, goto_action])
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_finalize_mission():
publish_complete = A_SimplePublisher(
topic=auv_config.MISSION_COMPLETE_TOPIC, message_object=Empty())
set_finalized = pt.blackboard.SetBlackboardVariable(
variable_name=bb_enums.MISSION_FINALIZED,
variable_value=True,
name='A_SetMissionFinalized->True')
unset_plan_is_go = pt.blackboard.SetBlackboardVariable(
variable_name=bb_enums.PLAN_IS_GO,
variable_value=False,
name='A_SetPlanIsGo->False')
#surface on plan completion
#planned_surface = A_GotoWaypoint(action_namespace = auv_config.PLANNED_SURFACE_ACTION_NAMESPACE,
# goal_tolerance = auv_config.WAYPOINT_TOLERANCE,
# goal_tf_frame = auv_config.UTM_LINK)
#is_submerged = C_AtDVLDepth(0.5)
return Sequence(
name="SQ-FinalizeMission",
children=[
C_HaveCoarseMission(),
C_PlanIsNotChanged(),
C_PlanCompleted(),
#is_submerged,
#planned_surface,
publish_complete,
unset_plan_is_go,
set_finalized
])
# The root of the tree is here
planned_mission = const_execute_mission_tree()
# use this to kind of set the tree to 'idle' mode that wont attempt
# to control anything and just chills as an observer
# finalized = CheckBlackboardVariableValue(bb_enums.MISSION_FINALIZED,
# True,
# "C_MissionFinalized")
def const_sampling_tree():
# sample
need_to_sample = C_NeedToSample()
sample_action = A_Sample(action_namespace=auv_config.SAMPLE_NAMESPACE,
timeout=auv_config.SAMPLE_TIMEOUT)
sample_maneuver = Sequence(name="SQ-SampleWaypoint",
children=[need_to_sample, sample_action])
return sample_maneuver
run_tree = Fallback(
name="FB-Run",
children=[
# finalized,
const_finalize_mission(),
# const_sampling_tree(),
planned_mission
# const_leader_follower()
])
root = Sequence(
name='SQ-ROOT',
children=[
const_data_ingestion_tree(),
const_safety_tree(),
const_wall_plan_tree(),
# const_buoy_localisation_tree(),
# const_dvl_tree(),
run_tree
])
return ptr.trees.BehaviourTree(root)
def const_tree(auv_config):
"""
construct the entire tree.
the structure of the code reflects the structure of the tree itself.
sub-trees are constructed in inner functions.
auv_config is in scope of all these inner functions.
auv_config is a simple data object with a bunch of UPPERCASE fields in it.
"""
# slightly hacky way to keep track of 'runnable' actions
# such actions should add their names to this list in their init
# just for Neptus vehicle state for now
bb = pt.blackboard.Blackboard()
bb.set(bb_enums.MANEUVER_ACTIONS, [])
def const_data_ingestion_tree():
read_abort = ptr.subscribers.EventToBlackboard(
name="A_ReadAbort",
topic_name=auv_config.ABORT_TOPIC,
variable_name=bb_enums.ABORT)
read_depth = ReadTopic(
name="A_ReadDepth",
topic_name=auv_config.DEPTH_TOPIC,
topic_type=Float64,
blackboard_variables={
bb_enums.DEPTH: 'data'
} # this takes the Float64.data field and puts into the bb
)
read_alt = ReadTopic(
name="A_ReadAlt",
topic_name=auv_config.ALTITUDE_TOPIC,
topic_type=DVL,
blackboard_variables={bb_enums.ALTITUDE: 'altitude'})
read_leak = ReadTopic(name="A_ReadLeak",
topic_name=auv_config.LEAK_TOPIC,
topic_type=Leak,
blackboard_variables={bb_enums.LEAK: 'value'})
read_detection = ReadTopic(
name="A_ReadCameraDetection",
topic_name=auv_config.CAMERA_DETECTION_TOPIC,
topic_type=PointStamped,
blackboard_variables={bb_enums.POI_POINT_STAMPED:
None} # read the entire message into the bb
)
read_gps = ReadTopic(name="A_ReadGPS",
topic_name=auv_config.GPS_FIX_TOPIC,
topic_type=NavSatFix,
blackboard_variables={bb_enums.GPS_FIX: None})
def const_neptus_tree():
update_neptus = Sequence(
name="SQ-UpdateNeptus",
children=[
A_UpdateNeptusEstimatedState(
auv_config.ESTIMATED_STATE_TOPIC),
A_UpdateNeptusPlanControlState(
auv_config.PLAN_CONTROL_STATE_TOPIC),
A_UpdateNeptusVehicleState(auv_config.VEHICLE_STATE_TOPIC),
A_UpdateNeptusPlanDB(auv_config.PLANDB_TOPIC,
auv_config.UTM_LINK,
auv_config.LOCAL_LINK),
A_UpdateNeptusPlanControl(auv_config.PLAN_CONTROL_TOPIC),
A_VizPublishPlan(auv_config.PLAN_VIZ_TOPIC)
])
return update_neptus
update_tf = A_UpdateTF(auv_config.UTM_LINK, auv_config.BASE_LINK)
update_latlon = A_UpdateLatLon()
set_utm_from_gps = A_SetUTMFromGPS()
neptus_tree = const_neptus_tree()
return Sequence(
name="SQ-DataIngestion",
# dont show all the things inside here
blackbox_level=1,
children=[
read_abort, read_leak, read_depth, read_alt, read_detection,
read_gps, set_utm_from_gps, update_tf, update_latlon,
neptus_tree
])
def const_dvl_tree():
switch_on = Sequence(name="SQ_SwitchOnDVL",
children=[
C_AtDVLDepth(auv_config.DVL_RUNNING_DEPTH),
A_SetDVLRunning(
auv_config.START_STOP_DVL_NAMESPACE, True,
auv_config.DVL_COOLDOWN)
])
switch_off = Fallback(name="FB_SwitchOffDVL",
children=[
switch_on,
A_SetDVLRunning(
auv_config.START_STOP_DVL_NAMESPACE,
False, auv_config.DVL_COOLDOWN)
])
return switch_off
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_leader_follower():
return Sequence(
name="SQ_LeaderFollower",
children=[
C_LeaderFollowerEnabled(config.ENABLE_LEADER_FOLLOWER),
C_LeaderExists(config.BASE_LINK, config.LEADER_LINK),
C_LeaderIsFarEnough(config.BASE_LINK, config.LEADER_LINK,
config.MIN_DISTANCE_TO_LEADER),
A_FollowLeader(config.FOLLOW_ACTION_NAMESPACE,
config.LEADER_LINK)
])
def const_autonomous_updates():
poi_tree = Fallback(name="FB_Poi",
children=[
C_NoNewPOIDetected(common_globals.POI_DIST),
A_UpdateMissonForPOI(
auv_config.UTM_LINK, auv_config.LOCAL_LINK,
auv_config.POI_DETECTOR_LINK)
])
return Fallback(name="FB_AutonomousUpdates",
children=[C_AutonomyDisabled(), poi_tree])
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
])
# The root of the tree is here
planned_mission = Sequence(name="SQ_PlannedMission",
children=[
const_synch_tree(),
const_autonomous_updates(),
const_execute_mission_tree()
])
run_tree = Fallback(name="FB-Run",
children=[planned_mission,
const_leader_follower()])
root = Sequence(
name='SQ-ROOT',
children=[
const_data_ingestion_tree(),
const_safety_tree(),
#const_dvl_tree(),
run_tree
])
return ptr.trees.BehaviourTree(root)
def const_data_ingestion_tree():
read_abort = ptr.subscribers.EventToBlackboard(
name="A_ReadAbort",
topic_name=auv_config.ABORT_TOPIC,
variable_name=bb_enums.ABORT)
read_alt = ReadTopic(
name="A_ReadAlt",
topic_name=auv_config.ALTITUDE_TOPIC,
topic_type=DVL,
blackboard_variables={bb_enums.ALTITUDE: 'altitude'})
read_leak = ReadTopic(name="A_ReadLeak",
topic_name=auv_config.LEAK_TOPIC,
topic_type=Leak,
blackboard_variables={bb_enums.LEAK: 'value'})
read_detection = ReadTopic(
name="A_ReadCameraDetection",
topic_name=auv_config.CAMERA_DETECTION_TOPIC,
topic_type=PointStamped,
blackboard_variables={bb_enums.POI_POINT_STAMPED:
None} # read the entire message into the bb
)
read_latlon = ReadTopic(name="A_ReadLatlon",
topic_name=auv_config.LATLON_TOPIC,
topic_type=GeoPoint,
blackboard_variables={
bb_enums.CURRENT_LATITUDE: 'latitude',
bb_enums.CURRENT_LONGITUDE: 'longitude'
})
def const_neptus_tree():
update_neptus = Sequence(
name="SQ-UpdateNeptus",
children=[
A_UpdateNeptusEstimatedState(
auv_config.ESTIMATED_STATE_TOPIC),
A_UpdateNeptusPlanControlState(
auv_config.PLAN_CONTROL_STATE_TOPIC),
A_UpdateNeptusVehicleState(auv_config.VEHICLE_STATE_TOPIC),
A_UpdateNeptusPlanDB(auv_config.PLANDB_TOPIC,
auv_config.UTM_LINK,
auv_config.LOCAL_LINK,
auv_config.LATLONTOUTM_SERVICE),
A_UpdateNeptusPlanControl(auv_config.PLAN_CONTROL_TOPIC),
A_VizPublishPlan(auv_config.PLAN_VIZ_TOPIC)
])
return update_neptus
update_tf = A_UpdateTF(auv_config.UTM_LINK, auv_config.BASE_LINK)
neptus_tree = const_neptus_tree()
publish_heartbeat = A_SimplePublisher(topic=auv_config.HEARTBEAT_TOPIC,
message_object=Empty())
return Sequence(
name="SQ-DataIngestion",
# dont show all the things inside here
blackbox_level=1,
children=[
read_abort, read_leak, read_alt, read_detection, read_latlon,
update_tf, neptus_tree, publish_heartbeat
])
def const_data_ingestion_tree():
read_abort = ptr.subscribers.EventToBlackboard(
name="A_ReadAbort",
topic_name=auv_config.ABORT_TOPIC,
variable_name=bb_enums.ABORT)
read_depth = ReadTopic(
name="A_ReadDepth",
topic_name=auv_config.DEPTH_TOPIC,
topic_type=Float64,
blackboard_variables={
bb_enums.DEPTH: 'data'
} # this takes the Float64.data field and puts into the bb
)
read_alt = ReadTopic(
name="A_ReadAlt",
topic_name=auv_config.ALTITUDE_TOPIC,
topic_type=DVL,
blackboard_variables={bb_enums.ALTITUDE: 'altitude'})
read_leak = ReadTopic(name="A_ReadLeak",
topic_name=auv_config.LEAK_TOPIC,
topic_type=Leak,
blackboard_variables={bb_enums.LEAK: 'value'})
read_detection = ReadTopic(
name="A_ReadCameraDetection",
topic_name=auv_config.CAMERA_DETECTION_TOPIC,
topic_type=PointStamped,
blackboard_variables={bb_enums.POI_POINT_STAMPED:
None} # read the entire message into the bb
)
read_gps = ReadTopic(name="A_ReadGPS",
topic_name=auv_config.GPS_FIX_TOPIC,
topic_type=NavSatFix,
blackboard_variables={bb_enums.GPS_FIX: None})
def const_neptus_tree():
update_neptus = Sequence(
name="SQ-UpdateNeptus",
children=[
A_UpdateNeptusEstimatedState(
auv_config.ESTIMATED_STATE_TOPIC),
A_UpdateNeptusPlanControlState(
auv_config.PLAN_CONTROL_STATE_TOPIC),
A_UpdateNeptusVehicleState(auv_config.VEHICLE_STATE_TOPIC),
A_UpdateNeptusPlanDB(auv_config.PLANDB_TOPIC,
auv_config.UTM_LINK,
auv_config.LOCAL_LINK),
A_UpdateNeptusPlanControl(auv_config.PLAN_CONTROL_TOPIC),
A_VizPublishPlan(auv_config.PLAN_VIZ_TOPIC)
])
return update_neptus
update_tf = A_UpdateTF(auv_config.UTM_LINK, auv_config.BASE_LINK)
update_latlon = A_UpdateLatLon()
set_utm_from_gps = A_SetUTMFromGPS()
neptus_tree = const_neptus_tree()
return Sequence(
name="SQ-DataIngestion",
# dont show all the things inside here
blackbox_level=1,
children=[
read_abort, read_leak, read_depth, read_alt, read_detection,
read_gps, set_utm_from_gps, update_tf, update_latlon,
neptus_tree
])
def const_tree(auv_config):
"""
construct the entire tree.
the structure of the code reflects the structure of the tree itself.
sub-trees are constructed in inner functions.
auv_config is in scope of all these inner functions.
auv_config is a simple data object with a bunch of UPPERCASE fields in it.
"""
# slightly hacky way to keep track of 'runnable' actions
# such actions should add their names to this list in their init
# just for Neptus vehicle state for now
bb = pt.blackboard.Blackboard()
bb.set(bb_enums.MANEUVER_ACTIONS, [])
# just for clarity when looking at the bb in the field
bb.set(bb_enums.MISSION_FINALIZED, False)
def const_data_ingestion_tree():
read_abort = ptr.subscribers.EventToBlackboard(
name="A_ReadAbort",
topic_name=auv_config.ABORT_TOPIC,
variable_name=bb_enums.ABORT)
read_alt = ReadTopic(
name="A_ReadAlt",
topic_name=auv_config.ALTITUDE_TOPIC,
topic_type=DVL,
blackboard_variables={bb_enums.ALTITUDE: 'altitude'})
read_leak = ReadTopic(name="A_ReadLeak",
topic_name=auv_config.LEAK_TOPIC,
topic_type=Leak,
blackboard_variables={bb_enums.LEAK: 'value'})
read_detection = ReadTopic(
name="A_ReadCameraDetection",
topic_name=auv_config.CAMERA_DETECTION_TOPIC,
topic_type=PointStamped,
blackboard_variables={bb_enums.POI_POINT_STAMPED:
None} # read the entire message into the bb
)
read_latlon = ReadTopic(name="A_ReadLatlon",
topic_name=auv_config.LATLON_TOPIC,
topic_type=GeoPoint,
blackboard_variables={
bb_enums.CURRENT_LATITUDE: 'latitude',
bb_enums.CURRENT_LONGITUDE: 'longitude'
})
def const_neptus_tree():
update_neptus = Sequence(
name="SQ-UpdateNeptus",
children=[
A_UpdateNeptusEstimatedState(
auv_config.ESTIMATED_STATE_TOPIC),
A_UpdateNeptusPlanControlState(
auv_config.PLAN_CONTROL_STATE_TOPIC),
A_UpdateNeptusVehicleState(auv_config.VEHICLE_STATE_TOPIC),
A_UpdateNeptusPlanDB(auv_config.PLANDB_TOPIC,
auv_config.UTM_LINK,
auv_config.LOCAL_LINK,
auv_config.LATLONTOUTM_SERVICE),
A_UpdateNeptusPlanControl(auv_config.PLAN_CONTROL_TOPIC),
A_VizPublishPlan(auv_config.PLAN_VIZ_TOPIC)
])
return update_neptus
update_tf = A_UpdateTF(auv_config.UTM_LINK, auv_config.BASE_LINK)
neptus_tree = const_neptus_tree()
publish_heartbeat = A_SimplePublisher(topic=auv_config.HEARTBEAT_TOPIC,
message_object=Empty())
return Sequence(
name="SQ-DataIngestion",
# dont show all the things inside here
blackbox_level=1,
children=[
read_abort, read_leak, read_alt, read_detection, read_latlon,
update_tf, neptus_tree, publish_heartbeat
])
def const_dvl_tree():
switch_on = Sequence(name="SQ_SwitchOnDVL",
children=[
C_AtDVLDepth(auv_config.DVL_RUNNING_DEPTH),
A_SetDVLRunning(
auv_config.START_STOP_DVL_NAMESPACE, True,
auv_config.DVL_COOLDOWN)
])
switch_off = Fallback(name="FB_SwitchOffDVL",
children=[
switch_on,
A_SetDVLRunning(
auv_config.START_STOP_DVL_NAMESPACE,
False, auv_config.DVL_COOLDOWN)
])
return switch_off
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)
])
def const_leader_follower():
return Sequence(
name="SQ_LeaderFollower",
children=[
C_LeaderFollowerEnabled(config.ENABLE_LEADER_FOLLOWER),
C_LeaderExists(config.BASE_LINK, config.LEADER_LINK),
C_LeaderIsFarEnough(config.BASE_LINK, config.LEADER_LINK,
config.MIN_DISTANCE_TO_LEADER),
A_FollowLeader(config.FOLLOW_ACTION_NAMESPACE,
config.LEADER_LINK)
])
def const_autonomous_updates():
poi_tree = Fallback(name="FB_Poi",
children=[
C_NoNewPOIDetected(common_globals.POI_DIST),
A_UpdateMissonForPOI(
auv_config.UTM_LINK, auv_config.LOCAL_LINK,
auv_config.POI_DETECTOR_LINK,
auv_config.LATLONTOUTM_SERVICE)
])
return Fallback(name="FB_AutonomousUpdates",
children=[C_AutonomyDisabled(), poi_tree])
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_finalize_mission():
publish_complete = A_SimplePublisher(
topic=auv_config.MISSION_COMPLETE_TOPIC, message_object=Empty())
set_finalized = pt.blackboard.SetBlackboardVariable(
variable_name=bb_enums.MISSION_FINALIZED,
variable_value=True,
name='A_SetMissionFinalized')
return Sequence(name="SQ-FinalizeMission",
children=[
C_HaveCoarseMission(),
C_StartPlanReceived(),
C_PlanIsNotChanged(),
C_PlanCompleted(), publish_complete, set_finalized
])
# The root of the tree is here
# planned_mission = Sequence(name="SQ_PlannedMission",
# children=[
# const_synch_tree(),
# XXX stuff in here are not modified to work with utm-frame-everything
# they _could_ but not tested.
# const_autonomous_updates(),
# const_execute_mission_tree()
# ])
planned_mission = const_execute_mission_tree()
# use this to kind of set the tree to 'idle' mode that wont attempt
# to control anything and just chills as an observer
finalized = CheckBlackboardVariableValue(bb_enums.MISSION_FINALIZED, True,
"MissionFinalized")
run_tree = Fallback(
name="FB-Run",
children=[
finalized,
const_finalize_mission(), planned_mission
# const_leader_follower()
])
root = Sequence(
name='SQ-ROOT',
children=[
const_data_ingestion_tree(),
const_safety_tree(),
# const_dvl_tree(),
run_tree
])
return ptr.trees.BehaviourTree(root)