def __init__(self, mainpoints_file):
rospy.init_node('final_planner', anonymous=True)
# parse main way points
self.CURVATURE = 0.05 #0.075
self.TRY_DISTANCE = 50
self.SAMPLED_HEADINGS = np.linspace(0, 2 * np.pi, 16, endpoint=False)
self.SPEED = 0.5
self.main_path = _parse_mainpoints(mainpoints_file)
self.main_XYHV = toXYHVPath(self.main_path,
desired_speed=1.5 * self.SPEED)
self.waypoints = None
self.rem_points = None
pose_topic = rospy.get_param("/final_planner/pose_topic",
"/sim_car_pose/pose")
self.curpose = None
self.map = self.get_map()
self.map_x = self.map.info.origin.position.x
self.map_y = self.map.info.origin.position.y
self.map_angle = util.rosquaternion_to_angle(
self.map.info.origin.orientation)
self.map_c = np.cos(self.map_angle)
self.map_s = np.sin(self.map_angle)
self.map_data = self.load_permissible_region(self.map)
self.main_path = util.world_to_map(self.main_path, self.map.info)
self.pose_sub = rospy.Subscriber(pose_topic, PoseStamped, self.pose_cb)
self.rp_waypoints = rospy.Publisher('xx_waypoints',
Marker,
queue_size=20)
self.planning_env = DubinsMapEnvironment(self.map_data.transpose(),
curvature=self.CURVATURE)
rospy.wait_for_message(pose_topic, PoseStamped)
self.PathCompletedService = rospy.Service(
"/final_planner/path_complete", SignalPathComplete,
self.path_completed_cb)
self.RunWaypointsService = rospy.Service(
"/final_planner/run_waypoints", ReadFile, self.waypoints_received)
self.controller = rospy.ServiceProxy("/controller/follow_path",
StampedFollowPath())
self.on_main_path = False
self.returned = False
self.num_hit = 0
# self.rp_waypoints = rospy.Publisher('xx_waypoints', Marker, queue_size=100)
rospy.spin()
def __init__(self,
heuristic_func,
weight_func,
num_vertices,
connection_radius,
graph_file='ros_graph.pkl',
do_shortcut=False,
num_goals=1,
curvature=0.04):
"""
@param heuristic_func: Heuristic function to be used in lazy_astar
@param weight_func: Weight function to be used in lazy_astar
@param num_vertices: Number of vertices in the graph
@param connection_radius: Radius for connecting vertices
@param graph_file: File to load. If provided and the file does not exist,
then the graph is constructed and saved in this filename
@param do_shortcut: If True, shortcut the path
@param num_goals: If > 1, takes multiple goals
"""
rospy.init_node('planner', anonymous=True)
# load map
self.map = self.get_map()
self.map_x = self.map.info.origin.position.x
self.map_y = self.map.info.origin.position.y
self.map_angle = util.rosquaternion_to_angle(
self.map.info.origin.orientation)
self.map_c = np.cos(self.map_angle)
self.map_s = np.sin(self.map_angle)
self.map_data = self.load_permissible_region(self.map)
rospy.Subscriber('/move_base_simple/goal', PoseStamped, self.get_goal)
rospy.Subscriber(rospy.get_param("~pose_topic", "/sim_car_pose/pose"),
PoseStamped, self.get_current_pose)
self.multi_goals = num_goals > 1
self.num_goals = num_goals
if self.multi_goals:
print("Accept multiple goals")
self.goals = []
self.route_sent = False
self.do_shortcut = do_shortcut
# Setup planning env
self.planning_env = DubinsMapEnvironment(self.map_data.transpose(),
curvature=curvature)
if os.path.exists(graph_file):
print("Opening {}".format(graph_file))
self.G = graph_maker.load_graph(graph_file)
else:
print("Generating graph, wait for completion")
self.G = graph_maker.make_graph(
self.planning_env,
sampler=DubinsSampler(self.planning_env),
num_vertices=num_vertices,
connection_radius=connection_radius,
saveto=graph_file,
lazy=True)
print("visualize graph")
self.planning_env.visualize_graph(self.G, saveto="graph.png")
self.num_vertices = num_vertices
self.connection_radius = connection_radius
self.heuristic_func = lambda n1, n2: heuristic_func(
n1, n2, self.planning_env, self.G)
self.weight_func = lambda n1, n2: weight_func(n1, n2, self.
planning_env, self.G)
print("Ready to take goals")
rospy.spin()
def __init__(self, heuristic_func, weight_func, num_vertices, connection_radius,
graph_file='ros_graph_lab.pkl', do_shortcut=False, num_goals=1,
curvature=0.02, plan_time=2, plan_with_budget=False):
"""
@param heuristic_func: Heuristic function to be used in lazy_astar
@param weight_func: Weight function to be used in lazy_astar
@param num_vertices: Number of vertices in the graph
@param connection_radius: Radius for connecting vertices
@param graph_file: File to load. If provided and the file does not exist,
then the graph is constructed and saved in this filename
@param do_shortcut: If True, shortcut the path
@param num_goals: If > 1, takes multiple goals
"""
rospy.init_node('planner', anonymous=True)
# load map
self.map = self.get_map()
self.map_x = self.map.info.origin.position.x
self.map_y = self.map.info.origin.position.y
self.map_angle = util.rosquaternion_to_angle(self.map.info.origin.orientation)
self.map_c = np.cos(self.map_angle)
self.map_s = np.sin(self.map_angle)
self.map_data = self.load_permissible_region(self.map)
self.goals = None
self.total_planning_time = plan_time
self.time_left = plan_time
self.plan_with_budget = plan_with_budget
rospy.Service("/planner/generate_path", GeneratePath, self.gen_path)
# np.save('MapData.npy', self.map_data)
self.num_goals = num_goals
self.do_shortcut = do_shortcut
# Setup planning env
self.planning_env = DubinsMapEnvironment(self.map_data.transpose(), curvature=curvature)
# injected code: update xlimit, ylimit of environment
# to tightly hold all permissible grids.
self.tight_sampling_limits(self.planning_env)
self.sampler = DubinsSampler(self.planning_env)
if os.path.exists(graph_file):
print("Opening {}".format(graph_file))
self.G = graph_maker.load_graph(graph_file)
#self.planning_env.visualize_graph(self.G, saveto="")
else:
print("Generating graph, wait for completion")
start_time = time.time()
self.G = graph_maker.make_graph(self.planning_env,
sampler=self.sampler,
num_vertices=num_vertices,
connection_radius=connection_radius,
saveto=graph_file,
lazy=True)
print("graph generation time:", time.time() - start_time)
print("visualize graph")
self.planning_env.visualize_graph(self.G, saveto="graph.png")
self.num_vertices = num_vertices
self.connection_radius = connection_radius
self.heuristic_func = lambda n1, n2: heuristic_func(n1, n2, self.planning_env, self.G)
self.weight_func = lambda n1, n2: weight_func(n1, n2, self.planning_env, self.G)
print "Ready to take goals"
self.curvature = curvature
debug_plan = False
if debug_plan:
self.visualiza_plan = False
self.path_nodes = None
while not rospy.is_shutdown():
if self.visualiza_plan:
self.planning_env.visualize_plan(self.G, self.path_nodes,
tuple(self.start),tuple(self.goal))
self.visualiza_plan = False
rospy.spin()