def __init__(self, world_map_init, occupancy_map_init, x_gt_init):
"""
Initialize starting pose and uncertainty parameters here.
Inputs:
world_map_init:
list of lists [x,y,theta,id], each representing
a landmark in the world map. Note that there is an ambiguity
between landmarks such that multiple landmarks may have the
same id number.
occupancy_map_init:
map of 1s and 0s, 1 = occupied and 0 = free
x_gt_init:
3x1 numpy array that represents the true starting
position of the robot
"""
self.done = False
self.x_gt = x_gt_init
self.v = 0
self.omega = 0
self.x_t = np.array([[0.0, 0.0, 0.0]]).T
self.dt = 1.0 / 60
self.THETA_MAX = np.pi / 2 # Angle at which we can see markers
self.particles = [] # Particles to plot - list of (x,y,theta,weight)
self.num_particles = 50
self.world_map = world_map_init
self.occupancy_map = occupancy_map_init
self.iteration = 0
self.graph = self.generate_graph(occupancy_map_init)
self.shortest_path = astar(self.graph, 0, len(self.graph.nodes) - 1)[0]
self.checkpoint = 0 # Index of point in shortest path that we are heading towards
self.CHECKPOINT_RADIUS = .1
self.init_particles()
def __init__(self,world_map_init,occupancy_map_init,x_gt_init):
"""
Initialize starting pose and uncertainty parameters here.
Inputs:
world_map_init:
list of lists [x,y,theta,id], each representing
a landmark in the world map. Note that there is an ambiguity
between landmarks such that multiple landmarks may have the
same id number.
occupancy_map_init:
map of 1s and 0s, 1 = occupied and 0 = free
x_gt_init:
3x1 numpy array that represents the true starting
position of the robot
"""
self.done=False
self.x_gt = x_gt_init
self.v = 0
self.omega = 0
self.x_t = np.array([[0.0,0.0,0.0]]).T
self.dt=1.0/60
self.THETA_MAX = np.pi/2 # Angle at which we can see markers
self.particles = [] # Particles to plot - list of (x,y,theta,weight)
self.num_particles = 50
self.world_map = world_map_init
self.occupancy_map = occupancy_map_init
self.iteration = 0
self.graph = self.generate_graph(occupancy_map_init)
self.shortest_path = astar(self.graph, 0, len(self.graph.nodes) - 1)[0]
self.checkpoint = 0 # Index of point in shortest path that we are heading towards
self.CHECKPOINT_RADIUS = .1
self.init_particles()
def __init__(self,world_map_init,occupancy_map_init,x_gt_init):
"""
Initialize starting pose and uncertainty parameters here.
Inputs:
world_map_init: list of lists [x,y,theta,id], each representing
a landmark in the world map. Note that there is an ambiguity
between landmarks such that multiple landmarks may have the
same id number.
x_gt_init: 3x1 numpy array that represents the true starting
position of the robot
"""
self.done=False
self._fresh = False
self.x_t = np.array([[0.0,0.0,0.0]]).T
self.x_gt = x_gt_init
self.world_map = world_map_init
self.occupancy_map = occupancy_map_init
self.iteration = 0
self.graph = self.generate_graph(occupancy_map_init)
self.v = 0
self.omega = 0
self.shortest_path = astar(self.graph,1,28)[0]
self.checkpoint = 0
self.CHECKPOINT_RADIUS = .05
self._pub = rospy.Publisher("/"+name+"/cmd_vel",Twist,queue_size=10)
name = rospy.get_param("~myname","pi4")
rospy.Subscriber("/"+name+"/filter_est",PoseStamped,self._pose_callback)
