def aStar_client(goal):
rospy.wait_for_service('aStar')
try:
aStar = rospy.ServiceProxy('aStar', AStar)
resp = aStar(goal.x, goal.y)
waypoints = []
for n in range(len(resp.pathX)):
p = Point()
p.x = resp.pathX[n]
p.y = resp.pathY[n]
waypoints.append(p)
#print "we got da points! " + str(waypoints)
return waypoints
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def aStar_client(goal):
rospy.wait_for_service('aStar')
try:
aStar = rospy.ServiceProxy('aStar', AStar)
resp = aStar(goal.x, goal.y)
waypoints = []
for n in range(len(resp.pathX)):
p = Point()
p.x = resp.pathX[n]
p.y = resp.pathY[n]
waypoints.append(p)
#print "we got da points! " + str(waypoints)
print "Received waypoints: " + str(waypoints)
return waypoints
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def readMap(msg):
global mapInfo #map information such as width and hight, and cell sizes.
global mapData #the cells of the map, with 100 = impassable and 0 = empty, -1 = unexplored.
global pub_map
global mapProcessed
global odom_list
global path
#mapData = msg.data
#mapInfo = msg.info
(tran, rot) = odom_list.lookupTransform('map', 'base_footprint',
rospy.Time(0))
startPoint = Point()
startPoint.x = tran[0]
startPoint.y = tran[1]
#print "resizing new map"
resizedMap = mapResize(0.2, msg.info, msg.data)
mapInfo = resizedMap.info
mapData = resizedMap.data
#print "expanding new map."
mapData = obstacleExpansion(1, mapInfo, mapData)
resizedMap.data = mapData
print "publishing new map"
pub_map.publish(resizedMap)
#print "plan a new path."
paths = aStar(globalToGrid(startPoint, mapInfo), goal, mapInfo, mapData,
pub_frontier, pub_expanded)
publishGridList(paths[0], mapInfo, pub_path)
print "path in readmap"
path = paths[0]
mapProcessed = 1
def readMap(msg):
global mapInfo #map information such as width and hight, and cell sizes.
global mapData #the cells of the map, with 100 = impassable and 0 = empty, -1 = unexplored.
global pub_map
global mapProcessed
global odom_list
global path
#mapData = msg.data
#mapInfo = msg.info
(tran, rot) = odom_list.lookupTransform('map','base_footprint', rospy.Time(0))
startPoint = Point()
startPoint.x = tran[0]
startPoint.y = tran[1]
#print "resizing new map"
resizedMap = mapResize(0.2, msg.info, msg.data)
mapInfo = resizedMap.info
mapData = resizedMap.data
#print "expanding new map."
mapData = obstacleExpansion(1, mapInfo, mapData)
resizedMap.data = mapData
print "publishing new map"
pub_map.publish(resizedMap)
#print "plan a new path."
paths = aStar(globalToGrid(startPoint, mapInfo), goal, mapInfo, mapData, pub_frontier, pub_expanded)
publishGridList(paths[0], mapInfo, pub_path)
print "path in readmap"
path = paths[0]
mapProcessed = 1
# Use this command to make the program wait for some seconds
rospy.sleep(rospy.Duration(1, 0))
print "Starting pathfinder"
#print out our debug map, startting by makeing a list of all of the wall locations
#pubMap(pub_path, mapInfo, mapData)
lastGoal = (-1, -1)
lastStart = (-1, -1)
newMap = obstacleExpansion(1, mapInfo, mapData, pub_waypoints)
pubMap(pub_waypoints, mapInfo, newMap)
r = rospy.Rate(10)
while not rospy.is_shutdown():
if (goal is not lastGoal) or (start is not lastStart):
lastStart = start
lastGoal = goal
paths = aStar(start, goal, mapInfo, mapData, pub_frontier,
pub_expanded)
publishGridList(paths[0], mapInfo, pub_path)
#publishGridList(paths[1], mapInfo, pub_waypoints)
r.sleep()
print "exiting pathfinder"
#go back up the stack and print all of the points where there is a 100 onto the map. See if the shape is rotated.
#we will need to make all of those functions anyway.
def getNextmove(movingNode, nodeGrid):
goalNode = getGoal(movingNode,grid,goalGrid)
nextSpot = aStar(movingNode, goalNode, newgrid)
return nextSpot
cityB = ""
print("Veuillez choisir une ville d'arrivée: ")
print(listName)
cityB = input()
#getting heuristic
tempHeuristic = ""
while tempHeuristic not in dicHeur.keys():
cls()
print("Choisissez une heuristique parmis : ")
for k in dicHeur.keys():
print(k + "=>" + dicHeur[k])
tempHeuristic = input()
#search
path, steps = aStar(dicCity, dicCity[cityA], dicCity[cityB],
listHeuris[int(tempHeuristic)])
#posttreatment operations
parent = path.parent
listCity = []
listCity.append(str(dicCity[cityB]))
while parent:
listCity.append(str(parent))
parent = parent.parent
listCity.reverse()
#displaying
cls()
print("Trajet recherché : " + cityA + "->" + cityB)
print("trajet trouvé en : " + str(steps) + " visites.")
print("Bilan du Trajet : ")
rospy.sleep(rospy.Duration(1, 0))
print "Starting pathfinder"
#print out our debug map, startting by makeing a list of all of the wall locations
#pubMap(pub_path, mapInfo, mapData)
lastGoal = (-1,-1)
lastStart = (-1,-1)
newMap = obstacleExpansion(1, mapInfo, mapData,pub_waypoints)
pubMap(pub_waypoints, mapInfo, newMap)
r = rospy.Rate(10)
while not rospy.is_shutdown():
if (goal is not lastGoal) or (start is not lastStart):
lastStart = start
lastGoal = goal
paths = aStar(start, goal, mapInfo, mapData, pub_frontier, pub_expanded)
publishGridList(paths[0], mapInfo, pub_path)
#publishGridList(paths[1], mapInfo, pub_waypoints)
r.sleep()
print "exiting pathfinder"
#go back up the stack and print all of the points where there is a 100 onto the map. See if the shape is rotated.
#we will need to make all of those functions anyway.
def getNextmove(movingNode, nodeGrid):
goalNode = getGoal(movingNode, grid, goalGrid)
nextSpot = aStar(movingNode, goalNode, newgrid)
return nextSpot
def __init__(self):
global action
# Initialize the ShowBase class from which we inherit, which will
# create a window and set up everything we need for rendering into it.
action = "start"
ShowBase.__init__(self)
winProps = WindowProperties()
winProps.setTitle("LASO Simulation - (RLP 2019-2020)")
base.win.requestProperties(winProps)
if not base.win.getGsg().getSupportsBasicShaders():
print(
"Error: Video driver reports that depth textures are not supported."
)
return
if not base.win.getGsg().getSupportsDepthTexture():
print(
"Error: Video driver reports that depth textures are not supported."
)
return
self.accept("escape", finalitzar) # Escape quits
# Disable default mouse-based camera control. This is a method on the
# ShowBase class from which we inherit.
#self.disableMouse()
# Place the camera
camera.setPosHpr(0, 0, 80, 0, -90, 0)
base.trackball.node().set_pos(0, 200, 0)
base.trackball.node().set_hpr(0, 60, 0)
#camera.setPosHpr(0, 0, 25, 0, -90, 0)
#base.camLens.setNearFar(200, 600)
#self.enableMouse()
# Load the maze and place it in the scene
#self.maze = loader.loadModel("models/maze")
self.skybox = loader.loadModel("models/skybox")
self.skybox.reparentTo(render)
self.skybox.setPosHpr(0, 0, 45 + MAZE_OFFSETS[CURR_MAZE], 0, -180, 0)
#self.skybox.setScale(2.54)
self.taula = loader.loadModel("models/taula")
self.taula.reparentTo(render)
self.taula.setPosHpr(0, 0, -53.6 + MAZE_OFFSETS[CURR_MAZE], 0, 0, 0)
self.taula.setScale(1.8)
self.laso_box = loader.loadModel("models/laso_box")
self.laso_box.reparentTo(render)
self.laso_box.setPosHpr(0, 0, MAZE_OFFSETS[CURR_MAZE], 0, 0, 0)
self.laso_ax = loader.loadModel("models/laso_ax")
self.laso_ax.reparentTo(self.laso_box)
self.laso_ax.setPosHpr(0, 0, MAZE_HEIGHT, 0, 0, 0)
self.maze_i = CURR_MAZE
self.maze = loader.loadModel("models/" + MAZES_NAME[self.maze_i])
self.maze.reparentTo(render)
self.maze.setPosHpr(0, 0, MAZE_HEIGHT, 0, 0, 0)
#self.maze.setScale(2.54)
# Load custom maze
#self.maze2 = loader.loadModel("models/lab4")
#self.maze2.reparentTo(render)
# Most times, you want collisions to be tested against invisible geometry
# rather than every polygon. This is because testing against every polygon
# in the scene is usually too slow. You can have simplified or approximate
# geometry for the solids and still get good results.
#
# Sometimes you'll want to create and position your own collision solids in
# code, but it's often easier to have them built automatically. This can be
# done by adding special tags into an egg file. Check maze.egg and ball.egg
# and look for lines starting with <Collide>. The part is brackets tells
# Panda exactly what to do. Polyset means to use the polygons in that group
# as solids, while Sphere tells panda to make a collision sphere around them
# Keep means to keep the polygons in the group as visable geometry (good
# for the ball, not for the triggers), and descend means to make sure that
# the settings are applied to any subgroups.
#
# Once we have the collision tags in the models, we can get to them using
# NodePath's find command
# Find the collision node named wall_collide
#self.walls = self.maze.find("**/wall_collide")
self.walls = self.maze.find("**/wall_col")
# Collision objects are sorted using BitMasks. BitMasks are ordinary numbers
# with extra methods for working with them as binary bits. Every collision
# solid has both a from mask and an into mask. Before Panda tests two
# objects, it checks to make sure that the from and into collision masks
# have at least one bit in common. That way things that shouldn't interact
# won't. Normal model nodes have collision masks as well. By default they
# are set to bit 20. If you want to collide against actual visable polygons,
# set a from collide mask to include bit 20
#
# For this example, we will make everything we want the ball to collide with
# include bit 0
self.walls.node().setIntoCollideMask(BitMask32.bit(0))
# CollisionNodes are usually invisible but can be shown. Uncomment the next
# line to see the collision walls
#self.walls.show() # Show wall colliders
# We will now find the triggers for the holes and set their masks to 0 as
# well. We also set their names to make them easier to identify during
# collisions
self.loseTriggers = []
#for i in range(6):
for i in range(3):
#trigger = self.maze.find("**/hole_collide" + str(i))
trigger = self.maze.find("**/hole" + str(i + 1) + "_col")
trigger.node().setIntoCollideMask(BitMask32.bit(0))
trigger.node().setName("loseTrigger")
self.loseTriggers.append(trigger)
# Uncomment this line to see the triggers
#trigger.show() # Show lose triggers colliders
# Ground_collide is a single polygon on the same plane as the ground in the
# maze. We will use a ray to collide with it so that we will know exactly
# what height to put the ball at every frame. Since this is not something
# that we want the ball itself to collide with, it has a different
# bitmask.
#self.mazeGround = self.maze.find("**/ground_collide")
self.mazeGround = self.maze.find("**/ground_col")
self.mazeGround.node().setIntoCollideMask(BitMask32.bit(1))
# Load the ball and attach it to the scene
# It is on a root dummy node so that we can rotate the ball itself without
# rotating the ray that will be attached to it
self.ballRoot = render.attachNewNode("ballRoot")
self.ball = loader.loadModel("models/bball")
self.ball.reparentTo(self.ballRoot)
# Find the collison sphere for the ball which was created in the egg file
# Notice that it has a from collision mask of bit 0, and an into collison
# mask of no bits. This means that the ball can only cause collisions, not
# be collided into
self.ballSphere = self.ball.find("**/ball")
self.ballSphere.node().setFromCollideMask(BitMask32.bit(0))
self.ballSphere.node().setIntoCollideMask(BitMask32.allOff())
# No we create a ray to start above the ball and cast down. This is to
# Determine the height the ball should be at and the angle the floor is
# tilting. We could have used the sphere around the ball itself, but it
# would not be as reliable
self.ballGroundRay = CollisionRay() # Create the ray
self.ballGroundRay.setOrigin(0, 0, 10) # Set its origin
self.ballGroundRay.setDirection(0, 0, -1) # And its direction
# Collision solids go in CollisionNode
# Create and name the node
self.ballGroundCol = CollisionNode('groundRay')
self.ballGroundCol.addSolid(self.ballGroundRay) # Add the ray
self.ballGroundCol.setFromCollideMask(
BitMask32.bit(1)) # Set its bitmasks
self.ballGroundCol.setIntoCollideMask(BitMask32.allOff())
# Attach the node to the ballRoot so that the ray is relative to the ball
# (it will always be 10 feet over the ball and point down)
self.ballGroundColNp = self.ballRoot.attachNewNode(self.ballGroundCol)
# Uncomment this line to see the ray
#self.ballGroundColNp.show() # Show ball collision ray
# Finally, we create a CollisionTraverser. CollisionTraversers are what
# do the job of walking the scene graph and calculating collisions.
# For a traverser to actually do collisions, you need to call
# traverser.traverse() on a part of the scene. Fortunately, ShowBase
# has a task that does this for the entire scene once a frame. By
# assigning it to self.cTrav, we designate that this is the one that
# it should call traverse() on each frame.
self.cTrav = CollisionTraverser()
# Collision traversers tell collision handlers about collisions, and then
# the handler decides what to do with the information. We are using a
# CollisionHandlerQueue, which simply creates a list of all of the
# collisions in a given pass. There are more sophisticated handlers like
# one that sends events and another that tries to keep collided objects
# apart, but the results are often better with a simple queue
self.cHandler = CollisionHandlerQueue()
# Now we add the collision nodes that can create a collision to the
# traverser. The traverser will compare these to all others nodes in the
# scene. There is a limit of 32 CollisionNodes per traverser
# We add the collider, and the handler to use as a pair
self.cTrav.addCollider(self.ballSphere, self.cHandler)
self.cTrav.addCollider(self.ballGroundColNp, self.cHandler)
# Collision traversers have a built in tool to help visualize collisions.
# Uncomment the next line to see it.
#self.cTrav.showCollisions(render) # Show traveser collisions
# This section deals with lighting for the ball. Only the ball was lit
# because the maze has static lighting pregenerated by the modeler
self.alightColor = 0.1
ambientLight = AmbientLight("ambientLight")
ambientLight.setColor(
(self.alightColor, self.alightColor, self.alightColor, 1))
#ambientLight.setColor((1, 0, 0, 1))
self.ambientL = render.attachNewNode(ambientLight)
render.setLight(self.ambientL)
directionalLight = DirectionalLight("directionalLight")
directionalLight.setDirection(LVector3(0, 1, 0))
#directionalLight.setColor((0.375, 0.375, 0.375, 1))
directionalLight.setColor((1, 1, 1, 1))
directionalLight.setSpecularColor((1, 1, 1, 1))
#self.directL = render.attachNewNode(directionalLight)
#render.setLight(self.directL)
"""
self.teapot = loader.loadModel('teapot')
self.teapot.reparentTo(render)
self.teapot.setPos(0, 0, 0)
self.teapotMovement = self.teapot.hprInterval(50, LPoint3(0, 360, 360))
self.teapotMovement.loop()
"""
self.lightColor = 1000
self.light = render.attachNewNode(Spotlight("Spot"))
self.light.node().setScene(render)
self.light.node().setShadowCaster(True, 1024, 1024)
self.light.node().setAttenuation((1, 0, 1))
#self.light.node().showFrustum()
self.light.node().getLens().setFov(48)
self.light.node().getLens().setNearFar(5, 500)
#self.light.node().setColor((100000, 100000, 100000, 1))
self.light.node().setColor(
(self.lightColor, self.lightColor, self.lightColor, 1))
self.light.setPos(0, 0, 100)
self.light.setHpr(LVector3(0, -90, 0))
render.setLight(self.light)
plight = PointLight('plight')
plight.setColor((0.8, 0.8, 0.8, 1))
plnp = render.attachNewNode(plight)
plnp.setPos(0, 0, 80)
render.setLight(plnp)
render.setShaderAuto()
#self.maze2.setPos(0, 0, 10)
#self.ballRoot.setLight(render.attachNewNode(ambientLight))
#self.ballRoot.setLight(render.attachNewNode(directionalLight))
# Maze 2 light
#self.maze2.setLight(self.light)
#self.maze2.setLight(self.ambientL)
#self.maze2.hide()
#self.maze.hide()
# This section deals with adding a specular highlight to the ball to make
# it look shiny. Normally, this is specified in the .egg file.
m = Material()
m.setSpecular((1, 1, 1, 1))
m.setShininess(96)
self.ball.setMaterial(m, 1)
#self.maze2.setMaterial(m,1)
# Set maze rotation speed
self.mazeSpeed = 10
self.mazeVoiceSpeed = 8
# Set maze max rotation
self.mazeMaxRotation = 10
self.mazeVoiceMaxRotation = 10
# Distància minima per passar al següent punt
self.minDist = 25
# Pas per saltar punts del path
self.pas = 25
base.setBackgroundColor(0.2, 0.2, 0.2)
self.camera2_buffer = base.win.makeTextureBuffer("c2buffer",
PI_CAMERA_RES,
PI_CAMERA_RES,
to_ram=True)
#mytexture.write("text1.png")
# print(mytexture)
# cv2.imshow("Texure1", mytexture)
#self.camera2_buffer.setSort(-100)
self.camera2 = base.makeCamera(self.camera2_buffer)
self.camera2.reparentTo(render)
self.camera2.setPosHpr(0, 0, PI_CAMERA_H, 0, -90, 0)
self.camera2.node().getLens().setFov(PI_CAMERA_FOV)
self.camera2.node().getLens().setFilmSize(1, 1)
self.digitizer = Digitizer()
self.aStar = aStar()
self.path = None
self.ready_to_solve = False
# Finally, we call start for more initialization
self.start()