How to display texts on the cells of visual tools

Under the course “Path Planning Basics>unit2 and unit 3”, the program changes the colour of the cells to yellow and orange to differentiate between visited and to be discovered cells.


The line of code used was:
grid_viz.set_color(current_node,“pale yellow”)

I want to know how to display data/text onto that cell whose color was changed so that I can better visualize why that cell was chosen. (attaching the rest of the code)
Thanks in advance

#! /usr/bin/env python


Greedy Best-First Search path planning algorithm exercise

Author: Roberto Zegers R.

Copyright: Copyright (c) 2020, Roberto Zegers R.

License: BSD-3-Clause

Date: Nov 30, 2020

Usage: roslaunch unit3 unit3_gbfs_exercise.launch


import rospy

from math import sqrt

def manhattan_distance(a, b):

  return (abs(a[0] - b[0]) + abs(a[1] - b[1]))

def euclidean_distance(a, b):

    # implement the body of this function

  return sqrt((a[0]-b[0])**2+(a[1]-b[1])**2)

def find_neighbors(index, width, height, costmap, orthogonal_step_cost):


  Identifies neighbor nodes inspecting the 8 adjacent neighbors

  Checks if neighbor is inside the map boundaries and if is not an obstacle according to a threshold

  Returns a list with valid neighbour nodes as [index, step_cost] pairs


  neighbors = []

  # length of diagonal = length of one side by the square root of 2 (1.41421)

  diagonal_step_cost = orthogonal_step_cost * 1.41421

  # threshold value used to reject neighbor nodes as they are considered as obstacles [1-254]

  lethal_cost = 1

  upper = index - width

  if upper > 0:

    if costmap[upper] < lethal_cost:

      step_cost = orthogonal_step_cost + costmap[upper]/255

      neighbors.append([upper, step_cost])

  left = index - 1

  if left % width > 0:

    if costmap[left] < lethal_cost:

      step_cost = orthogonal_step_cost + costmap[left]/255

      neighbors.append([left, step_cost])

  upper_left = index - width - 1

  if upper_left > 0 and upper_left % width > 0:

    if costmap[upper_left] < lethal_cost:

      step_cost = diagonal_step_cost + costmap[upper_left]/255

      neighbors.append([index - width - 1, step_cost])

  upper_right = index - width + 1

  if upper_right > 0 and (upper_right) % width != (width - 1):

    if costmap[upper_right] < lethal_cost:

      step_cost = diagonal_step_cost + costmap[upper_right]/255

      neighbors.append([upper_right, step_cost])

  right = index + 1

  if right % width != (width + 1):

    if costmap[right] < lethal_cost:

      step_cost = orthogonal_step_cost + costmap[right]/255

      neighbors.append([right, step_cost])

  lower_left = index + width - 1

  if lower_left < height * width and lower_left % width != 0:

    if costmap[lower_left] < lethal_cost:

      step_cost = diagonal_step_cost + costmap[lower_left]/255

      neighbors.append([lower_left, step_cost])

  lower = index + width

  if lower <= height * width:

    if costmap[lower] < lethal_cost:

      step_cost = orthogonal_step_cost + costmap[lower]/255

      neighbors.append([lower, step_cost])

  lower_right = index + width + 1

  if (lower_right) <= height * width and lower_right % width != (width - 1):

    if costmap[lower_right] < lethal_cost:

      step_cost = diagonal_step_cost + costmap[lower_right]/255

      neighbors.append([lower_right, step_cost])

  return neighbors

def indexToWorld(flatmap_index, map_width, map_resolution, map_origin = [0,0]):


    Converts a flatmap index value to world coordinates (meters)

    flatmap_index: a linear index value, specifying a cell/pixel in an 1-D array

    map_width: number of columns in the occupancy grid

    map_resolution: side lenght of each grid map cell in meters

    map_origin: the x,y position in grid cell coordinates of the world's coordinate origin

    Returns a list containing x,y coordinates in the world frame of reference


    # convert to x,y grid cell/pixel coordinates

    grid_cell_map_x = flatmap_index % map_width

    grid_cell_map_y = flatmap_index // map_width

    # convert to world coordinates

    x = map_resolution * grid_cell_map_x + map_origin[0]

    y = map_resolution * grid_cell_map_y + map_origin[1]

    return [x,y]

def greedy_bfs(start_index, goal_index, width, height, costmap, resolution, origin, grid_viz = None):


  Performs Greedy Best-First Search on a costmap with a given start and goal node


  # create an open_list

  open_list = []

  # set to hold already processed nodes

  closed_list = set()

  # dict for mapping children to parent

  parents = dict()


  # dict for mapping h costs (heuristic costs) to nodes

  h_costs = dict()

  # determine the h cost (heuristic cost) for the start node

  from_xy = indexToWorld(start_index, width, resolution, origin)

  to_xy = indexToWorld(goal_index, width, resolution, origin)

  h_cost = euclidean_distance(from_xy, to_xy)



  # set the start's node h_cost

  h_costs[start_index] = h_cost

  # add start node to open list

  open_list.append([start_index, h_cost])

  shortest_path = []

  path_found = False

  rospy.loginfo('Greedy BFS: Done with initialization')

  # Main loop, executes as long as there are still nodes inside open_list

  while open_list:

    # sort open_list according to the lowest 'g_cost' value (second element of each sublist)

    open_list.sort(key = lambda x: x[1]) 

    # extract the first element (the one with the lowest 'g_cost' value)

    current_node = open_list.pop(0)[0]

    # Close current_node to prevent from visting it again


    # Optional: visualize closed nodes

    grid_viz.set_color(current_node,"pale yellow")

    # If current_node is the goal, exit the main loop

    if current_node == goal_index:

      path_found = True


    # Get neighbors of current_node

    neighbors = find_neighbors(current_node, width, height, costmap, resolution)

    # Loop neighbors

    for neighbor_index, step_cost in neighbors:

      # Check if the neighbor has already been visited

      if neighbor_index in closed_list:


      # pure heuristic 'h_cost'

      from_xy = indexToWorld(neighbor_index, width, resolution, origin)

      to_xy = indexToWorld(goal_index, width, resolution, origin)

      h_cost = euclidean_distance(from_xy, to_xy)

      #h_cost = manhattan_distance(from_xy, to_xy)

    #   print("h_cost=",h_cost)


      # Check if the neighbor is in open_list

      in_open_list = False

      for idx, element in enumerate(open_list):

        if element[0] == neighbor_index:

          in_open_list = True


      # CASE 1: neighbor already in open_list

      if in_open_list:

        if h_cost < h_costs[neighbor_index]:

          # Update the node's heuristic cost

          h_costs[neighbor_index] = h_cost

          parents[neighbor_index] = current_node

          # Update the node's h_cost inside open_list

          open_list[idx] = [neighbor_index, h_cost]

      # CASE 2: neighbor not in open_list


        # Set the node's heuristic cost

        h_costs[neighbor_index] = h_cost

        parents[neighbor_index] = current_node

        # Add neighbor to open_list

        open_list.append([neighbor_index, h_cost])

        # Optional: visualize frontier


  rospy.loginfo('Greedy BFS: Done traversing nodes in open_list')

  if not path_found:

    rospy.logwarn('Greedy BFS: No path found!')

    return shortest_path

  # Reconstruct path by working backwards from target

  if path_found:

      node = goal_index


      while node != start_index:


          # get next node

          node = parents[node]

  # reverse list

  shortest_path = shortest_path[::-1]

  rospy.loginfo('Greedy BFS: Done reconstructing path')

  return shortest_path

This topic was automatically closed 10 days after the last reply. New replies are no longer allowed.