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
