Engineering/marl4.tl

mission marl {
  """
  This mission is a test to deploy MARL in the Gazebo sim for multi-agent target traccking using range-only.
  """

  arguments {
    # Mission-wide settings

    MissionTimeout = 6 hour
      """
      Maximum duration of mission
      """

    NeedCommsTime = 40 minute
      """
      Elapsed time after previous surface communications when vehicle will
      begin to ascend for additional surface communications
      """

    # You probably need to change these.

    ContactLabelOne = 8 count
      """
      The acoustic address of the first asset to be pinged
      tethys = 6
      brizo = 11
      daphne = 8
      galene = 9
      makai = 5
      pontus = 10
      triton = 12
      tiny = 20
      """
    ContactLabelTwo = 0 count
      """
      The acoustic address of the second asset to be pinged
      set to 0 if only one target is to be pinged
      """
    SendDataLabel = 10 count
      """
      The acoustic address of the asset to send observation data 255 broadcast (it doesn't work,says it is not mapped), 10 is pontus
      This is the default value, but it will be overwritten by bash file duirng simulation launch.
      set to 0 if no data sending is desired
      """

    SendDataStrPing = 1 count
      """
      If 0 = we will use the new string funciton to send data over DAT modem (slower but we can receive more data such as range to target from other vehicle)
      If 1 = we will use the old pinging method to send data (faster but we can only receive the position of the other vehicle)
      """

    ContactDepth = NaN meter
      """
      The asset's depth to be tracked
      """

    LatHeading = 36.775 degree
      """
      The latitude of the center of the circle. Fill this in to specify a
      start position, it will be overwritten once the vehicle receives an
      acoustic signal from the contact.
      """

    LonHeading = -121.865 degree
      """
      The longitude of the center of the circle. Fill this in to specify a
      start position, it will be overwritten once the vehicle receives an
      acoustic signal from the contact.
      """

    CenterNorthingsDelta = 0 meter
      """
      Desired change in latitude to achieve, expressed as a distance.
      """

    CenterEastingsDelta = 0 meter
      """
      Desired change in longitude to achieve, expressed as a distance.
      """

    CircleRadius = 50 meter
      """
      Radius of circle around acoustElapsedSinceLastSendDataic contact during wait time
      """

    SpeedUpdateTimeout = 4 hour
      """
      Max time allowed between speed command updates.
      """

    StateUpdateDelay = 10 second
      """
      Minimum time delay between state updates to avoid rapid changes.
      """

    HorizontalCommandMode = 0 count
      """
      Mission horizontal command mode. 0 = Heading command (default). 1 = Rudder angle command. 2 = Waypoint command
      """

    AcousticContactTimeout = 4 hour
      """
      If the vehicle does not receive an acoustic signal for more than this
      length of time, it will surface for communications with shore.
      """

    SendObsUpdatePeriod = 480 second
      """
      Time to wait after an a-comms data has been sent before sending new data.
      """

    PingRangeUpdatePeriod = 120 second
      """
      How long to wait between acoustic queries
      """

    # parameters for constant depth trajectory
    Depth = 20 meter
      """
      Depth of flat and level flight during the mission.
      """

    MaxDepth = 60 meter
      """
      Maximum depth for the entire mission.
      """

    MinAltitude = 5 meter
      """
      Minimum height above the sea floor for the entire mission.
      """

    MassHold = true
      """
      Set to True in order to hold mass at default position, False to allow
      mass to run on its own.
      """

    BuoyancyHold = true
      """
      Set to True in order to hold buoyancy at neutral position, False to
      allow buoyancy to run on its own.
      """

    #  You probably do not need to change these.


    NumberOfPings = 1 count
      """
      Number of return pings to request with each acoustic query (more than 1
      will activate oneway mode)
      """

    CircleMaxError = 10 meter
      """
      If this distance away from the circle, drive straight towards (or away
      from the center). Otherwise, try to reduce distance from the ideal
      circle.
      """

    CircleTurnToPort = false
      """
      If true, vehicle turns to the left around the center point. If false,
      vehicle turns to the right.
      """

    Speed = 0 meter_per_second
      """
      Vehicle speed.
      """

    #MyTempVar = "init" #only for old lrauv-sim applications
    MyTempVar = NaN none_str
       """
      A custom variable  from the backseat driver interface.
      """

    NewSendObservations = 0 count
      """
      A custom variable from the backseat driver interface to indicate new data to send.
      0 = no new data
      1 = new data available
      """

    TrackingUpdatePeriod = 180 second
    """
    Internal variable to set how long to wait between updating internal tracking variables
    """

    ContactLabel = 8 count
    """
    This is the default value, but it will be overwritten by bash file duirng simulation launch, and by ContactLabelOne duirng mission.
    """

    # You are even less likely to need to change these.

    KwpHeading = 0.010 radian_per_meter
      """
      Used to relax waypoint cross-track error constant that is adjusted for
      docking. (You can override this setting by passing an argument.)
      """

    MinOffshore = 1.5 kilometer
      """
      Minimum offshore distance for the entire mission.
      """

    auxNewSendObservations = 0 count
      """
      Auxiliary variable to help with new send observations flag elapsed time
      """

  }

  output {
    PingEndOne = false
      """
      Mission variable (don't change). Flag to count elapsed time after pinging target one.
      """
    PingEndTwo = false
      """
      Mission variable (don't change). Flag to count elapsed time after pinging target two.
      """
    PingEndThree = false
      """
      Mission variable (don't change). Flag to count elapsed time after pinging target three.
      """
    HeadingMode = 0 count
      """
      Heading angle command mode enumaration.
      """

    RudderMode = 1 count
      """
      Rudder angle command mode enumaration.
      """

    WpMode = 2 count
      """
      Waypoint command mode enumaration.
      """

    Heading = 0 degree
      """
      Commanded vehicle heading. Mission will init Heading to current vehicle heading.
      """

    RudderAngle = 0 degree
      """
      Commanded vehicle RudderAngle. Only acitve when mission is in rudder commands mode.
      """

    ElapsedSinceLastSendData = 0 second
      """
      Time elapsed since last Send Data fix.
      """
    ElapsedSinceLastPing = 0 second
      """
      Time elapsed since last Ping fix.
      """
    CurrentContactRange = 0 meter
      """
      Current range to acoustic contact.
      """
    ElapsedSincePingEndTwo = 0 second
      """
      Time elapsed since pinging target two ended.
      """
    ElapsedSincePingEndOne = 0 second
      """
      Time elapsed since pinging target one ended.
      """
    ElapsedSincePingEndThree = 0 second
      """
      Time elapsed since pinging target three ended.
      """
    ContactLatitude = 0 degree
      """
      The latitude of the contact being tracked.
      """ 
    ContactLongitude = 0 degree
      """ 
      The longitude of the contact being tracked.
      """
  }

  # Missions should always start with a timeout.

  timeout duration=MissionTimeout

  # Add tracking directive high in the stack (above NeedComms to enable tracking from surface).

  insert Insert/AbortDrift.tl {
    redefineArg AcousticTimeout = AcousticContactTimeout
  }

  behavior Estimation:TrackAcousticContact {
    run in parallel

    set contactDepthSetting = ContactDepth
    set contactLabelSetting = ContactLabel
    set numberOfSamplesSetting = NumberOfPings
    set updatePeriodSetting = TrackingUpdatePeriod
  }

  ##This is a hack to make sure the scratchpad var is created with the correct underlying DataType
  behavior Guidance:Execute {
    run in sequence

    set command = "set _.othersObservations string \"initialvalue\""
  }

  aggregate SendObservationData {
    run when ( called )

    syslog info "Got new data to send : " + customUri "_.send_observations"~none_str 
              + ". Old temp var is " + MyTempVar~none_str + ". NewSendObservations flag is " + NewSendObservations~count

    assign in sequence ElapsedSinceLastSendData = elapsed ( MyTempVar )
    assign in sequence MyTempVar = customUri "_.send_observations"


    aggregate CehckNewData {
      run in sequence

      break if ( NewSendObservations != 1 count)

      behavior Sensor:SendDirect {
        run in sequence

        set destType = "modem"
        set destId = SendDataLabel
        set destName = "_.othersObservations"
        set value = MyTempVar
        set unit = "none_str"
      }

      syslog info "SendData: sent send_observations : " + MyTempVar~none_str
          + " to nearby vehicle on address: " + SendDataLabel~count
          + ". Elapsed time " + ElapsedSinceLastSendData~minute

      syslog info "SYNC STUF: Waiting to transmit next observation data: " + SendObsUpdatePeriod~second + " seconds."

      behavior Guidance:Wait {
        run in sequence

        set duration = SendObsUpdatePeriod
        }
    }
    #reset the new data flag
    assign in sequence NewSendObservations = 0 count
  }

  ##new from December 2025
  aggregate UpdateInternalVarialbe {
    run when (
      elapsed ( customUri "_.contactLabelToLcm" ) > TrackingUpdatePeriod )

    #Variable used to send contactLabel to backseat app (using LCM)
    assign in sequence customUri "_.contactLabelToLcm" = ContactLabel
    assign in sequence customUri "_.ContactLabelOneToLcm" = ContactLabelOne
    assign in sequence customUri "_.ContactLabelTwoToLcm" = ContactLabelTwo
    assign in sequence customUri "_.SendDataLabelToLcm" = SendDataLabel
    assign in sequence customUri "_.SendDataStrPingToLcm" = SendDataStrPing 


    syslog info "Updating contactLabelToLcm " + customUri "_.contactLabelToLcm"~count + ", ContactLabelOneToLcm " + customUri "_.ContactLabelOneToLcm"~count
      + ", ContactLabelTwoToLcm " + customUri "_.ContactLabelTwoToLcm"~count  
  }

  aggregate UpdateNewSendObservations {
    run when (
      elapsed ( auxNewSendObservations ) >= TrackingUpdatePeriod )

    #Variable used to notify the mission handler that new data is available to send
    assign in sequence NewSendObservations = customUri "_.new_send_observations"
    assign in sequence auxNewSendObservations = 1 count

    syslog info "Updating new_send_observation flag " + NewSendObservations~count
  }

  ##continuing with old stuff

  # aggregate UpdateContactLabel {
  #   run when (
  #     elapsed ( customUri "_.contactLabelSetting" ) < ( elapsed ( ContactLabel ) )
  #   )

  #   assign in sequence ContactLabel = abs ( customUri "_.contactLabelSetting" )

  #   syslog info "Upadating ContactLabel " + customUri "_.contactLabelSetting"~count
  # }

  # Most missions have the NeedComms aggregate at high priority.

  insert id="NeedComms" Insert/NeedComms.tl

  assign in sequence NeedComms:DiveInterval = NeedCommsTime

  # Missions should almost always start with standard safety envelopes; most missions should not expose the parameters of these envelopes.

  insert Insert/StandardEnvelopes.tl

  assign in sequence StandardEnvelopes:MinAltitude = MinAltitude

  assign in sequence StandardEnvelopes:MaxDepth = MaxDepth

  assign in sequence StandardEnvelopes:MinOffshore = MinOffshore

  # Add guidance update directives high in the stack.

  aggregate SpeedTimeout {
    run when ( elapsed ( Speed ) >= SpeedUpdateTimeout )

    assign in sequence Speed = 0 meter_per_second

    syslog fault "Speed timeout reached. Stopping vehicle (speed " + Speed~meter_per_second + ")."
  }

  aggregate UpdateSpeed {
    run when (
      elapsed ( customUri "_.speedCmd" ) < ( elapsed ( Speed ) )
      and ( elapsed ( Speed ) > StateUpdateDelay )
    )

    assign in sequence Speed = customUri "_.speedCmd"

    #syslog info "Updating vehicle speed " + Speed~meter_per_second
  }

  aggregate UpdateCommandMode {
    run when (
      elapsed ( customUri "_.horizontalCmdMode" ) < ( elapsed ( HorizontalCommandMode ) )
      and (
        customUri "_.horizontalCmdMode" == HeadingMode
        or ( customUri "_.horizontalCmdMode" == RudderMode )
        or ( customUri "_.horizontalCmdMode" == WpMode )
      )
      and ( elapsed ( HorizontalCommandMode ) > StateUpdateDelay )
    )

    assign in sequence HorizontalCommandMode = customUri "_.horizontalCmdMode"

    #syslog info "Updating horizontal command mode " + HorizontalCommandMode~count
  }

  aggregate UpdateHeading {
    run when (
      elapsed ( customUri "_.headingCmd" ) < ( elapsed ( Heading ) )
      and ( elapsed ( Heading ) > StateUpdateDelay )
    )

    assign in sequence Heading = customUri "_.headingCmd"

    syslog info "Updating heading " + Heading~degree
  }

  aggregate UpdateRudder {
    run when (
      elapsed ( customUri "_.rudderAngleCmd" ) < ( elapsed ( RudderAngle ) )
      and ( elapsed ( RudderAngle ) > StateUpdateDelay )
    )

    assign in sequence RudderAngle = customUri "_.rudderAngleCmd"

    syslog info "Updating rudder angle " + RudderAngle~degree
  }

  aggregate UpdateLat {
    run when (
      elapsed ( customUri "_.wpLatCmd" ) < ( elapsed ( LatHeading ) )
      and ( elapsed ( LatHeading ) > StateUpdateDelay )
    )

    break if ( isNaN ( customUri "_.wpLatCmd" ) )

    assign in sequence LatHeading = customUri "_.wpLatCmd"

    syslog info "Updated lrauv heading lat location to " + LatHeading~degree
  }

  aggregate UpdateLon {
    run when (
      elapsed ( customUri "_.wpLonCmd" ) < ( elapsed ( LonHeading ) )
      and ( elapsed ( LonHeading ) > StateUpdateDelay )
    )

    break if ( isNaN ( customUri "_.wpLonCmd" ) )

    assign in sequence LonHeading = customUri "_.wpLonCmd"

    syslog info "Updated lrauv heading lon location to " + LonHeading~degree
  }

  insert id="Science" Insert/Science.tl

  # Power the Backseat payload.

  insert Insert/BackseatDriver.tl

  assign in sequence BackseatDriver:EnableBackseat = true

  insert Insert/PowerOnly.tl

  # Check for additional instructions first.
  call id="StartingMission" priorityHere=false refId="NeedComms"

  # Start mission to syncronize acoustics
  aggregate PingTargetOne{
    """
    Start pinging the first target
    """
    run when ( called )

    syslog info "SYNC STUF: 2. Starting PingTargetOne mission."

    aggregate SetTargetLabel {
      run in sequence

      syslog info "SYNC STUF: Setting ping range to TargetOne: " + ContactLabelOne~count 

      assign in sequence ContactLabel = ContactLabelOne

    }
    syslog info "SYNC STUF: Setting completed TargetOne " +  ContactLabelOne~count
  }

  aggregate PingTargetTwo{
    """
    Start pinging the second target
    """
    run when ( called )

    syslog info "SYNC STUF: 2.b Starting PingTargetTwo mission."

    aggregate SetTargetLabel {
      run in sequence

      syslog info "SYNC STUF: Setting ping range to TargetTwo: " + ContactLabelTwo~count 

      assign in sequence ContactLabel = ContactLabelTwo

    }
    syslog info "SYNC STUF: Setting completed TargetTwo " +  ContactLabelTwo~count
  }

  aggregate PingAgentOne{
    """
    Start pinging the second target
    """
    run when ( called )

    syslog info "SYNC STUF: 2.c Starting PingAgentOne mission."

    aggregate SetTargetLabel {
      run in sequence

      syslog info "SYNC STUF: Setting ping range to AgentOne: " + SendDataLabel ~count 

      assign in sequence ContactLabel = SendDataLabel

    }
    syslog info "SYNC STUF: Setting completed AgentOne " +  SendDataLabel~count
  }

  aggregate InsertWait {
  run when ( called )

    syslog info "SYNC STUF: Current range to contact is " + Estimation:TrackAcousticContact.range_to_contact~m
        + ". CurrentContactRange is " + CurrentContactRange~m

    aggregate CheckRange {
      run in sequence repeat=5

      break if (  
          Estimation:TrackAcousticContact.range_to_contact != CurrentContactRange
          and Estimation:TrackAcousticContact.range_to_contact != NaN meter
        )

      syslog info "SYNC STUF: Waiting 30 seconds for rangings."

      behavior Guidance:Wait {
        run in sequence

        set duration = 30 second
      }
      syslog info "SYNC STUF: Current range to contact is " + Estimation:TrackAcousticContact.range_to_contact~m
        + ". CurrentContactRange is " + CurrentContactRange~m
    }
  }

  aggregate JoystickControl {
    run in sequence

    behavior Guidance:Buoyancy {
      run in parallel

      set position = Control:VerticalControl.buoyancyNeutral
    }

    behavior Guidance:Pitch {
      run while ( MassHold )

      set massPosition = Control:VerticalControl.massDefault
    }

    behavior Guidance:Pitch {
      run in parallel

      set depth = Depth
    }

    behavior Guidance:SetSpeed {
      run in parallel

      set speed = Speed
    }

    aggregate CmdHeading {
      run while ( HorizontalCommandMode == HeadingMode )

      behavior Guidance:Point {
        run in parallel

        set forceUpdate = true
        set heading = Heading
      }
    }

    aggregate CmdRudder {
      run while ( HorizontalCommandMode == RudderMode )

      behavior Guidance:Point {
        run in parallel

        set forceUpdate = true
        set rudderAngle = RudderAngle
      }
    }

    aggregate CmdWp {
      run while (
        ( HorizontalCommandMode == WpMode )
        and ( not isNaN ( LatHeading ) )
        and ( not isNaN ( LonHeading ) )
      )

      behavior Guidance:Waypoint {
        #This needs to be run in sequence, in parallel is not working!
        run in sequence

        set latitude = LatHeading
        set longitude = LonHeading
      }
    }

    aggregate SyncAcousticsLoop {
      run in parallel

      #1- ranging target 1
      aggregate PingLoop1 {
        run in sequence

        syslog info "SYNC STUF: A. Starting PingLoop1 aggregate."
        assign in sequence TrackingUpdatePeriod = PingRangeUpdatePeriod

        call id="StartPingOnTargetOne" priorityHere=false refId="PingTargetOne"
        assign in sequence CurrentContactRange = Estimation:TrackAcousticContact.range_to_contact
        call id="WaitForRangeOnTargetOne" priorityHere=false refId="InsertWait"
        assign in sequence CurrentContactRange = Estimation:TrackAcousticContact.range_to_contact
        assign in sequence ElapsedSincePingEndOne = elapsed ( PingEndOne )
        syslog info "SYNC STUF: Finished pinging Target at address " +  ContactLabelOne~count
              + ". Target distance " + CurrentContactRange~m
              + ". Elapsed time " + ElapsedSincePingEndOne~minute
        assign in sequence PingEndOne = true
      }

      #2- ranging target 2
      aggregate PingLoop2 {
        run in sequence 

        break if ( ContactLabelTwo == 0 count )

        syslog info "SYNC STUF: B. Starting PingLoop2 aggregate."
        assign in sequence TrackingUpdatePeriod = PingRangeUpdatePeriod

        #insert a wait before pinging the second target
        syslog important "SYNC STUF: Wait before firing sample"
        behavior Guidance:Wait {
          run in sequence
          set duration = 15 second
        }

        call id="StartPingOnTargetTwo" priorityHere=false refId="PingTargetTwo"
        assign in sequence CurrentContactRange = Estimation:TrackAcousticContact.range_to_contact
        call id="WaitForRangeOnTargetTwo" priorityHere=false refId="InsertWait"
        assign in sequence CurrentContactRange = Estimation:TrackAcousticContact.range_to_contact
        assign in sequence ElapsedSincePingEndTwo = elapsed ( PingEndTwo )
        syslog info "SYNC STUF: Finished pinging Target at address " +  ContactLabelTwo~count
              + ". Target distance " + CurrentContactRange~m
              + ". Elapsed time " + ElapsedSincePingEndTwo~minute
        assign in sequence PingEndTwo = true
      }

      #3- sending observation data using string function
      aggregate SendDataLoop {
        run in sequence

        break if ( SendDataLabel == 0 count or SendDataStrPing == 1 count )  

        syslog info "SYNC STUF: C. Starting SendDataLoop, sending string to other LRAUVs."  
        #increase the ping period to allow for data sending    
        assign in sequence TrackingUpdatePeriod = SendObsUpdatePeriod + PingRangeUpdatePeriod

        #insert a wait before starting over
        syslog important "SYNC STUF: Wait before starting over"
        behavior Guidance:Wait {
          run in sequence
          set duration = 15 second
        }

        #sending observation data
        call id="SendObservationDataCmd" priorityHere=false refId="SendObservationData"
      }

      #4- measuring other vehicle position using pinging, insead of the string funciton
      aggregate PingLoop3 {
        run in sequence 

        break if ( SendDataLabel == 0 count or SendDataStrPing == 0 count  )

        syslog info "SYNC STUF: D. Starting PingLoop3 to know the position of other agents aggregate."
        assign in sequence TrackingUpdatePeriod = PingRangeUpdatePeriod

        #2- insert a wait before pinging the second target
        syslog important "SYNC STUF: Wait before firing sample"
        behavior Guidance:Wait {
          run in sequence
          set duration = 15 second
        }

        #3- ranging agent 1
        call id="StartPingOnAgentOne" priorityHere=false refId="PingAgentOne"
        assign in sequence CurrentContactRange = Estimation:TrackAcousticContact.range_to_contact
        call id="WaitForRangeOnTargetTwo" priorityHere=false refId="InsertWait"
        assign in sequence CurrentContactRange = Estimation:TrackAcousticContact.range_to_contact
        assign in sequence ElapsedSincePingEndThree = elapsed ( PingEndThree )
        assign in sequence ContactLatitude = Estimation:TrackAcousticContact.contact_latitude
        assign in sequence ContactLongitude = Estimation:TrackAcousticContact.contact_longitude
        assign in sequence customUri "_.AgentContactLatitude" = ContactLatitude
        assign in sequence customUri "_.AgentContactLongitude" = ContactLongitude
        syslog info "SYNC STUF: Finished pinging Agent at address " +  SendDataLabel~count
              + ". Target distance " + CurrentContactRange~m
              + ". Agent latitude " + ContactLatitude~degree
              + ". Agent longitude " + ContactLongitude~degree
              + ". Elapsed time " + ElapsedSincePingEndThree~minute
        assign in sequence PingEndThree = true
      }

    }
  }
}