def __init__(self, map_service_name, halton_points, disc_radius,
collision_delta, source_topic, target_topic, pub_topic,
service_topic, car_width, car_length, pose_topic):
print("[Planner Node] Getting map from service...")
rospy.wait_for_service(map_service_name)
self.map_msg = rospy.ServiceProxy(map_service_name, GetMap)().map
print("[Planner Node] ...got map")
print("[Planner Node] Generating graph file...")
graph_file = GraphGenerator.generate_graph_file(
self.map_msg, halton_points, disc_radius, car_width, car_length,
collision_delta)
print("[Planner Node] ..graph generated")
self.environment = HaltonEnvironment(self.map_msg, graph_file, None,
None, car_width, car_length,
disc_radius, collision_delta)
self.planner = HaltonPlanner(self.environment)
self.source_pose = None
self.source_updated = False
self.source_yaw = None
self.source_lock = Lock()
self.target_pose = None
self.target_updated = False
self.target_yaw = None
self.target_lock = Lock()
self.cur_plan = None
self.plan_lock = Lock()
self.orientation_window_size = 21
self.big_plan = np.array([])
self.count = 0
self.count2 = 0
if pub_topic is not None:
self.plan_pub = rospy.Publisher(pub_topic, PoseArray, queue_size=1)
self.source_sub = rospy.Subscriber(source_topic,
PoseWithCovarianceStamped,
self.source_cb,
queue_size=1)
self.target_sub = rospy.Subscriber(target_topic,
PoseStamped,
self.target_cb,
queue_size=1)
else:
self.plan_pub = None
if service_topic is not None:
self.plan_service = rospy.Service(service_topic, GetPlan,
self.get_plan_cb)
else:
self.plan_service = None
print '[Planner Node] Ready to plan'
def __init__(
self,
map_service_name,
halton_points,
disc_radius,
collision_delta,
pub_topic,
car_width,
car_length,
pose_arr,
start_waypoint_topic, #for visualization in rviz
good_waypoint_topic, #for visualization in rviz
bad_waypoint_topic, #for visualization in rviz
start_waypoint_pose, #for visualization in rviz
good_waypoint_pose, #for visualization in rviz
bad_waypoint_pose #for visualization in rviz
):
print("[Planner Node] Getting map from service...")
rospy.wait_for_service(map_service_name)
self.map_msg = rospy.ServiceProxy(map_service_name, GetMap)().map
print("[Planner Node] ...got map")
print("[Planner Node] Generating graph file...")
graph_file = GraphGenerator.generate_graph_file(
self.map_msg, halton_points, disc_radius, car_width, car_length,
collision_delta)
print("[Planner Node] ..graph generated")
self.environment = HaltonEnvironment(self.map_msg, graph_file, None,
None, car_width, car_length,
disc_radius, collision_delta)
self.planner = HaltonPlanner(self.environment)
self.source_yaw = None #0
self.target_yaw = None #0
self.cur_plan = None
self.plan_lock = Lock()
self.pose_arr = pose_arr
self.orientation_window_size = 21
self.start_waypoint_pose = start_waypoint_pose
self.good_waypoint_pose = good_waypoint_pose
self.bad_waypoint_pose = bad_waypoint_pose
#waypoints visualization purpose
self.start_waypoint_pub = rospy.Publisher(start_waypoint_topic,
Marker,
queue_size=100)
self.good_waypoint_pub = rospy.Publisher(good_waypoint_topic,
MarkerArray,
queue_size=100)
self.bad_waypoint_pub = rospy.Publisher(bad_waypoint_topic,
MarkerArray,
queue_size=100)
print('pub topic: ', pub_topic)
if pub_topic is not None:
self.plan_pub = rospy.Publisher(pub_topic, PoseArray, queue_size=1)
else:
self.plan_pub = None
print '[Planner Node] Ready to plan'
