def hit_waypoints(self, dubins_path):
w_dubins_path = util.map_to_world(dubins_path, self.map.info)
forward_path = toXYHVPath(w_dubins_path, desired_speed=self.SPEED)
self.backward_path = toXYHVPath(w_dubins_path[::-1],
desired_speed=-self.SPEED)
print "Found path to a waypoint."
print 'sjdflkjshadflkjashdf', type(forward_path)
rospy.sleep(0.1)
success = self.controller(forward_path, 'branch_forward')
print "Controller started.", success
def hit_waypoints(self, dubins_path):
w_dubins_path = util.map_to_world(dubins_path, self.map.info)
h = Header()
h.stamp = rospy.Time.now()
# waypoints = np.concatenate((w_dubins_path, w_dubins_path[::-1]), axis=0)
# speeds = np.empty(2*len(dubins_path))
# speeds[:len(dubins_path)] = self.SPEED
# speeds[len(dubins_path):] = -self.SPEED
# path = XYHVPath(h,[XYHV(*[waypoint[0], waypoint[1], waypoint[2], speed]) \
# for waypoint, speed in zip(waypoints, speeds)])
waypoints = w_dubins_path
forward_path = XYHVPath(h,[XYHV(*[waypoint[0], waypoint[1], waypoint[2], speed]) \
for waypoint, speed in zip(waypoints, speeds)])
backward_path = XYHVPath(h,[XYHV(*[waypoint[0], waypoint[1], waypoint[2], -speed]) \
for waypoint, speed in zip(waypoints[::-1], speeds)])
print "Found path to a waypoint."
success = self.controller(forward_path)
print "Controller started.", success
def plan_to_goal(self, start, goal):
"""
Plan a path from start to goal
Return a path in world frame.
"""
# TODO: Implement here
# 1. Add start and goal nodes to the graph
# 2. Call the lazy a* planner
# 3. Do shortcut if necessary
self.G, start_id = graph_maker.add_node(
self.G,
start,
env=self.planning_env,
connection_radius=self.connection_radius,
start_from_config=True,
lazy=True)
self.G, goal_id = graph_maker.add_node(
self.G,
goal,
env=self.planning_env,
connection_radius=self.connection_radius,
start_from_config=False,
lazy=True)
lastar_start = time.time()
path = lazy_astar.astar_path(self.G,
source=start_id,
target=goal_id,
weight=self.weight_func,
heuristic=self.heuristic_func)
lastar_end = time.time()
print("Lazy planning time: ", lastar_end - lastar_start)
#self.planning_env.visualize_plan(self.G,path)
lastar_end = time.time()
if (len(path) > 2):
path = self.planning_env.shortcut(self.G, path)
print("Shortcut time: ", time.time() - lastar_end)
#self.planning_env.visualize_plan(self.G,path)
# Convert the generated map path to world path.
# configs is the list of waypoints on the path.
configs = self.planning_env.get_path_on_graph(self.G, path)
world_points = util.map_to_world(configs, self.map.info)
return world_points
def run(self):
rospy.sleep(1.0)
self.num_hit = 0
self.on_main_path = False
self.returned = False
ros_poses = []
for i in range(len(self.waypoints)):
rpose = util.particle_to_pose(self.waypoints[i])
ros_poses.append(rpose)
marker = make_marker(rpose, i, "goalpoint")
self.rp_waypoints.publish(marker)
assert len(ros_poses) == len(self.waypoints)
self.waypoints = util.world_to_map(self.waypoints, self.map.info)
# start to main
init_pose = self.curpose
mnt_point_idx = np.argsort(
np.linalg.norm(init_pose[:2] - self.main_path[:, :2], axis=-1))
for i in range(50, mnt_point_idx.shape[0]):
mnt_point = self.main_path[mnt_point_idx[i]]
bridge, bridge_len = self.try_dubins(
init_pose,
mnt_point,
headings=np.linspace(-0.3, 0.3, 7, endpoint=True) +
mnt_point[2])
if bridge is not None:
bridge = util.map_to_world(bridge, self.map.info)
break
resp = self.controller(toXYHVPath(bridge, desired_speed=self.SPEED),
'on_bridge')
print 'start ! '
# hit way points
rate = rospy.Rate(5)
while self.num_hit < len(self.waypoints):
if not self.on_main_path:
continue
curpose = self.curpose
for wid in self.rem_points:
if np.linalg.norm(curpose[:2] -
self.waypoints[wid, :2]) > self.TRY_DISTANCE:
continue
dubins_path, length = self.try_dubins(curpose,
self.waypoints[wid])
if length > 0:
self.on_main_path = False
self.hit_waypoints(dubins_path)
self.rem_points.remove(wid)
marker = Marker()
marker.ns = "waypoints"
marker.id = wid
marker.action = 2
self.rp_waypoints.publish(marker)
break
rate.sleep()
rospy.sleep(5.0)
# main to start
while True:
if np.linalg.norm(self.curpose -
init_pose) <= 0.7 * self.TRY_DISTANCE:
# find returned path to init_pose
bridge, bridge_len = self.try_dubins(self.curpose, init_pose)
# if found, sent path to controller
print 'aaaaaaaaaaaaa'
if bridge is not None:
print 'bbbbbbbbbbbbbbb'
end_path = toXYHVPath(bridge, desired_speed=self.SPEED)
#IPython.embed()
break
rate.sleep()
rospy.sleep(5.0)
resp = self.controller(end_path, 'off_bridge')
while not self.returned:
rate.sleep()
def sample_in_world(self, num): '''world frame multi sampler''' samples, scores = self.sample_multi(num) samples = util.map_to_world(samples, self.map_info) samples[:,2] = scores return samples