def dist(pos0, pos1): dx = float(pos0[0]) - float(pos1[0]) dy = float(pos0[1]) - float(pos1[1]) return math.sqrt(dx**2.0 + dy**2.0) if __name__ == "__main__": rospy.init_node("astar_tester") t = Timer("MapStorage") map = MapStorage() map.downsample_map_by_half() map.downsample_map_by_half() map.expand_map() map.expand_map() map.expand_map() t.print_delta() def_marker = Marker() def_marker.type = Marker.SPHERE def_marker.action = Marker.ADD def_marker.scale.x = 0.10 def_marker.scale.y = 0.10 def_marker.scale.z = 0.10 def_marker.color.r = 0.0 def_marker.color.g = 1.0 def_marker.color.b = 0.0 def_marker.color.a = 1.0 markers = MarkerPlacer("astar_markers", "map", 1000, def_marker) # Original map # Heuristic: rank = dist(n, goal) + cost # cost = current[1].cost + X
self.vel_uniform_dist) d_theta = ang_vel / self.odom_rate d_x = (lin_vel * math.cos(self.avg_theta))/self.odom_rate d_y = (lin_vel * math.sin(self.avg_theta))/self.odom_rate self.avg_x += d_x self.avg_y += d_y self.avg_theta += d_theta if __name__ == "__main__": rospy.init_node("particle_filter") start = rospy.get_param("/robot_start", [-64.0, 0.0, 0.0]) x, y, t = start[0], start[1], start[2] scan_topic = "noisy_base_scan" pf = ParticleFilter(odom_rate=20.0, sens_median=0.0, sens_std_dev=0.1, vel_uniform_dist=0.1, num_particles=400, base_scan=scan_topic, start_pos=start, debug=True) # rospy.sleep(10.0) rate = rospy.Rate(1) t = Timer("Particle filter") try: while not rospy.is_shutdown(): t.reset() pf.run() t.print_delta() rate.sleep() except KeyboardInterrupt: pass