if __name__ == "__main__":
signal.signal(signal.SIGINT, signal_handler)
global X, Y, theta
X = 1500.0 # mm
Y = 1000.0 # mm
theta = 0.0 # ? rad not sure
global wheelDiameter, entraxe, Nticks, wheelRadius
Nticks = 1024.00 # ticks
entraxe = 275.00 # mm
wheelDiameter = 61.00 # mm
wheelRadius = wheelDiameter / 2.0 # mm
global coordpub, rightsub, leftsub
rospy.init_node("rospy_tracker", anonymous=False)
rightsub = Subscriber("/N2/reality", Int8)
leftsub = Subscriber("/N1/reality", Int8)
q = 3 # buffer queu size
deltaT = 0.005 # time interval for sync in s
coordpub = rospy.Publisher("/coords", Coordinates, queue_size=3)
rospy.loginfo("> tracker succesfully initialised")
while True:
pload = Coordinates()
pload.x = X
pload.y = Y
pload.theta = w(theta)
pload.xdot = 0
pload.ydot = 0
pload.thetadot = 0
coordpub.publish(pload)
time.sleep(0.2)
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from bot_coordinates.msg import Coordinates
from bot_coordinates.srv import TrajectoryGen, TrajectoryGenResponse
import math
import numpy as np
coord = Coordinates()
def handle_trajectory_generator(req):
global coord
targetcoords = np.array([req.x, req.y])
botcoords = np.array([coord.x, coord.y])
relcoords = targetcoords - botcoords
distance = np.linalg.norm(relcoords)
#distance = np.sqrt(math.pow((req.x - coord.x), 2) + math.pow((req.y - coord.y),2)) #√( (xb - xa)² + (yb - ya)² ) distance entre les anciennes coordonnées et la target en mm
#vecX = req.x - coord.x # vecteur du chemin a parcourir pour atteindre la target
#vecY = req.y - coord.y
u = np.array([0, 1])
u2 = relcoords / distance
theta = np.arccos(np.dot(u, u2))
#theta = np.arccos(vecY/(np.sqrt(math.pow(vecX,2) + math.pow(vecY,2)))) # angle du vecteur par rapport a l'axe Y : arccos(u.v/(||u||*||v||)) en rad
compensation = 1
if req.x < coord.x:
compensation = -1 # si la target est a "gauche" du point actuel l'angle doit etre négatif ou sinon faut le soustraire a 360
newtheta = compensation * theta
rospy.loginfo("theta : " + str(theta) + " | distance : " + str(distance))
return TrajectoryGenResponse(True)