def is_valid(self, grid_map):
"""
Return true if the location on the map is valid, ie, in obstacle free zone
"""
goal_pixel = world_to_pixel((self.x, self.y), (700, 2000))
if grid_map[goal_pixel[0]][goal_pixel[1]]:
return True
return False
def laser_callback(self, msg):
rospy.loginfo("Laser CallBack")
self.pf.filter(sensor_msg=msg, twist=self.odem_msg.twist)
self.wanderer.islocalized = self.pf.isLocalized
particles = self.pf.particle_set
particle_points = []
for p in particles:
x, y = transformations.world_to_pixel(
(p.x, p.y), (self.MAPHEIGHT, self.MAPWIDTH))
particle_points.append([x, y])
if self.pf.isLocalized:
start = Node(self.pf.centroid[0], self.pf.centroid[1],
self.pf.centroid[2])
end = Node(self.goal[0], self.goal[1], self.goal[2])
self.pathplanner = astar.PathPlanner(start, pi, end)
self.pathplanner.plan()
self.map_update(particle_points)
def is_move_valid(self, grid_map, move): """ Return true if required move is legal """ goal = self.apply_move(move) # convert goal coordinates to pixel coordinates before checking this goal_pixel = world_to_pixel((goal.x, goal.y), (700, 2000)) # check if too close to the walls if goal_pixel[0] >= SAFETY_OFFSET and not grid_map[goal_pixel[0]-SAFETY_OFFSET][goal_pixel[1]]: return False if goal_pixel[1] >= SAFETY_OFFSET and not grid_map[goal_pixel[0]][goal_pixel[1]-SAFETY_OFFSET]: return False if goal_pixel[0] >= SAFETY_OFFSET and goal_pixel[1] >= SAFETY_OFFSET and not grid_map[goal_pixel[0]-SAFETY_OFFSET][goal_pixel[1]-SAFETY_OFFSET]: return False if grid_map[goal_pixel[0]][goal_pixel[1]]: return True return False
def check_within_map(self, world_x, world_y, image_size_pixel):
pixel_point = transformations.world_to_pixel([world_x, world_y],
image_size_pixel)
rospy.loginfo(pixel_point)
return self.helper.test_pixel_in_map(pixel_point[0], pixel_point[1])
def get_particle_laser_value(self, angle):
# convert particle world location to pixel location
pixel_points = transformations.world_to_pixel([self.x, self.y], self.map_helper.get_image_size_pixels())
pixel_theta = transformations.worldtheta_to_pixeltheta(self.theta)
return self.map_helper.cast_pixel_ray_on_map(pixel_points[0], pixel_points[1], pixel_theta)