def compute_route(self, node_source, source_ori, node_target, target_ori):
self._previous_node = node_source
a_star = AStar()
a_star.init_grid(
self._map.get_graph_resolution()[0],
self._map.get_graph_resolution()[1],
self._map.get_walls_directed(node_source, source_ori, node_target,
target_ori), node_source, node_target)
route = a_star.solve()
# JuSt a Corner Case
# Clean this to avoid having to use this function
if route is None:
a_star = AStar()
a_star.init_grid(
self._map.get_graph_resolution()[0],
self._map.get_graph_resolution()[1],
self._map.get_walls_directed(node_source,
source_ori,
node_target,
target_ori,
both_walls=False), node_source,
node_target)
route = a_star.solve()
self._route = route
return route
def send_pose_sp(self):
if self.occupancy and self.has_robot_location and self.nav_sp:
state_min = (-int(round(self.plan_horizon)),
-int(round(self.plan_horizon)))
state_max = (int(round(self.plan_horizon)),
int(round(self.plan_horizon)))
# Round initial, goal positions to grid resolution
x_init = round_pt_to_grid(self.robot_translation[:2],
self.plan_resolution)
x_goal = round_pt_to_grid(self.nav_sp[:2], self.plan_resolution)
astar = AStar(state_min, state_max, x_init, x_goal, self.occupancy,
self.plan_resolution)
# uncomment to add buffering to obstacles
bufferRadius = 4
astar.bufferOccupancy(bufferRadius)
rospy.loginfo("Computing new navigation plan")
if astar.solve():
# If initial state == goal, path len == 1
# Handle case where A* solves, but no 2nd element -> don't send msg
if len(astar.path) < self.short_path_len:
rospy.loginfo("Path goal matches current state")
# Typical use case
else:
wp_x, wp_y, wp_th = self.next_waypoint(astar)
self.publish_path(astar, wp_x, wp_y, wp_th)
else:
rospy.logwarn("Could not find path")
def traveling_salesman_exact(x_init, x_goals, statespace_lo, statespace_hi,
occupancy, resolution):
paths = {}
for i in range(len(x_goals)):
astar = AStar(statespace_lo, statespace_hi, x_init, x_goals[i],
occupancy, resolution)
if astar.solve():
paths[(0, i + 1)] = len(astar.path)
else:
paths[(0, i + 1)] = np.float('inf')
for pair in itertools.combinations(range(len(x_goals)), 2):
astar = AStar(statespace_lo, statespace_hi, x_goals[pair[0]],
x_goals[pair[1]], occupancy, resolution)
pair = [x + 1 for x in list(pair)]
pair = tuple(pair)
if astar.solve():
paths[pair] = len(astar.path)
else:
paths[pair] = np.float('inf')
min_length = np.float('inf')
min_circuit = [0]
for circuit in itertools.permutations(range(len(x_goals))):
circuit = [x + 1 for x in list(circuit)]
circuit = [0] + circuit
curr_length = get_circuit_length(circuit, paths)
if curr_length < min_length:
min_length = curr_length
min_circuit = circuit
min_circuit = min_circuit[1:]
min_circuit = [x - 1 for x in min_circuit]
wps = [x_goals[i] for i in min_circuit]
print("Found path with length: {}:".format(min_length))
print(min_circuit)
print(wps)
return min_circuit
def find_closest(x_init, x_goals, statespace_lo, statespace_hi, occupancy,
resolution):
min_dist = np.float('inf')
closest = None
for x_goal in x_goals:
astar = AStar(statespace_lo, statespace_hi, x_init, x_goal, occupancy,
resolution)
if astar.solve() and len(astar.path) < min_dist:
min_dist = len(astar.path)
closest = x_goal
return closest, min_dist
def send_pose_sp(self):
try:
(robot_translation,
robot_rotation) = self.trans_listener.lookupTransform(
"/map", "/base_footprint", rospy.Time(0))
self.has_robot_location = True
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
robot_translation = (0, 0, 0)
robot_rotation = (0, 0, 0, 1)
self.has_robot_location = False
if self.occupancy and self.has_robot_location and self.nav_sp:
state_min = (-int(round(self.plan_horizon)),
-int(round(self.plan_horizon)))
state_max = (int(round(self.plan_horizon)),
int(round(self.plan_horizon)))
x_init = (round(robot_translation[0] / self.plan_resolution) *
self.plan_resolution,
round(robot_translation[1] / self.plan_resolution) *
self.plan_resolution)
x_goal = ((round(self.nav_sp[0] / self.plan_resolution)) *
self.plan_resolution,
round(self.nav_sp[1] / self.plan_resolution) *
self.plan_resolution)
rospy.logwarn("x_init (after rounding): (%6.4f,%6.4f)", x_init[0],
x_init[1])
rospy.logwarn("x_goal (after rounding): (%6.4f,%6.4f)", x_goal[0],
x_goal[1])
astar = AStar(state_min, state_max, x_init, x_goal, self.occupancy,
self.plan_resolution)
rospy.loginfo("Computing Navigation Plan")
if astar.solve():
rospy.loginfo(
"Navigation Success. Found %d waypoint path to (%6.3f, %6.3f)",
len(astar.path), astar.path[-1][0], astar.path[-1][1])
# a naive path follower we could use
# pose_sp = (astar.path[1][0],astar.path[1][1],self.nav_sp[2])
# msg = Float32MultiArray()
# msg.data = pose_sp
# self.pose_sp_pub.publish(msg)
# astar.plot_path()
path_msg = self.buildPath(astar.path)
self.nav_path_pub.publish(path_msg)
else:
rospy.logwarn("Navigation Failure")
def main(self):
clock = pygame.time.Clock()
graph = grid2graph(self.world) # Convert grid into graph
astar = AStar() # Initialise Astar
res = 25 # Resolution = cm per grid square
while not self.quit:
source = self.node_nb(self.buggy_pos, res) # Source = buggy pos
sink = self.node_nb(self.cur_pos,
res * self.scale) # Sink = cursor pos
[node.reset() for node in graph] # Reset every node
path = astar.solve(graph, source, sink) # Solve graph
self.event_handler(
) # Put all keypress events into key_status dictionary
self.update_window(path, res) # Update display
clock.tick(self.fps) # Restrict rate
def send_pose_sp(self):
try:
(robot_translation,
robot_rotation) = self.trans_listener.lookupTransform(
"/map", "/base_footprint", rospy.Time(0))
self.has_robot_location = True
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
robot_translation = (0, 0, 0)
robot_rotation = (0, 0, 0, 1)
self.has_robot_location = False
if self.occupancy and self.has_robot_location and self.nav_sp:
state_min = (-int(round(self.plan_horizon)),
-int(round(self.plan_horizon)))
state_max = (int(round(self.plan_horizon)),
int(round(self.plan_horizon)))
x_init = (int(round(robot_translation[0])),
int(round(robot_translation[1])))
x_goal = (int(round(self.nav_sp[0])), int(round(self.nav_sp[1])))
astar = AStar(state_min, state_max, x_init, x_goal, self.occupancy,
self.plan_resolution)
rospy.loginfo("Computing navigation plan")
if astar.solve():
# a naive path follower we could use
# pose_sp = (astar.path[1][0],astar.path[1][1],self.nav_sp[2])
# msg = Float32MultiArray()
# msg.data = pose_sp
# self.pose_sp_pub.publish(msg)
# astar.plot_path()
path_msg = Path()
path_msg.header.frame_id = 'map'
for state in astar.path:
pose_st = PoseStamped()
pose_st.pose.position.x = state[0]
pose_st.pose.position.y = state[1]
pose_st.header.frame_id = 'map'
path_msg.poses.append(pose_st)
self.nav_path_pub.publish(path_msg)
else:
rospy.logwarn("Could not find path")
def update_path(self):
try:
(robot_translation,
robot_rotation) = self.trans_listener.lookupTransform(
"/map", "/base_footprint", rospy.Time(0))
self.has_robot_location = True
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
robot_translation = (0, 0, 0)
robot_rotation = (0, 0, 0, 1)
self.has_robot_location = False
if self.occupancy and self.has_robot_location:
state_min = (-int(round(self.plan_horizon)),
-int(round(self.plan_horizon)))
state_max = (int(round(self.plan_horizon)),
int(round(self.plan_horizon)))
#x_init = (int(round(robot_translation[0])), int(round(robot_translation[1])))
#x_goal = (int(round(self.nav_sp[0])), int(round(self.nav_sp[1])))
x_init = self.snap_to_grid(
(robot_translation[0], robot_translation[1]),
self.plan_resolution)
x_goal = self.snap_to_grid((self.nav_sp[0], self.nav_sp[1]),
self.plan_resolution)
astar = AStar(state_min, state_max, x_init, x_goal, self.occupancy,
self.plan_resolution)
rospy.loginfo("Computing navigation plan")
if astar.solve():
path_msg = Path()
path_msg.header.frame_id = 'map'
for idx, state in enumerate(astar.path):
pose_st = PoseStamped()
pose_st.pose.position.x = state[0]
pose_st.pose.position.y = state[1]
pose_st.header.frame_id = 'map'
path_msg.poses.append(pose_st)
if idx == len(astar.path) - 1:
self.wp_node_pub.publish(pose_st)
self.current_path = path_msg
else:
rospy.logwarn("Could not find path")
def re_plan(self, robot_translation, robot_rotation):
state_min = (-int(round(self.plan_horizon)),
-int(round(self.plan_horizon)))
state_max = (int(round(self.plan_horizon)),
int(round(self.plan_horizon)))
x_init = self.round_to_grid(robot_translation[0], robot_translation[1])
x_goal = self.round_to_grid(self.nav_sp[0], self.nav_sp[1])
# plan using astar
astar = AStar(state_min, state_max, x_init, x_goal, self.occupancy,
self.plan_resolution)
rospy.loginfo("Computing navigation plan")
if astar.solve():
# a naive path follower we could use
self.path = astar.path
self.counter = 0
self.send_waypoint()
path_msg = Path()
path_msg.header.frame_id = 'map'
for state in astar.path:
rospy.logerr("path (%f, %f)", state[0], state[1])
pose_st = PoseStamped()
pose_st.pose.position.x = state[0]
pose_st.pose.position.y = state[1]
pose_st.header.frame_id = 'map'
path_msg.poses.append(pose_st)
self.nav_path_pub.publish(path_msg)
rospy.logerr("Have a solution!")
self.nav_sp_reached = False
else:
rospy.logwarn("Could not find path")
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):
self.current_plan = []
return
# makes sure we have a map
if not self.occupancy:
self.current_plan = []
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 = THETA_START_P * theta_err
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
self.nav_pose_pub.publish(pose_g_msg)
return
else:
# just stop
cmd_x_dot = 0
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)
map_2d.generate_random_map(2, 0.25, 3, 2) # Generate start and goal point start = generate_free_point(map_2d, 0, 0.1 * map_2d.size_x, 0, 0.1 * map_2d.size_y) goal = generate_free_point(map_2d, 0.9 * map_2d.size_x, map_2d.size_x - 1, 0.9 * map_2d.size_y, map_2d.size_y - 1) # # Fill in the start/goal point map_2d[start] = 5 map_2d[goal] = 2 # dijk_planner = Dijkstra() # path = dijk_planner.solve(map_2d, start, goal) # dijk_planner.animate(map_2d) astar_planner = AStar() path = astar_planner.solve(map_2d, start, goal) astar_planner.animate(map_2d, 50) # for i in path: # map_2d[i] = 3 # map_2d.display()
class Navigator():
def __init__(self):
rospy.init_node('navigator')
self.trajectory_published = False
self.map_width = 10
self.map_height = 10
self.obstacles = [((6, 6), (8, 7)), ((2, 0), (8, 2)), ((2, 4), (4, 6)),
((6, 2), (8, 4))]
self.start_pos = (None, None)
self.end_pos = (8.0, 8.0)
self.pos = np.array([None, None])
self.theta = None
self.mode = None
self.trajectory = None
# subscribers
rospy.Subscriber('estimator/state', State, self.stateCallback)
rospy.Subscriber('state_machine/mode', Int8, self.modeCallback)
# publishers
self.pub_trajectory = rospy.Publisher('cmd_trajectory',
Trajectory,
queue_size=10)
def stateCallback(self, msg):
self.pos = np.array(msg.pos.data)
self.theta = msg.theta.data
if self.start_pos[0] == None:
self.start_pos = (round(self.pos[0], 1), round(self.pos[1], 1))
def modeCallback(self, msg):
self.mode = Mode(msg.data)
def callAstar(self):
occupancy = DetOccupancyGrid2D(self.map_width, self.map_height,
self.obstacles)
self.astar = AStar((0, 0), (self.map_width, self.map_height),
self.start_pos, self.end_pos, occupancy)
if not self.astar.solve():
exit(0)
self.trajectory = Trajectory()
self.trajectory.x.data = (self.astar.path * self.astar.resolution)[:,
0]
self.trajectory.y.data = (self.astar.path * self.astar.resolution)[:,
1]
self.trajectory.theta.data = np.zeros((len(self.trajectory.x.data), 1))
#astar.plot_path()
def publish(self):
self.pub_trajectory.publish(self.trajectory)
self.trajectory_published = True
def run(self):
rate = rospy.Rate(10) # 10 Hz
while not rospy.is_shutdown() and not self.trajectory_published:
if self.mode == Mode.IDLE:
self.callAstar()
self.publish()
rate.sleep()
obstacles = zip(obs_lower_corners,obs_upper_corners)
occupancy = DetOccupancyGrid2D(width, height, obstacles)
x_goals = []
for i in range(10S):
x_goal = tuple(np.random.randint(0,height-2,2).tolist())
while not (occupancy.is_free(x_goal)):
x_goal = tuple(np.random.randint(0,height-2,2).tolist())
x_goals.append(x_goal)
x_init = x_goals[0]
x_goals = x_goals[1:]
ts_fast_circuit = traveling_salesman.traveling_salesman_fast(x_init, x_goals, (0,0), (width, height), occupancy, 1)
ts_fast = [x_init] + [x_goals[i] for i in ts_fast_circuit]
fig1 = plt.figure()
for i in range(len(ts_fast) - 1):
astar = AStar((0, 0), (width, height), ts_fast[i], ts_fast[i+1], occupancy)
if astar.solve():
astar.plot_path(fig1, r"$x_{"+str(i)+"}$", r"$x_{"+str(i+1)+"}$" + str(len(astar.path)), pcolor='blue')
fig2 = plt.figure()
ts_exact_circuit = traveling_salesman.traveling_salesman_exact(x_init, x_goals, (0,0), (width, height), occupancy, 1)
ts_exact = [x_init] + [x_goals[i] for i in ts_exact_circuit]
for i in range(len(ts_exact) - 1):
astar = AStar((0, 0), (width, height), ts_exact[i], ts_exact[i+1], occupancy)
if astar.solve():
astar.plot_path(fig2, r"$x_{"+str(i)+"}$", r"$x_{"+str(i+1)+"}$" + str(len(astar.path)), pcolor='red')
plt.show()
def send_pose_sp(self):
try:
(robot_translation,
robot_rotation) = self.trans_listener.lookupTransform(
"/map", "/base_footprint", rospy.Time(0))
self.has_robot_location = True
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
robot_translation = (0, 0, 0)
robot_rotation = (0, 0, 0, 1)
self.has_robot_location = False
if self.occupancy and self.has_robot_location and self.nav_sp:
state_min = (-int(round(self.plan_horizon)),
-int(round(self.plan_horizon)))
state_max = (int(round(self.plan_horizon)),
int(round(self.plan_horizon)))
x_init = (self.round_to_res(robot_translation[0]),
self.round_to_res(robot_translation[1]))
x_goal = (self.round_to_res(self.nav_sp[0]),
self.round_to_res(self.nav_sp[1]))
astar = AStar(state_min, state_max, x_init, x_goal, self.occupancy,
self.plan_resolution)
#debug = Float32MultiArray()
#debug.layout.dim.append(MultiArrayDimension())
#debug.layout.dim[0].label = "length"
#debug.layout.dim[0].size = len(self.cur_path)
#debug.layout.dim[0].stride = 1
#debug.data = self.cur_path
#self.nav_debug.publish(debug)
rospy.loginfo("Computing navigation plan")
if self.stillValid(astar) and not (self.need_new_path):
flag = Int32MultiArray()
flag.layout.dim.append(MultiArrayDimension())
flag.layout.dim[0].label = "length"
flag.layout.dim[0].size = 1
flag.layout.dim[0].stride = 1
flag.data = [1]
self.path_validity.publish(flag)
else:
# reset because going to get a new path
self.need_new_path = False
flag = Int32MultiArray()
flag.layout.dim.append(MultiArrayDimension())
flag.layout.dim[0].label = "length"
flag.layout.dim[0].size = 1
flag.layout.dim[0].stride = 1
flag.data = [0]
self.path_validity.publish(flag)
if astar.solve():
# a naive path follower we could use
# pose_sp = (astar.path[1][0],astar.path[1][1],self.nav_sp[2])
# msg = Float32MultiArray()
# msg.data = pose_sp
# self.pose_sp_pub.publish(msg)
# astar.plot_path()
path_msg = Path()
path_msg.header.frame_id = 'map'
self.cur_path = []
for state in astar.path:
pose_st = PoseStamped()
pose_st.pose.position.x = state[0]
pose_st.pose.position.y = state[1]
pose_st.header.frame_id = 'map'
path_msg.poses.append(pose_st)
self.cur_path.append([state[0], state[1]])
self.nav_path_pub.publish(path_msg)
else:
rospy.logwarn("Could not find path")
def send_pose_sp(self):
try:
(robot_translation,
robot_rotation) = self.trans_listener.lookupTransform(
"/map", "/base_footprint", rospy.Time(0))
self.has_robot_location = True
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
robot_translation = (0, 0, 0)
robot_rotation = (0, 0, 0, 1)
self.has_robot_location = False
if self.occupancy and self.has_robot_location and self.nav_sp:
state_min = (-int(round(self.plan_horizon)),
-int(round(self.plan_horizon)))
state_max = (int(round(self.plan_horizon)),
int(round(self.plan_horizon)))
x_init = (round(robot_translation[0] / self.plan_resolution) *
self.plan_resolution,
round(robot_translation[1] / self.plan_resolution) *
self.plan_resolution)
x_goal = (round(self.nav_sp[0] / self.plan_resolution) *
self.plan_resolution,
round(self.nav_sp[1] / self.plan_resolution) *
self.plan_resolution)
if np.linalg.norm(np.array(x_goal) - np.array(x_init)) < 0.1:
if np.linalg.norm(
np.array(robot_translation[0:1]) -
np.array(self.nav_sp[0:1])) < 0.1:
rospy.logwarn("I have reached my target" +
str(self.nav_sp))
self.has_valid_path.publish(False)
return
msg = Float32MultiArray()
msg.data = self.nav_sp
self.pose_sp_pub.publish(msg)
self.has_valid_path.publish(True)
return
astar = AStar(state_min, state_max, x_init, x_goal, self.occupancy,
self.plan_resolution)
rospy.loginfo("Computing navigation plan")
if astar.solve():
# a naive path follower we could use
# make angle the angle between point before and point after
if (len(astar.path) > 2):
angle = np.arctan2(astar.path[2][1] - astar.path[0][1],
astar.path[2][0] - astar.path[0][0])
else:
angle = self.nav_sp[2]
pose_sp = (astar.path[1][0], astar.path[1][1], angle)
msg = Float32MultiArray()
msg.data = pose_sp
self.pose_sp_pub.publish(msg)
rospy.logwarn(robot_translation)
# astar.plot_path()
path_msg = Path()
path_msg.header.frame_id = 'map'
for state in astar.path:
pose_st = PoseStamped()
pose_st.pose.position.x = state[0]
pose_st.pose.position.y = state[1]
pose_st.header.frame_id = 'map'
path_msg.poses.append(pose_st)
self.nav_path_pub.publish(path_msg)
self.has_valid_path.publish(True)
else:
rospy.logwarn("Could not find path")
self.has_valid_path.publish(False)
if not self.occupancy:
rospy.logwarn('No occupancy grid')
