def Lidar_usings(laser_scan):
# TODO implement lidar and pathplanning here
global resultXY, lidar_pub, lidarsub, bot_coords
#rospy.loginfo("lidar hasn t been implemented yet")
ranges = np.array(laser_scan.ranges)
vectors_sum = np.array([0.0, 0.0])
#rospy.loginfo(len(laser_scan.ranges))
mid_angle = (len(laser_scan.ranges) / 2.0) * laser_scan.angle_increment
k = 1 #Coefficient de poids vectoriel
for i in range(len(laser_scan.ranges)):
if (laser_scan.ranges[i] >= 0.05):
angle = i * laser_scan.angle_increment - mid_angle
vectors_sum[0] -= (
1 / (float(laser_scan.ranges[i])**2)) * np.cos(angle)
vectors_sum[1] += (
1 / (float(laser_scan.ranges[i])**2)) * np.sin(angle)
k_mur = 25
vectors_sum[0] += k_mur / float((bot_coords[0] / 1000.0)**2)
vectors_sum[0] -= k_mur / float(((3000.0 - bot_coords[0]) / 1000.0)**2)
vectors_sum[1] += k_mur / float((bot_coords[1] / 1000.0)**2)
vectors_sum[1] -= k_mur / float(((2000.0 - bot_coords[1]) / 1000.0)**2)
vectors_sum *= k
resultXY = vectors_sum
z = Coordinates()
z.x = vectors_sum[0]
z.y = vectors_sum[1]
lidar_pub.publish(z)
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 Lidar_usings(laser_scan):
global resultXY, lidar_pub, lidarsub, bot_coords
#rospy.loginfo("lidar hasn t been implemented yet")
ranges = np.array(laser_scan.ranges)
vectors_sum = np.array([0.0,0.0])
#rospy.loginfo(len(laser_scan.ranges))
mid_angle = (len(laser_scan.ranges)/2.0)*laser_scan.angle_increment
k = 1 #Coefficient de poids vectoriel
for i in range(len(laser_scan.ranges)) :
if (laser_scan.ranges[i] >= 0.05 and (not np.isinf(laser_scan.ranges[i]))) :
angle = bot_coords[2] - laser_scan.angle_min - (laser_scan.angle_max - laser_scan.angle_min)*(float(i)/float(len(laser_scan.ranges)))
pos = np.array([np.cos(angle),np.sin(angle)])
pos*= laser_scan.ranges[i]
pos[0] += bot_coords[0]/1000.0
pos[1] += bot_coords[1]/1000.0
#rospy.loginfo(pos)
if(pos[0] >= 0 and pos[1] >= 0 and pos[0]<= 3.0 and pos[1]<= 2.0) :
vectors_sum[0] -= (1/(float(laser_scan.ranges[i])**2))*np.cos(angle)
vectors_sum[1] -= (1/(float(laser_scan.ranges[i])**2))*np.sin(angle)
#else :rospy.loginfo("AHAHAHAHAHH BOlOSS")
k_mur = 25
vectors_sum[0] += k_mur/float((bot_coords[0]/1000.0)**2)
vectors_sum[0] -= k_mur/float(((3000.0 - bot_coords[0])/1000.0)**2)
vectors_sum[1] += k_mur/float((bot_coords[1]/1000.0)**2)
vectors_sum[1] -= k_mur/float(((2000.0- bot_coords[1])/1000.0)**2)
vectors_sum *= k
resultXY = vectors_sum
z = Coordinates()
z.x = vectors_sum[0]
z.y = vectors_sum[1]
lidar_pub.publish(z)
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 lidarcallback(laser_scan):
# TODO implement lidar and pathplanning here
global resultXY, lidar_pub
#rospy.loginfo("lidar hasn t been implemented yet")
ranges = np.array(laser_scan.ranges)
vectors_sum = np.array([0.0,0.0])
mid_angle = (len(ranges)//2)*laser_scan.angle_increment
k = 0.005 #Coefficient de poids vectoriel
for i in range(len(ranges)) :
angle = i*laser_scan.angle_increment - mid_angle
vectors_sum[0] += (1/laser_scan.ranges[i]**2)*np.cos(angle) + XY[0]
vectors_sum[1] += (1/laser_scan.ranges[i]**2)*np.sin(angle) + XY[1]
vectors_sum *= k
resultXY = vectors_sum
z = Coordinates()
z.x = vectors_sum[0]
z.y = vectors_sum[1]
lidar_pub.publish(z)
rospy.loginfo("X = " + str(vectors_sum[0]) + " Y = " + str(vectors_sum[1]) + " ")
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)
#!/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):
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
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(newtheta) + " | distance : " +
str(distance))
return TrajectoryGenResponse(True)
def coordsub_callback(msg):
global coord
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)