def updatepos(rightmsg, leftmsg):
global entraxe, X, Y, theta, coordpub, wheelRadius, Nticks, wheelDiameter
Vl = np.pi * wheelDiameter * (rightmsg.data / Nticks) # mm Dr
Vr = np.pi * wheelDiameter * (leftmsg.data / Nticks) # mm Dl
V = (Vr + Vl) / 2.0 # mm DC
thetadot = (Vr - Vl) / entraxe # rad
Xdot = (np.sin(thetadot) * np.cos(theta) +
np.sin(theta) * np.cos(thetadot) - np.sin(theta))
Ydot = (np.cos(theta) - np.cos(thetadot) * np.cos(theta) +
np.sin(theta) * np.sin(thetadot))
# Xdot = np.cos(theta+thetadot/2.0)
# Ydot = np.sin(theta+thetadot/2.0)
rospy.loginfo(str(Xdot) + " | " + str(Ydot))
Xdot *= (V) * 1000
Ydot *= (V) * 1000
theta -= thetadot # rad
X += Xdot
Y += Ydot
pload = Coordinates()
pload.x = X
pload.y = Y
pload.theta = w(theta)
pload.xdot = Xdot
pload.ydot = Ydot
pload.thetadot = thetadot
coordpub.publish(pload)
def updatepos(rightmsg, leftmsg):
global entraxe, X, Y, theta, coordpub, wheelRadius, Nticks, wheelDiameter
Vl = np.pi * wheelDiameter * (rightmsg.data / Nticks) # mm Dr
Vr = np.pi * wheelDiameter * (leftmsg.data / Nticks) # mm Dl
V = (Vr + Vl) / 2.0 # mm DC
thetadot = (Vr - Vl) / entraxe # rad
Xdot = np.cos(theta) * V
Ydot = np.sin(theta) * V
theta -= thetadot # rad
X += Xdot
Y += Ydot
# rospy.loginfo("| "+str(X)+" "+str(Y)+" "+str(theta)+" |")
pload = Coordinates()
pload.x = X
pload.y = Y
pload.theta = w(theta)
pload.xdot = Xdot
pload.ydot = Ydot
pload.thetadot = thetadot
coordpub.publish(pload)
def signal_handler(signal, frame):
rospy.signal_shutdown("manual stop") #gracefully shutdown
sys.exit(0)
def w(angle):
alpha = np.arctan2(np.sin(angle), np.cos(angle))
alpha = ((np.pi + alpha) % 2 * np.pi) - np.pi
return alpha
if __name__ == "__main__":
signal.signal(signal.SIGINT, signal_handler)
X = 1000.0 #mm
Y = 1500.0 #mm
theta = 0.0 #? rad not sure
rospy.init_node("rospy_tracker", anonymous=False)
coordpub = rospy.Publisher("/coords", Coordinates, queue_size=1)
rospy.loginfo("> tracker succesfully initialised")
themsg = Coordinates()
themsg.x = X
themsg.y = Y
themsg.ydot = 0
themsg.xdot = 0
themsg.thetadot = 0
while True:
time.sleep(0.2)
theta += 0.01
themsg.theta = w(theta)
coordpub.publish(themsg)
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)