Please help with python ROS code. Please implement the "timer" function according to what the comments are asking for using A $*$ algorithm:

#$!/$usr$/$bin$/$env python$3$

from grid import grid$2$D

import rospy

from nav$\_$msgs$.$msg import Odometry, OccupancyGrid, Path

from tf$.$transformations import euler$\_$from$\_$quaternion

from geometry$\_$msgs$.$msg import PoseStamped

import numpy as np

import pdb

from copy import copy

class planner$()$:

def $\_\_$init$\_\_($self$)$:

self.goal$=$None

self.current$\_$xyT$=$np$.$zeros$((3),$dtype$=$float$)$

# ROS Subscriber functions

self.odom$\_$sub $=$ rospy.Subscriber$("/$odom$",$ Odometry, self.odom$\_$cb$)$

self.map$\_$sub $=$ rospy.Subscriber$("/$map$",$ OccupancyGrid, self.map$\_$cb$)$

self.goal$\_$sub $=$ rospy.Subscriber$("/$move$\_$base$\_$simple$/$goal$",$ PoseStamped, self.goal$\_$cb$)$

self.timer$\_$sub $=$ rospy.Timer$($rospy$.$Duration$(5),$ self.timer$)$

# ROS publishers

self.path$\_$pub $=$ rospy.Publisher$("/$path$",$ Path, queue$\_$size$=5)$

self.target$\_$pub $=$ rospy.Publisher$("/$target$\_$xy$",$ PoseStamped, queue$\_$size$=5)$

# Map Cb $-$ build the grid

def map$\_$cb$($self$,$ map$\_$msg: OccupancyGrid$)$:

print$("$Map Received"$)$

self.map$=$grid$2$D$($map$\_$msg$)$

self.map$\_$sub.unregister$()$

# Retrieve the latest robot position

def odom$\_$cb$($self$,$ odom$\_$msg:Odometry$)$:

self.current$\_$xyT$[0]=$odom$\_$msg$.$pose.pose.position.x

self.current$\_$xyT$[1]=$odom$\_$msg$.$pose.pose.position.y

quat$=[$odom$\_$msg$.$pose.pose.orientation.x$,$

odom$\_$msg$.$pose.pose.orientation.y$,$

odom$\_$msg$.$pose.pose.orientation.z$,$

odom$\_$msg$.$pose.pose.orientation.w$]$

rpy$=$euler$\_$from$\_$quaternion$($quat$)$

self.current$\_$xyT$[2]=$rpy$[2]$

def goal$\_$cb$($self$,$ target: PoseStamped$)$:

# Build a plan to reach the target from the current robot position

self.goal$=$np$.$array$([$target$.$pose.position.x$,$ target.pose.position.y$])$

def publish$\_$path$($self$,$ path$\_$arr: np$.$ndarray$)$:

# path$\_$arr should be of format $[[$X$1,$ Y$1],[$X$2,$ Y$2],...,[$Xn$,$ Yn$]]$

pth$=$Path$()$

pth$.$header.frame$\_$id$="/$map$"$

pth$.$header.stamp$=$rospy.Time$()$

for idx in range$($path$\_$arr.shape$[0])$:

PS$=$PoseStamped$()$

PS$.$header$=$pth$.$header

PS$.$pose.position.x$=$path$\_$arr$[$idx$,0]$

PS$.$pose.position.y$=$path$\_$arr$[$idx$,1]$

PS$.$pose.orientation.w$=1\mathrm{.}0$

pth$.$poses.append$($PS$)$

self.path$\_$pub.publish$($pth$)$

def publish$\_$target$($self$,$ X$,$ Y$)$:

ps$=$PoseStamped$()$

ps$.$header.frame$\_$id$="/$map$"$

ps$.$header.stamp$=$rospy.Time$()$

ps$.$pose.position.x$=$X

ps$.$pose.position.y$=$Y

self.target$\_$pub.publish$($ps$)$

def timer$($self$,$ tmr$)$:

# You need to build a path consisting of a series of waypoints

# in XY space and send them one at a time to the behavioral

# controller

# A few things to consider:

# $1)$ What space can the robot actually reach? You have a map, but it

# represents space thought to contain obstacles

# $2)$ How finely do you sample the path? Do you need to send a new waypoint

# every $5$mm$?$ That's the default map grid resolution

# $3)$ We will add new waypoints via RViz interface so you do need to rebuild.

# How often you rebuild is up to you.

# Sample $-$ Create a path $($REPLACE$)$

path$=$np$.$array$([[1,0],[1,2],[-1,2],[-4,3],[-6,1]])$

# Sample $-$ Publish path and a target for the behavior$-$based controller

self.publish$\_$path$($path$)$

self.publish$\_$target$($path$[0,0],$path$[0,1])$

if $\_\_$name$\_\_==\text{'}\_\_$main$\_\_\text{'}$:

rospy.init$\_$node$("$planner$")$

pl$=$planner$()$

rospy.spin$()$