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)