from primutils import Primitive, read_primitives
from pioneer_hallway.srv import *
'''
used to keep track of current state
update these when we do forward projection
and/or when we look up the new state
'''
'''
Read the primitives from the file
store the change in velocity as tuples
for easy calculatioin when keeping track
of the current state
TODO: read these from the file given
remove the hardcoded values
'''
primitives = read_primitives("../../../doc/motionPrimitive/primitives.txt")
#simulation_flag = sys.argv[2]
projected_pose = (0.0, 0.0, 0.0, 0.0)
predicted_pose = (0.0, 0.0, 0.0)
vel = 0.0
goal_is_set = True
planner_finished = True
first_iteration = True
'''
t = obst.predictions[i].header.stamp.sec
x = obst.predictions[i].pose.pose.position.x
y = obst.predictions[i].pose.pose.position.y
cov = obst.predictions[i].pose.covariance
6x6 covariance matrix
(x, y, z, rotX, rotY, rotZ)
"""
Author: Jordan Ramsdell
Desc: Demonstrates how to use read_primitives and Primitive
Usage: python3 prim_demo.py primitives.txt
"""
from primutils import Primitive, read_primitives
import math
import sys
if __name__ == '__main__':
# Reads the primitives.txt file given as the first argument
# ... and returns a list of Primitive objects
primitives = read_primitives(sys.argv[1])
print(primitives)
p = primitives[0] # we're looking at the first primitive generated
# angular acceleration and linear acceleration are stored in each primitive
print("Angular / Linear Accel: {}/{}\n".format(p.wa, p.va))
# the divisors are also stored (used in generating keys)
print("Divisors: {}\n".format(p.config))
# entries is a dictionary shared between Primitives,
# where we store the precalculated applications of primitives
print("# of precalculated entries: {}\n".format(len(p.entries)))
x = 4
y = 4
linear_velocity = 0.3
# angular_velocity = math.pi / 5
heading = math.pi / 2