unit4_pp/scripts/unit4_exercise_solution.py
def rrt(initial_position, target_position, width, height, map, map_resolution, map_origin, tree_viz):
  ''' 
  Performs Rapidly-Exploring Random Trees (RRT) algorithm on a costmap with a given start and goal node
  '''
  # Create the root node
  root_node = Node(initial_position)
  # A list to keep all nodes in the tree
  nodes = [root_node]
  # Iteration counter
  interations = 0
  # Iterations limit
  max_iterations = 10000
  # Incremental distance (for simplicity value is expressed in grid cells)
  max_branch_lenght = 20
  # Tolerance margin allowed around target position (value in grid cells)
  goal_tolerance = 20
  # A list to hold the output path from start to goal
  path = []

  while True:
    # Increment the number of iterations
    interations += 1
    # Check if we have exeeded max iterations
    if interations > max_iterations:
      rospy.logwarn("RRT: Max iterations exceeded")
      return path
    # Generate a new random point anywhere in the map
    random_point = [rand(width), rand(height)]

    # Find the closest node
    closest_node = find_closest_node(random_point, nodes)

    # Create a new point at the max. branch distance towards the random point
    candidate_point = create_new_branch_point(closest_node.coordinates, random_point, max_branch_lenght)

    # Verifiy that the new branch is collision free
    if not collision_detected(closest_node.coordinates, candidate_point, map, width):
      # Create a new instance of a Node object and add it to the tree
      latest_node = Node(candidate_point, closest_node)
      nodes.append(latest_node)

      # Optional: Visualize tree graph in Rviz
      tree_viz.append(latest_node)

      # Check if the goal has been reached
      if test_goal(latest_node.coordinates, target_position, goal_tolerance):
        rospy.loginfo('RRT: Goal reached')
        break

  rospy.loginfo('RRT: Path search ended')

  # Reconstruct path by working backwards from target
  path.append(target_position)
  node = latest_node
  while node.parent:
      path.append(node.coordinates)
      node = node.parent
  path.append(node.coordinates)
  # Reverse list
  path.reverse()
  rospy.loginfo('RRT: Done reconstructing path')

  return path
 
  Performs Rapidly-Exploring Random Trees (RRT) algorithm on a costmap with a given start and goal node
  
Performs Rapidly-Exploring Random Trees (RRT) algorithm on a costmap with a given start and goal node
Performs Rapidly - Exploring Random Trees ( RRT ) algorithm on a costmap with a given start and goal node
def rrt(initial_position, target_position, width, height, map, map_resolution, map_origin, tree_viz):
  root_node = Node(initial_position)
  nodes = [root_node]
  interations = 0
  max_iterations = 10000
  max_branch_lenght = 20
  goal_tolerance = 20
  path = []
  while True:
    interations += 1
    if interations > max_iterations:
      rospy.logwarn("RRT: Max iterations exceeded")
      return path
    random_point = [rand(width), rand(height)]
    closest_node = find_closest_node(random_point, nodes)
    candidate_point = create_new_branch_point(closest_node.coordinates, random_point, max_branch_lenght)
    if not collision_detected(closest_node.coordinates, candidate_point, map, width):
      latest_node = Node(candidate_point, closest_node)
      nodes.append(latest_node)
      tree_viz.append(latest_node)
      if test_goal(latest_node.coordinates, target_position, goal_tolerance):
        rospy.loginfo('RRT: Goal reached')
        break
  rospy.loginfo('RRT: Path search ended')
  path.append(target_position)
  node = latest_node
  while node.parent:
      path.append(node.coordinates)
      node = node.parent
  path.append(node.coordinates)
  path.reverse()
  rospy.loginfo('RRT: Done reconstructing path')
  return path
def rrt ( initial_position , target_position , width , height , map , map_resolution , map_origin , tree_viz ) : root_node = Node ( initial_position ) nodes = [ root_node ] interations = 0 max_iterations = 10000 max_branch_lenght = 20 goal_tolerance = 20 path = [ ] while True : interations += 1 if interations > max_iterations : rospy . logwarn ( "RRT: Max iterations exceeded" ) return path random_point = [ rand ( width ) , rand ( height ) ] closest_node = find_closest_node ( random_point , nodes ) candidate_point = create_new_branch_point ( closest_node . coordinates , random_point , max_branch_lenght ) if not collision_detected ( closest_node . coordinates , candidate_point , map , width ) : latest_node = Node ( candidate_point , closest_node ) nodes . append ( latest_node ) tree_viz . append ( latest_node ) if test_goal ( latest_node . coordinates , target_position , goal_tolerance ) : rospy . loginfo ( 'RRT: Goal reached' ) break rospy . loginfo ( 'RRT: Path search ended' ) path . append ( target_position ) node = latest_node while node . parent : path . append ( node . coordinates ) node = node . parent path . append ( node . coordinates ) path . reverse ( ) rospy . loginfo ( 'RRT: Done reconstructing path' ) return path
Performs Rapidly-Exploring Random Trees (RRT) algorithm on a costmap with a given start and goal node
Performs Rapidly - Exploring Random Trees ( RRT ) algorithm on a costmap with a given start and goal node
''' 
  Performs Rapidly-Exploring Random Trees (RRT) algorithm on a costmap with a given start and goal node
  ''' # Create the root node # A list to keep all nodes in the tree # Iteration counter # Iterations limit # Incremental distance (for simplicity value is expressed in grid cells) # Tolerance margin allowed around target position (value in grid cells) # A list to hold the output path from start to goal # Increment the number of iterations # Check if we have exeeded max iterations # Generate a new random point anywhere in the map # Find the closest node # Create a new point at the max. branch distance towards the random point # Verifiy that the new branch is collision free # Create a new instance of a Node object and add it to the tree # Optional: Visualize tree graph in Rviz # Check if the goal has been reached # Reconstruct path by working backwards from target # Reverse list
initial_position
target_position
width
height
map
map_resolution
map_origin
tree_viz