def init(self, robot_name):
super(AStarCont, self).init(robot_name)
target_loc = search_pose("run_temp.world", "target0")
self.goal = Point(target_loc[0], target_loc[1])
self.offset = Point(8, 8)
# print "The target is at: %.2f %.2f" % (self.goal.x, self.goal.y)
# print "The robot is at: %.2f %.2f" % (self.pos.px, self.pos.py)
self.planner = algs.a_star_planner(self.grid_num, self.offset)
# TODO: Must be a better way to do this.
# leader.py would like to have a copy of the position proxy so that it may track location
return self.pos
def state_go(self):
idt = self.client.read()
replan = False
# check for obstacles, for a*
for i in range(0, self.ran.ranges_count):
# figure out location of the obstacle...
tao = (2 * math.pi * i) / self.ran.ranges_count
obs_x = self.ran.ranges[i] * math.cos(tao)
obs_y = self.ran.ranges[i] * math.sin(tao)
# obs_x and obs_y are relative to the robot, and I'm okay with that.
if self.add_obstacle(obs_x, obs_y):
replan = True
# reached waypoint?
grid_pos = algs.gridify(Point(self.pos.px, self.pos.py), self.grid_num, self.offset)
grid_way = algs.gridify(self.c_waypoint, self.grid_num, self.offset)
if grid_pos == grid_way:
replan = True
if replan:
self.path = self.planner.plan(Point(self.pos.px, self.pos.py), self.goal)
if self.path == None:
# Reset the planner.
self.planner = algs.a_star_planner(self.grid_num, self.offset)
elif len(self.path) > 2:
self.c_waypoint = self.path[1]
n_waypoint = self.path[2]
theta = math.atan2(n_waypoint.y - self.c_waypoint.y, n_waypoint.x - self.c_waypoint.x)
self.pla.set_cmd_pose(self.c_waypoint.x, self.c_waypoint.y, theta)
elif len(self.path) > 1:
# Only one waypoint left
self.c_waypoint = self.path[1]
theta = self.pos.pa
self.pla.set_cmd_pose(self.c_waypoint.x, self.c_waypoint.y, theta)
# No else, should be finished by now.
self.prev_points.append(draw_all(self.gra, self.pos, self.offset, self.grid_num, None, self.path, self.prev_points))
def state_reset(self):
self.prev_points = []
self.pla.enable(0)
self.planner = algs.a_star_planner(self.grid_num, self.offset)
self.c_waypoint = Point(0,0) # Haha! It looks like an owl.
self.path = []
