def plan_to(self, pose):
"""Plan a path to the given pose using A*."""
x_min = self.snap_to_grid((-self.plan_horizon, -self.plan_horizon))
x_max = self.snap_to_grid((self.plan_horizon, self.plan_horizon))
x_i = nearest_free((self.x, self.y), self.occupancy_strict,
self.plan_resolution)
x_g = nearest_free((pose[0], pose[1]), self.occupancy_strict,
self.plan_resolution)
publish_marker("goal", (x_g[0], x_g[1]), 100, (0, 0, 1))
problem = AStar(x_min, x_max, x_i, x_g, self.occupancy_strict,
self.occupancy, self.plan_resolution)
success = problem.solve()
if not success:
return None
return problem
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 run_navigator(self):
""" computes a path from current state to goal state using A* and sends it to the path controller """
# makes sure we have a location
try:
(translation, rotation) = self.trans_listener.lookupTransform(
'/map', '/base_footprint', rospy.Time(0))
self.x = translation[0]
self.y = translation[1]
euler = tf.transformations.euler_from_quaternion(rotation)
self.theta = euler[2]
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
print("tf exception in run navigator")
self.current_plan = []
return
# makes sure we have a map
if not self.occupancy:
self.current_plan = []
print("no map")
return
# if close to the goal, use the pose_controller instead
if self.close_to_end_location():
rospy.loginfo("Navigator: Close to nav goal using pose controller")
pose_g_msg = Pose2D()
pose_g_msg.x = self.x_g
pose_g_msg.y = self.y_g
pose_g_msg.theta = self.theta_g
self.nav_pose_pub.publish(pose_g_msg)
self.current_plan = []
self.V_prev = 0
return
# if there is no plan, we are far from the start of the plan,
# or the occupancy grid has been updated, update the current plan
if len(self.current_plan) == 0 or not (
self.close_to_start_location()) or self.occupancy_updated:
# use A* to compute new plan
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))
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")
if problem.solve():
if len(problem.path) > 3:
# cubic spline interpolation requires 4 points
self.current_plan = problem.path
self.current_plan_start_time = rospy.get_rostime()
self.current_plan_start_loc = [self.x, self.y]
self.occupancy_updated = False
# publish plan for visualization
path_msg = Path()
path_msg.header.frame_id = 'map'
for state in self.current_plan:
pose_st = PoseStamped()
pose_st.pose.position.x = state[0]
pose_st.pose.position.y = state[1]
pose_st.pose.orientation.w = 1
pose_st.header.frame_id = 'map'
path_msg.poses.append(pose_st)
self.nav_path_pub.publish(path_msg)
path_t = [0]
path_x = [self.current_plan[0][0]]
path_y = [self.current_plan[0][1]]
for i in range(len(self.current_plan) - 1):
dx = self.current_plan[i +
1][0] - self.current_plan[i][0]
dy = self.current_plan[i +
1][1] - self.current_plan[i][1]
path_t.append(path_t[i] +
np.sqrt(dx**2 + dy**2) / V_DES)
path_x.append(self.current_plan[i + 1][0])
path_y.append(self.current_plan[i + 1][1])
# interpolate the path with cubic spline
self.path_x_spline = scipy.interpolate.splrep(path_t,
path_x,
k=3,
s=SMOOTH)
self.path_y_spline = scipy.interpolate.splrep(path_t,
path_y,
k=3,
s=SMOOTH)
self.path_tf = path_t[-1]
# to inspect the interpolation and smoothing
# t_test = np.linspace(path_t[0],path_t[-1],1000)
# plt.plot(path_t,path_x,'ro')
# plt.plot(t_test,scipy.interpolate.splev(t_test,self.path_x_spline,der=0))
# plt.plot(path_t,path_y,'bo')
# plt.plot(t_test,scipy.interpolate.splev(t_test,self.path_y_spline,der=0))
# plt.show()
else:
rospy.logwarn("Navigator: Path too short, not updating")
else:
rospy.logwarn("Navigator: Could not find path")
self.current_plan = []
# if we have a path, execute it (we need at least 3 points for this controller)
if len(self.current_plan) > 3:
# if currently not moving, first line up with the plan
if self.V_prev == 0:
theta_init = np.arctan2(
self.current_plan[1][1] - self.current_plan[0][1],
self.current_plan[1][0] - self.current_plan[0][0])
theta_err = theta_init - self.theta
if abs(theta_err) > THETA_START_THRESH:
cmd_msg = Twist()
cmd_msg.linear.x = 0
cmd_msg.angular.z = np.sign(theta_err) * np.min(
[THETA_START_P * np.abs(theta_err), W_MAX])
self.nav_vel_pub.publish(cmd_msg)
return
# compute the "current" time along the path execution
t = (rospy.get_rostime() - self.current_plan_start_time).to_sec()
t = max(0.0, t)
t = min(t, self.path_tf)
x_d = scipy.interpolate.splev(t, self.path_x_spline, der=0)
y_d = scipy.interpolate.splev(t, self.path_y_spline, der=0)
xd_d = scipy.interpolate.splev(t, self.path_x_spline, der=1)
yd_d = scipy.interpolate.splev(t, self.path_y_spline, der=1)
xdd_d = scipy.interpolate.splev(t, self.path_x_spline, der=2)
ydd_d = scipy.interpolate.splev(t, self.path_y_spline, der=2)
# publish current desired x and y for visualization only
pathsp_msg = PoseStamped()
pathsp_msg.header.frame_id = 'map'
pathsp_msg.pose.position.x = x_d
pathsp_msg.pose.position.y = y_d
theta_d = np.arctan2(yd_d, xd_d)
quat_d = tf.transformations.quaternion_from_euler(0, 0, theta_d)
pathsp_msg.pose.orientation.x = quat_d[0]
pathsp_msg.pose.orientation.y = quat_d[1]
pathsp_msg.pose.orientation.z = quat_d[2]
pathsp_msg.pose.orientation.w = quat_d[3]
self.nav_pathsp_pub.publish(pathsp_msg)
if self.V_prev <= 0.0001:
self.V_prev = linalg.norm([xd_d, yd_d])
dt = (rospy.get_rostime() - self.V_prev_t).to_sec()
xd = self.V_prev * np.cos(self.theta)
yd = self.V_prev * np.sin(self.theta)
u = np.array([
xdd_d + KPX * (x_d - self.x) + KDX * (xd_d - xd),
ydd_d + KPY * (y_d - self.y) + KDY * (yd_d - yd)
])
J = np.array(
[[np.cos(self.theta), -self.V_prev * np.sin(self.theta)],
[np.sin(self.theta), self.V_prev * np.cos(self.theta)]])
a, om = linalg.solve(J, u)
V = self.V_prev + a * dt
# apply saturation limits
cmd_x_dot = np.sign(V) * min(V_MAX, np.abs(V))
cmd_theta_dot = np.sign(om) * min(W_MAX, np.abs(om))
elif len(self.current_plan) > 0:
# using the pose controller for paths too short
# just send the next point
pose_g_msg = Pose2D()
pose_g_msg.x = self.current_plan[0][0]
pose_g_msg.y = self.current_plan[0][1]
if len(self.current_plan) > 1:
pose_g_msg.theta = np.arctan2(
self.current_plan[1][1] - self.current_plan[0][1],
self.current_plan[1][0] - self.current_plan[0][0])
else:
pose_g_msg.theta = self.theta_g
rospy.loginfo(
"Navigator: Using Pose Controller For Paths Too Short")
self.nav_pose_pub.publish(pose_g_msg)
return
else:
# just stop
print("Going backward to find new path")
cmd_x_dot = BACKWARD_RECOVERY_SPEED
cmd_theta_dot = 0
# saving the last velocity for the controller
self.V_prev = cmd_x_dot
self.V_prev_t = rospy.get_rostime()
cmd_msg = Twist()
cmd_msg.linear.x = cmd_x_dot
cmd_msg.angular.z = cmd_theta_dot
self.nav_vel_pub.publish(cmd_msg)
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))
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 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