Esempio n. 1
0
#!/usr/bin/env python

from tracker import PlanarTracker
from communication import NaslabNetwork
from actuate import ROS2DimActuate
from numpy import pi
from path_generator import PathGenerator


connection = NaslabNetwork()
act = ROS2DimActuate()
tracker = PlanarTracker(act.actuate, connection.getStates)

for i in range(10):
    tracker.goToPoint(1, 0)
    tracker.faceDirection(pi/2)
    path = PathGenerator(path_type='circle', speed=.3)
    tracker.followPath(path)

connection = LabNavigation()
act = ROS2DimActuate()
tracker = PlanarTracker(act.actuate, connection.getStates)
tracker.setID(0)
path = PathGenerator(path_type='two_lines', speed=.2)


sleep(2)

# tracker.goToPoint(0, -2)
#
# tracker.faceDirection(-pi/4)

start_location = path.getBeginning()
tracker.goToPoint(start_location[0]+.3, start_location[1])
sleep(1)
tracker.faceDirection(pi / 2)
sleep(1)
path.startNow()
_, x, y, _, _, _, _ = connection.getStates(0)
tracker.followPath(path, x - start_location[0], y - start_location[1])

# tracker.goToPoint(-2.5/10, -2.5)
# tracker.faceDirection(pi / 2)
# path.setVelocity = .3
# sleep(1)
# path.startNow()
# tracker.followTrajectory(path)