def loadMapNRobot(self,mpath,rradius=0.35,gridres=0.10,idist=0.5,maxdist=1):
self.robotradius=rradius
self.vstp=vstpPY.VSTP()
self.gridres=gridres
self.idealdist=idist
self.maxdist=maxdist
self.vstp.init(rradius,gridres,idist,maxdist)
self.mapsegs = self.vstp.loadMap(mpath)
self.computeMapLimits()
self.raciox=(self.displayx-5)/(self.mapmaxx-self.mapminx)
self.racioy=(self.displayy-5)/(self.mapmaxy-self.mapminy)
def vstpFunc(self,iniX,iniY,goalx,goaly): #coordenadas de partida (inicio) print "odomx: %f odomy: %f" % (iniX,iniY) #coordenadas de destino (fim) print "goalx: %f goaly: %f" % (goaly,goaly) v=vstpPY.VSTP() #criacao do objeto v.init(self.robotRadius,self.gridResolution,self.idealDist,self.maxDist) self.mapsegs = v.loadMap(MAP) self.computeMapLimits(); self.traj_points =v.planTrajectory(iniX,iniY,goalx,goaly,True) #chamada a criacao do thread do mapa #self.toThread() self.trajDivider()
def __init__(self,
robotRadius=0.35,
gridResolution=0.1,
idealDist=0.5,
maxDist=1):
'''
self.robotRadius=robotRadius
self.gridResolution=gridResolution
self.idealDist=idealDist
self.maxDist=maxDist
'''
self.new_traj = list()
self.aux_traj = Point()
#self.odom_sub = rospy.Subscriber('/turtle1/pose', Pose, self.callbackOdom)
#self.danger_sub = rospy.Subscriber('/sonarFlag', Bool, self.callbackSonar)
#para o segway
#self.odom_sub = rospy.Subscriber('/segway_rmp_node/odom', Odometry, self.callbackOdom)
self.odom_sub = rospy.Subscriber('/odomUpdater', Pose2D,
self.callbackFalseOdom)
self.new_odom_sub = rospy.Subscriber('/new_odom', Pose2D,
self.callbackOdom)
#definicao do servico para receber obstaculos no caminho
self.s = rospy.Service('add_obstacle', add_obstacle,
self.def_add_obstacle)
self.v = vstpPY.VSTP() #criacao do objeto
self.v.init(robotRadius, gridResolution, idealDist, maxDist)
self.mapsegs = self.v.loadMap(MAP)
self.pose = Pose2D()
self.trueodomX = 0
self.trueodomY = 0
self.trueodomTheta = 0
self.falseodomX = 0
self.falseodomY = 0
self.falseodomTheta = 0
self.state = 0
self.traj_points = []