def replan(self):
"""
loads goal into pose controller
runs planner based on current pose
if plan long enough to track:
smooths resulting traj, loads it into traj_controller
sets self.current_plan_start_time
sets mode to ALIGN
else:
sets mode to PARK
"""
# Make sure we have a map
if not self.occupancy:
rospy.loginfo(
"Navigator: replanning canceled, waiting for occupancy map.")
self.switch_mode(Mode.IDLE)
return
# Attempt to plan a path
state_min = self.snap_to_grid((-self.plan_horizon, -self.plan_horizon))
state_max = self.snap_to_grid((self.plan_horizon, self.plan_horizon))
x_init = self.snap_to_grid((self.x, self.y))
self.plan_start = x_init
x_goal = self.snap_to_grid((self.x_g, self.y_g))
problem = AStar(state_min, state_max, x_init, x_goal, self.occupancy,
self.plan_resolution)
rospy.loginfo("Navigator: computing navigation plan")
success = problem.solve()
if not success:
rospy.loginfo("Planning failed")
return
rospy.loginfo("Planning Succeeded")
planned_path = problem.path
# Check whether path is too short
if len(planned_path) < 4:
rospy.loginfo("Path too short to track")
self.switch_mode(Mode.PARK)
return
# Smooth and generate a trajectory
traj_new, t_new = compute_smoothed_traj(planned_path, self.v_des,
self.spline_alpha,
self.traj_dt)
# If currently tracking a trajectory, check whether new trajectory will take more time to follow
if self.mode == Mode.TRACK:
t_remaining_curr = self.current_plan_duration - self.get_current_plan_time(
)
# Estimate duration of new trajectory
th_init_new = traj_new[0, 2]
th_err = wrapToPi(th_init_new - self.theta)
t_init_align = abs(th_err / self.om_max)
t_remaining_new = t_init_align + t_new[-1]
if t_remaining_new > t_remaining_curr:
rospy.loginfo(
"New plan rejected (longer duration than current plan)")
self.publish_smoothed_path(traj_new,
self.nav_smoothed_path_rej_pub)
return
# Otherwise follow the new plan
self.publish_planned_path(planned_path, self.nav_planned_path_pub)
self.publish_smoothed_path(traj_new, self.nav_smoothed_path_pub)
self.pose_controller.load_goal(self.x_g, self.y_g, self.theta_g)
self.traj_controller.load_traj(t_new, traj_new)
self.current_plan_start_time = rospy.get_rostime()
self.current_plan_duration = t_new[-1]
self.th_init = traj_new[0, 2]
self.heading_controller.load_goal(self.th_init)
if not self.aligned():
rospy.loginfo("Not aligned with start direction")
self.switch_mode(Mode.ALIGN)
return
rospy.loginfo("Ready to track")
self.switch_mode(Mode.TRACK)
def replan(self, force=False):
"""
loads goal into pose controller
runs planner based on current pose
if plan long enough to track:
smooths resulting traj, loads it into traj_controller
sets self.current_plan_start_time
sets mode to ALIGN
else:
sets mode to PARK
"""
# Make sure we have a map
if not self.occupancy:
rospy.loginfo(
"Navigator: replanning canceled, waiting for occupancy map.")
return
# Check if the goal is free
if not self.x_g or not self.y_g or not self.theta_g:
rospy.loginfo("No goal.")
return
X_g = (self.x_g, self.y_g)
# Before planning a path, load goal pose
self.pose_controller.load_goal(X_g[0], X_g[1], self.theta_g)
# Attempt to plan a path
state_min = self.snap_to_grid((-self.plan_horizon, -self.plan_horizon))
state_max = self.snap_to_grid((self.plan_horizon, self.plan_horizon))
x_init = nearest_free((self.x, self.y), self.occupancy_strict,
self.plan_resolution)
self.plan_start = x_init
problem = AStar(state_min, state_max, x_init, X_g,
self.occupancy_strict, self.occupancy,
self.plan_resolution)
success = problem.solve()
if not success:
if self.prev_goal != X_g:
self.error_count = 0
self.prev_goal = X_g
self.error_count += 1
return
planned_path = problem.path
# Check whether path is too short
if len(planned_path) < 4:
self.switch_mode(Mode.PARK)
return
# Smooth and generate a trajectory
traj_new, t_new = compute_smoothed_traj(planned_path, self.v_des,
self.spline_alpha,
self.traj_dt)
# If currently tracking a trajectory, check whether new trajectory will take more time to follow
if self.mode == Mode.TRACK:
t_remaining_curr = self.current_plan_duration - self.get_current_plan_time(
)
# Estimate duration of new trajectory
th_init_new = traj_new[0, 2]
th_err = wrapToPi(th_init_new - self.theta)
t_init_align = abs(th_err / self.om_max)
t_remaining_new = t_init_align + t_new[-1]
if t_remaining_new > t_remaining_curr and not (self.new_goal
or force):
self._publish_smoothed_path(traj_new,
self.nav_smoothed_path_rej_pub)
return
# Otherwise follow the new plan
self.publish_planned_path(planned_path, self.nav_planned_path_pub)
self._publish_smoothed_path(traj_new, self.nav_smoothed_path_pub)
self.traj_controller.load_traj(t_new, traj_new)
self.current_plan_start_time = rospy.get_rostime()
self.current_plan_duration = t_new[-1]
self.new_goal = False
self.th_init = traj_new[0, 2]
self.heading_controller.load_goal(self.th_init)
if not self.aligned():
self.switch_mode(Mode.ALIGN)
return
self.switch_mode(Mode.TRACK)
def replan(self):
"""
loads goal into pose controller
runs planner based on current pose
if plan long enough to track:
smooths resulting traj, loads it into traj_controller
sets self.current_plan_start_time
sets mode to ALIGN
else:
sets mode to PARK
"""
# Make sure we have a map
if not self.occupancy:
rospy.loginfo(
"Navigator: replanning canceled, waiting for occupancy map.")
self.switch_mode(Mode.IDLE)
return
# Attempt to plan a path
state_min = self.snap_to_grid((-self.plan_horizon, -self.plan_horizon))
state_max = self.snap_to_grid((self.plan_horizon, self.plan_horizon))
x_init = self.snap_to_grid((self.x, self.y))
self.plan_start = x_init
x_goal = self.snap_to_grid((self.x_g, self.y_g))
problem = AStar(state_min, state_max, x_init, x_goal, self.occupancy,
self.plan_resolution)
rospy.loginfo("Navigator: computing navigation plan")
success = problem.solve()
if not success:
rospy.loginfo("Planning failed")
#tell squirtle we could not find a path
msg = String()
msg.data = "no_path"
self.publish_squirtle.publish(msg)
if self.mode == Mode.BACKING_UP:
self.deflate_map()
self.switch_mode(Mode.IDLE)
return
rospy.loginfo("Planning Succeeded")
planned_path = problem.path
# Check whether path is too short
if self.at_goal():
#check the stop flag and don't post 'at_goal' if set (or else Squirtle will get confused at stop signs)
if not self.stop_flag:
rospy.loginfo("Path already at goal pose")
#tell squirtle we are at the goal
msg = String()
msg.data = "at_goal"
self.publish_squirtle.publish(msg)
self.switch_mode(Mode.IDLE)
return
elif len(planned_path) < 4:
rospy.loginfo("Path too short to track")
self.switch_mode(Mode.PARK)
return
# Smooth and generate a trajectory
traj_new, t_new = compute_smoothed_traj(planned_path, self.v_des,
self.spline_alpha,
self.traj_dt)
# If currently tracking a trajectory, check whether new trajectory will take more time to follow
if self.mode == Mode.TRACK:
t_remaining_curr = self.current_plan_duration - self.get_current_plan_time(
)
# Estimate duration of new trajectory
th_init_new = traj_new[0, 2]
th_err = wrapToPi(th_init_new - self.theta)
t_init_align = abs(th_err / self.om_max)
t_remaining_new = t_init_align + t_new[-1]
if t_remaining_new > t_remaining_curr:
rospy.loginfo(
"New plan rejected (longer duration than current plan)")
self.publish_smoothed_path(traj_new,
self.nav_smoothed_path_rej_pub)
return
# Otherwise follow the new plan
self.publish_planned_path(planned_path, self.nav_planned_path_pub)
self.publish_smoothed_path(traj_new, self.nav_smoothed_path_pub)
self.pose_controller.load_goal(self.x_g, self.y_g, self.theta_g)
self.traj_controller.load_traj(t_new, traj_new)
self.current_plan_start_time = rospy.get_rostime()
self.current_plan_duration = t_new[-1]
self.th_init = traj_new[0, 2]
self.heading_controller.load_goal(self.th_init)
if not self.aligned():
rospy.loginfo("Not aligned with start direction")
if self.mode == Mode.BACKING_UP:
self.switch_mode(Mode.INFLATE_ALIGN)
else:
self.switch_mode(Mode.ALIGN)
return
if self.mode == Mode.BACKING_UP: #what happens if we want to nav to new goal here...?
self.start_timer(INFLATE_TIME)
self.switch_mode(Mode.INFLATE)
else:
rospy.loginfo("Ready to track")
self.switch_mode(Mode.TRACK)
def execute(self, userdata):
rospy.loginfo('Executing state PLANNING')
ci_outcome = check_inputs(userdata)
if ci_outcome:
return ci_outcome
if not userdata.nav_in.occupancy:
rospy.loginfo(
"Navigator: replanning canceled, waiting for occupancy map.")
return 'failure'
# Attempt to plan a path
state_min = userdata.nav_out.snap_to_grid(
(-userdata.nav_in.plan_horizon, -userdata.nav_in.plan_horizon))
state_max = userdata.nav_out.snap_to_grid(
(userdata.nav_in.plan_horizon, userdata.nav_in.plan_horizon))
x_init = userdata.nav_out.snap_to_grid(
(userdata.nav_in.x, userdata.nav_in.y))
userdata.nav_out.plan_start = x_init
x_goal = userdata.nav_out.snap_to_grid(
(userdata.nav_in.x_g, userdata.nav_in.y_g))
problem = AStar(state_min, state_max, x_init, x_goal,
userdata.nav_out.occupancy,
userdata.nav_out.plan_resolution)
rospy.loginfo("Navigator: computing navigation plan")
success = problem.solve()
if not success:
rospy.loginfo("Planning failed")
return 'failure'
rospy.loginfo("Planning Succeeded")
planned_path = problem.path
# Check whether path is too short
if len(planned_path) < 4:
rospy.loginfo("Path too short to track")
return 'failure'
# Smooth and generate a trajectory
traj_new, t_new = compute_smoothed_traj(planned_path,
userdata.nav_in.v_des,
userdata.nav_in.spline_alpha,
userdata.nav_in.traj_dt)
# Otherwise follow the new plan
userdata.nav_out.publish_planned_path(
planned_path, userdata.nav_out.nav_planned_path_pub)
userdata.nav_out.publish_smoothed_path(
traj_new, userdata.nav_out.nav_smoothed_path_pub)
userdata.nav_out.pose_controller.load_goal(userdata.nav_in.x_g,
userdata.nav_in.y_g,
userdata.nav_in.theta_g)
userdata.nav_out.traj_controller.load_traj(t_new, traj_new)
userdata.nav_out.current_plan_start_time = rospy.get_rostime()
userdata.nav_out.current_plan_duration = t_new[-1]
userdata.nav_out.th_init = traj_new[0, 2]
userdata.nav_out.heading_controller.load_goal(traj_new[0, 2])
return 'success'
def replan(self):
"""
loads goal into pose controller
runs planner based on current pose
if plan long enough to track:
smooths resulting traj, loads it into traj_controller
sets self.current_plan_start_time
sets mode to ALIGN
else:
sets mode to PARK
"""
# Make sure we have a map
if not self.occupancy:
rospy.loginfo(
"Navigator: replanning canceled, waiting for occupancy map.")
self.switch_mode(Mode.IDLE)
return
# Attempt to plan a path
state_min = self.snap_to_grid((-self.plan_horizon, -self.plan_horizon))
state_max = self.snap_to_grid((self.plan_horizon, self.plan_horizon))
x_init = self.snap_to_grid((self.x, self.y))
self.plan_start = x_init
x_goal = self.snap_to_grid((self.x_g, self.y_g))
astar_problem = AStar(state_min, state_max, x_init, x_goal,
self.occupancy, self.plan_resolution)
rrt_problem = GeometricRRTConnect(state_min, state_max, x_init, x_goal,
self.occupancy, self.plan_resolution)
rospy.loginfo("Navigator: computing navigation plan")
rospy.loginfo("Current mode: " + str(self.mode))
astar_success = astar_problem.solve()
astar_path = astar_problem.path
rrt_success = rrt_problem.solve()
rrt_path = rrt_problem.path
success = (astar_success or rrt_success)
if not success:
rospy.loginfo("Planning failed")
return
else:
rospy.loginfo("Planning Succeeded")
if astar_success:
traj_new_astar, t_new_astar, path_length_astar = compute_smoothed_traj(
astar_path, self.v_des, self.spline_alpha, self.traj_dt)
else:
path_length_astar = np.Inf
if rrt_success:
traj_new_rrt, t_new_rrt, path_length_rrt = compute_smoothed_traj(
rrt_path, self.v_des, self.spline_alpha, self.traj_dt)
else:
path_length_rrt = np.Inf
if path_length_astar <= path_length_rrt:
traj_new = traj_new_astar
t_new = t_new_astar
planned_path = astar_path
rospy.loginfo("AStar Gave Shortest Path")
else:
traj_new = traj_new_rrt
t_new = t_new_rrt
planned_path = rrt_path
rospy.loginfo("RRT Gave Shortest Path")
# load goal into pose controller
self.pose_controller.load_goal(self.x_g, self.y_g, self.theta_g)
# Check whether path is too short
'''
if len(planned_path) < 4:
rospy.loginfo("Path too short to track")
self.switch_mode(Mode.PARK)
return
'''
# Smooth and generate a trajectory
# If currently tracking a trajectory, check whether new trajectory will take more time to follow
if self.mode == Mode.TRACK:
t_remaining_curr = self.current_plan_duration - self.get_current_plan_time(
)
# Estimate duration of new trajectory
th_init_new = traj_new[0, 2]
th_err = wrapToPi(th_init_new - self.theta)
t_init_align = abs(th_err / self.om_max)
t_remaining_new = t_init_align + t_new[-1]
if t_remaining_new > t_remaining_curr:
rospy.loginfo(
"New plan rejected (longer duration than current plan)")
self.publish_smoothed_path(traj_new,
self.nav_smoothed_path_rej_pub)
return
# Otherwise follow the new plan
self.publish_planned_path(planned_path, self.nav_planned_path_pub)
self.publish_smoothed_path(traj_new, self.nav_smoothed_path_pub)
self.traj_controller.load_traj(t_new, traj_new)
self.current_plan_start_time = rospy.get_rostime()
self.current_plan_duration = t_new[-1]
self.th_init = traj_new[0, 2]
self.heading_controller.load_goal(self.th_init)
if (not self.aligned() and self.mode != Mode.CROSS and self.mode != Mode.STOP) \
or (self.mode == Mode.IDLE):
rospy.loginfo("Not aligned with start direction")
self.switch_mode(Mode.ALIGN)
return