STP-Trick/Trick_Python.py at main · SimDynamX/STP-Trick · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
# THIS COMMENT LINE SHOULD BE THE FIRST LINE OF THE FILE
# DON'T CHANGE ANY OF THE BELOW; NECESSARY FOR JOINING SIMULATION
import os, sys, time, datetime, traceback
import spaceteams as st
def custom_exception_handler(exctype, value, tb):
    error_message = "".join(traceback.format_exception(exctype, value, tb))
    st.logger_fatal(error_message)
    exit(1)
sys.excepthook = custom_exception_handler
st.connect_to_sim(sys.argv)
import numpy as np
# DON'T CHANGE ANY OF THE ABOVE; NECESSARY FOR JOINING SIMULATION
################################################################
from variable_server import VariableServer  # NEEDED for Trick Sim Connections

# Set up references to entities and frames
this_sys = st.GetThisSystem()
canyon : st.Entity = this_sys.GetParam(st.VarType.entityRef, "CanyonRim")
canyon_fixed = canyon.GetBodyFixedFrame()

# Do whatever, now.
st.logger_info("Hello world!")

trick_entity = st.get_entities()[0] # optain the first entity in the sim

trick_entity.setLocation(st.frames.FramedLoc([0,0,0], canyon_fixed)) # NEEDED for setting initial location

# Hard coded connection to variable server (Port 7000)
variable_server = VariableServer('localhost', 7000)

# Check if a simulation was found
if variable_server is None:
    st.logger_error("No simulation found")
else:
    st.logger_info("Found simulation: " + str(variable_server))

    # Pings the simulation for data
    while True:
        # Sync Sim Clock to Trick Sim Time
        # sim_time = variable_server.get_value("dyn.cannon.time", units="s")
        # st.SimGlobals.SimClock.ResetTo(sim_time) # TODO make this work

        # Get Cannon Position
        # valu = variable_server.get_value("dyn.cannon.pos", units="m")
        # x, y = map(float, valu.split(','))
        # trick_entity.setLocation(st.frames.FramedLoc([x,0,y], canyon_fixed))

        # st.OnScreenLogMessage("dyn.cannon.pos: " + "(" + str(x) + ", " + str(y) + ")", "trick_python", st.Severity.Info)

        # Get Satellite Position
        valu = variable_server.get_value("dyn.satellite.pos", units="m")
        x, y, z = map(float, valu.split(','))
        x -= 6867500
        trick_entity.setLocation(st.frames.FramedLoc([x,y,z], canyon_fixed))

        # Satellite Velocity
        vel_value = variable_server.get_value("dyn.satellite.vel", units="m/s")
        vx, vy, vz = map(float, vel_value.split(','))

        st.OnScreenLogMessage("[Pos, Vel]: " + "(" + str(x) + " m" + ", " + str(y) + " m" + ", " + str(z) + " m" + " )" + ", " + "(" + str(vx) + " m/s" + ", " + str(vy) + " m/s" + ", " + str(vz) + " m/s" + " )", "trick_python", st.Severity.Info)

variable_server.close()
st.logger_info("connection closed")

st.leave_sim()