def __init__(self): rospy.init_node('maze_pro_solver', anonymous=True) # Try to load parameters from launch file, otherwise set to None try: positions = rospy.get_param('~position') startX, startY = positions['startX'], positions['startY'] targetX, targetY = positions['targetX'], positions['targetY'] start = (startX, startY) target = (targetX, targetY) except: start, target = None, None # Load maze matrix self.map_loader = MapLoader( start, target) # do not crop if target outside of maze self.map_matrix = self.map_loader.loadMap() Cell.resolution = self.map_loader.occupancy_grid.info.resolution # Calculate path self.path_finder = PathFinder(self.map_matrix) raw_path = self.path_finder.calculate_path() self.path = [ Cell(r - self.path_finder.start.row, c - self.path_finder.start.column) for r, c in raw_path ] # move rows to correct starting position self.goal = self.path[0].pose() self.path_index = 0 self.pose = Pose(0, 0, 0) # Setup publishers self.cmd_vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=5) # Setup subscribers odom_sub = rospy.Subscriber("/odom", Odometry, self.odom_callback)
def load(): starttime = time.time() * 1000 initdict_terrain = MapLoader.loadFile( "../../def/1_Terrain.yml")["terrain"] initdict_zones = MapLoader.loadFile("../../def/2_Zones.yml")["zones"] initdict_waypoints = MapLoader.loadFile( "../../def/3_Waypoints.yml")["waypoints"] initdict_entities = MapLoader.loadFile( "../../def/4_Entities.yml")["entities"] initdict_objects = MapLoader.loadFile( "../../def/5_Objects.yml")["objects"] # Instantiate objects before creating the map dict for zone in initdict_zones: initdict_zones[zone] = Zone(initdict_zones[zone]) for waypoint in initdict_waypoints: initdict_waypoints[waypoint] = Waypoint( initdict_waypoints[waypoint]) for entity in initdict_entities: initdict_entities[entity] = Entity(initdict_entities[entity]) for obj in initdict_objects: initdict_objects[obj] = Object(initdict_objects[obj]) rospy.loginfo("Loaded files in {0:.2f}ms.".format(time.time() * 1000 - starttime)) # Create main Map dict Map.MapDict = DictManager({ "terrain": Terrain(initdict_terrain), "zones": DictManager(initdict_zones), "waypoints": DictManager(initdict_waypoints), "entities": DictManager(initdict_entities), "objects": DictManager(initdict_objects) }) rospy.loginfo("Loaded map in {0:.2f}ms.".format(time.time() * 1000 - starttime))
def load(): # Loading XML files starttime = time.time() * 1000 xml_config = MapLoader.loadFile("1_config.xml") xml_terrain = MapLoader.loadFile("2_terrain.xml") xml_waypoints = MapLoader.loadFile("3_waypoints.xml") xml_objects = MapLoader.loadFile("4_objects.xml") xml_robot = MapLoader.loadFile("5_robot.xml") rospy.loginfo("Loaded files in {0:.2f}ms.".format(time.time() * 1000 - starttime)) # Setting current team to the default set one. for xml_team in xml_config.find("teams").findall("team"): if "default" in xml_team.attrib and bool( xml_team.attrib["default"]): if MapManager.CurrentTeam != "": raise ValueError( "ERROR Two or more teams have been set to default.") MapManager.CurrentTeam = xml_team.attrib["name"] MapManager.Teams.append(Team(xml_team)) if MapManager.CurrentTeam == "": raise ValueError("ERROR One team has to be set as default.") # Loading the color palette for xml_color in xml_config.find("colors").findall("color"): MapManager.Colors.append(Color(xml_color)) # Constructing object classes if not xml_objects.findall("classes"): raise KeyError("Objects file must have a 'classes' tag.") obj_classes = [] for c in xml_objects.find("classes").findall("class"): obj_classes.append(Class(c, obj_classes)) # Instantiate objects and create the map dict MapDict.Terrain = Terrain(xml_terrain) xml_objects.attrib["name"] = "map" MapDict.Objects = Container(xml_objects, obj_classes) MapDict.Waypoints = [ Waypoint(w) for w in xml_waypoints.findall("waypoint") ] MapDict.Robot = Robot(xml_robot, obj_classes) rospy.loginfo("Loaded map in {0:.2f}ms.".format(time.time() * 1000 - starttime))
def __init__(self, precision): rospy.init_node("global_planner", anonymous=True) # Try to load parameters from launch file, otherwise set to None try: positions = rospy.get_param("~position") startX, startY = positions["startX"], positions["startY"] targetX, targetY = positions["targetX"], positions["targetY"] start = (startX, startY) target = (targetX, targetY) except: start, target = None, None # Load maze matrix # do not crop if target outside of maze self.map_loader = MapLoader(start, target) self.map_matrix = self.map_loader.loadMap() GlobalPlanner.matrix = self.map_matrix Cell.resolution = self.map_loader.occupancy_grid.info.resolution self.precision = precision # Calculate path t = time.time() self.path_finder = AStar(self.map_matrix) raw_path = self.path_finder.search() print(time.time() - t) Cell.start = self.path_finder.start self.path = [Cell(r, c) for r, c in raw_path] self.goal = self.path[0].pose() self.path_index = 0 self.pose = Pose(0, 0, 0) # Setup publishers self.cmd_vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10) # self.pid_pub = rospy.Publisher("/pid_err", Float64, queue_size=10) # Setup subscribers _ = rospy.Subscriber("/odom", Odometry, self.odom_callback)
def fullPath(): # This is the main part of the demo program map_loader = MapLoader() # Create a MapLoader to load the world map from a simple image base_image = "simple" # This is the base file name of the input image for map generation map_loader.addFrame("/home/kinova/MillenCapstone/MadalynMillenCapstone/animation/","config_space_bw.png") scale = 0.1 map_loader.createMap(scale) # Discretize the map based on the the scaling factor # Create a big version of discretized map for better visualization big_map = resize(map_loader.map, (0,0),fx=(1.0/scale), fy=(1.0/scale), interpolation=INTER_NEAREST) imshow("Image",map_loader.image) imshow("Map", map_loader.map) imshow("Big", big_map) target_dir = "output" if not os.path.exists(target_dir): print "Creating target directory <",target_dir,"> ..." try: os.makedirs(target_dir) except: print "Failed to create target path!" exit() print "Writing the base images ..." imwrite(target_dir+"/"+base_image+"_img.png",map_loader.image) imwrite(target_dir+"/"+base_image+"_map.png",map_loader.map) imwrite(target_dir+"/"+base_image+"_big_map.png",big_map) print "Wait for key input..." #waitKey() print "Doing the search ..." grid = UndirectedGraph() # Using Russell and Norvig code start=(13,20) goal=(16,30) # Define the test cases we want to run tests = [("depth_first_", depth_first_graph_search), ("breadth_first_",breadth_first_search), ("uniform_cost_", uniform_cost_search)] '''("astar_search_euclid_", astar_search,0), ("astar_search_euclid2_", astar_search,4), ("astar_search_euclid3_", astar_search,5), ("astar_search_euclid025_", astar_search,6), ("astar_search_euclid05_", astar_search,7), ("astar_search_dx_", astar_search,1), ("astar_search_dy_", astar_search,2), ("astar_search_manhattan_", astar_search,3), ("greedy_search_euclid_", greedy_best_first_graph_search,0), ("greedy_search_dx_", greedy_best_first_graph_search,1), ("greedy_search_dy_", greedy_best_first_graph_search,2), ("greedy_search_manhattan_",greedy_best_first_graph_search,3) ]''' paths = [] smallPath = [] smallestPath = float('inf') radPoint = 0.123 radAdd = np.pi / 8 for test in tests: print "Set up the "+test[0]+" ..." file_name = target_dir+"/"+test[0]+base_image video_encoder = VideoEncoder(file_name, map_loader.map, frame_rate = 30.0, fps_factor=1.0, comp_height=1.0/scale, comp_width=2.0/scale) print " output to ",file_name problem2 = GridProblem(start, goal, grid, map_loader.map,scale,video_encoder) # Load the correct grid search algorithm and heuristics print "------------- call ---------------------" if (len(test) > 2): if (test[2] == 0): result, max_frontier_size = test[1](problem2, problem2.h_euclid) # elif (test[2] == 1): result, max_frontier_size = test[1](problem2, problem2.h_x_distance) # elif (test[2] == 2): result, max_frontier_size = test[1](problem2, problem2.h_y_distance) # elif (test[2] == 3): result, max_frontier_size = test[1](problem2, problem2.h_manhattan) # elif (test[2] == 4): result, max_frontier_size = test[1](problem2, problem2.h_euclid2) # elif (test[2] == 5): result, max_frontier_size = test[1](problem2, problem2.h_euclid3) # elif (test[2] == 6): result, max_frontier_size = test[1](problem2, problem2.h_euclid025) # elif (test[2] == 7): result, max_frontier_size = test[1](problem2, problem2.h_euclid05) # else: print "Help",test[2] else: result, max_frontier_size = test[1](problem2) #result,max_frontier_size=depth_first_graph_search(problem2) print "-------------return---------------------" #result = depth_first_graph_search(problem2) #result = breadth_first_search(problem2) #result = uniform_cost_search(problem2) #@result = astar_search(problem2, h=problem2.h_euclid)#manhattan)#y_distance) ftxt = open(file_name+'.txt','w') print " Result=",result print " expansions = ",problem2.expansion ftxt.write("expansions = "+str(problem2.expansion)+"\n") ftxt.write("max frontier = "+str(max_frontier_size)+"\n") if (result is not None): path = result.path() ftxt.write("path cost="+str(problem2.total_path_cost(path))+"\n") ftxt.write("Path="+str(path)+"\n") print "path cost=",problem2.total_path_cost(path) print "Path=",path print "Plotting path ..." map_loader.plotPath(path, 1.0)# scale) big_path = resize(map_loader.path, (0,0),fx=(1.0/scale), fy=(1.0/scale), interpolation=INTER_LINEAR) imshow("Path",big_path) imwrite(file_name+"_path.png",big_path) if len(path) < smallestPath: smallPath = path smallestPath = len(path) else: ftxt.write('no path!') ftxt.close() print " Close the video ..." problem2.video_encoder.release() waitKey(500) for p in smallPath: paths.append((radPoint, ((math.atan(float(p.state[1])/ float(p.state[0]))) * (np.pi / 180)) - radPoint)) print (float(p.state[1])/ float(p.state[0])) if radPoint < 3.11: radPoint = radPoint + radAdd return paths
from copy import deepcopy from maps import * from map_loader import MapLoader from cost_optimization import * map_loader = MapLoader() class MapMerge: def __init__(self): self.water_shed_map = None self.water_shed_matrix = None def get_map_name_by(self, id, basic_name): return "reg_" + str(float(id)) + "_" + str(basic_name) def build_watershed_id_to_pixels(self): water_shed_id_to_pixels = {} for i in range(len(self.water_shed_matrix)): row = self.water_shed_matrix[i] for j in range(len(row)): cell = row[j] if cell == self.water_shed_map.no_data_value: continue cell_obj = water_shed_id_to_pixels.get(cell, []) cell_obj.append({"x": i, "y": j}) water_shed_id_to_pixels[cell] = cell_obj return water_shed_id_to_pixels def merge_outputs_to_one_by_watershed_map(self, water_shed_map_name, maps,
class ProSolver: matrix = None def __init__(self): rospy.init_node('maze_pro_solver', anonymous=True) # Try to load parameters from launch file, otherwise set to None try: positions = rospy.get_param('~position') startX, startY = positions['startX'], positions['startY'] targetX, targetY = positions['targetX'], positions['targetY'] start = (startX, startY) target = (targetX, targetY) except: start, target = None, None # Load maze matrix self.map_loader = MapLoader( start, target) # do not crop if target outside of maze self.map_matrix = self.map_loader.loadMap() ProSolver.matrix = self.map_matrix Cell.resolution = self.map_loader.occupancy_grid.info.resolution # Calculate path self.path_finder = PathFinder(self.map_matrix) raw_path = self.path_finder.calculate_path() Cell.start = self.path_finder.start self.path = [Cell(r, c) for r, c in raw_path] self.goal = self.path[0].pose() self.path_index = 0 self.pose = Pose(0, 0, 0) # Setup publishers self.cmd_vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10) # Setup subscribers #odom_sub = rospy.Subscriber("/odom", Odometry, self.odom_callback) odom_sub = rospy.Subscriber("/amcl_pose", PoseWithCovarianceStamped, self.odom_callback) def odom_callback(self, msg): self.pose.x = msg.pose.pose.position.x self.pose.y = msg.pose.pose.position.y rot_q = msg.pose.pose.orientation (_, _, self.pose.theta) = euler_from_quaternion( [rot_q.x, rot_q.y, rot_q.z, rot_q.w]) def next_pose(self): try: self.path_index += 1 self.goal = self.path[self.path_index].pose() rospy.loginfo("Moving to next pose: [%s, %s]", self.goal.x, self.goal.y) except IndexError: rospy.logwarn("REACHED END OF PATH!") def run(self): rate = rospy.Rate(10) # 10hz speed = Twist() while not rospy.is_shutdown(): # Calculate command ang_speed = 0.4 inc_x = self.goal.x - self.pose.x inc_y = self.goal.y - self.pose.y angle_to_goal = atan2(inc_y, inc_x) real_angle = self.pose.theta # Normalize angle_diff if angle_to_goal < 0: angle_to_goal += 2 * pi real_angle += 2 * pi if real_angle < 0: real_angle += 2 * pi angle_to_goal += 2 * pi angle_diff = angle_to_goal - real_angle if (inc_x**2 + inc_y**2)**0.5 < 0.05: speed.linear.x = 0.0 speed.angular.z = 0.0 self.next_pose() elif abs(angle_diff) > 0.2: # increase tolerance? speed.linear.x = 0.0 if angle_diff < 0: speed.angular.z = -ang_speed else: speed.angular.z = +ang_speed else: speed.linear.x = 0.08 speed.angular.z = 0.0 self.cmd_vel_pub.publish(speed) rate.sleep()
def h_euclid025(self,node): dx = self.h_x_distance(node) dy = self.h_y_distance(node) dist = 0.25*np.sqrt(dx*dx + dy*dy) return int(dist) def h_euclid05(self,node): dx = self.h_x_distance(node) dy = self.h_y_distance(node) dist = 0.5*np.sqrt(dx*dx + dy*dy) return int(dist) # This is the main part of the demo program map_loader = MapLoader() # Create a MapLoader to load the world map from a simple image base_image = "config_space_bw" # This is the base file name of the input image for map generation map_loader.addFrame(".",base_image+".png") scale = 0.25 # scale of map - smaller scale implies larger grid size map_loader.createMap(scale, (-np.pi, np.pi), (-np.pi, np.pi)) # Discretize the map based on the the scaling factor # Create a big version of discretized map for better visualization big_map = cv2.resize(map_loader.map, (0,0),fx=(1.0/scale), fy=(1.0/scale), interpolation=cv2.INTER_NEAREST) cv2.imshow("Image",map_loader.image) cv2.imshow("Map", map_loader.map) cv2.imshow("Big", big_map)
def h_euclid025(self,node): dx = self.h_x_distance(node) dy = self.h_y_distance(node) dist = 0.25*np.sqrt(dx*dx + dy*dy) return int(dist) def h_euclid05(self,node): dx = self.h_x_distance(node) dy = self.h_y_distance(node) dist = 0.5*np.sqrt(dx*dx + dy*dy) return int(dist) # This is the main part of the demo program map_loader = MapLoader() # Create a MapLoader to load the world map from a simple image base_image = "simple" # This is the base file name of the input image for map generation map_loader.addFrame(".",base_image+".png") scale = 0.1 map_loader.createMap(scale) # Discretize the map based on the the scaling factor # Create a big version of discretized map for better visualization big_map = cv2.resize(map_loader.map, (0,0),fx=(1.0/scale), fy=(1.0/scale), interpolation=cv2.INTER_NEAREST) cv2.imshow("Image",map_loader.image) cv2.imshow("Map", map_loader.map) cv2.imshow("Big", big_map) target_dir = "output"
def load_map(self, map_id): self.map_id = map_id loader = MapLoader(map_id) self.map = loader.get_map() self.stations = loader.get_stations() self.x, self.y = loader.get_map_size()
def setup(self): MapLoader.store_maps(self) MapLoader.load_map(self, 'map0') self.player = MapLoader.get_player(self)
class GlobalPlanner: matrix = None def __init__(self, precision): rospy.init_node("global_planner", anonymous=True) # Try to load parameters from launch file, otherwise set to None try: positions = rospy.get_param("~position") startX, startY = positions["startX"], positions["startY"] targetX, targetY = positions["targetX"], positions["targetY"] start = (startX, startY) target = (targetX, targetY) except: start, target = None, None # Load maze matrix # do not crop if target outside of maze self.map_loader = MapLoader(start, target) self.map_matrix = self.map_loader.loadMap() GlobalPlanner.matrix = self.map_matrix Cell.resolution = self.map_loader.occupancy_grid.info.resolution self.precision = precision # Calculate path t = time.time() self.path_finder = AStar(self.map_matrix) raw_path = self.path_finder.search() print(time.time() - t) Cell.start = self.path_finder.start self.path = [Cell(r, c) for r, c in raw_path] self.goal = self.path[0].pose() self.path_index = 0 self.pose = Pose(0, 0, 0) # Setup publishers self.cmd_vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10) # self.pid_pub = rospy.Publisher("/pid_err", Float64, queue_size=10) # Setup subscribers _ = rospy.Subscriber("/odom", Odometry, self.odom_callback) # odom_sub = rospy.Subscriber("/amcl_pose", PoseWithCovarianceStamped, self.odom_callback) def odom_callback(self, msg): self.pose.x = msg.pose.pose.position.x self.pose.y = msg.pose.pose.position.y rot_q = msg.pose.pose.orientation (_, _, self.pose.theta) = euler_from_quaternion( [rot_q.x, rot_q.y, rot_q.z, rot_q.w]) def next_pose(self): try: self.path_index += 1 self.goal = self.path[self.path_index].pose() rospy.loginfo("Moving to next pose: [%s, %s]", self.goal.x, self.goal.y) except IndexError: rospy.logwarn("REACHED END OF PATH!") def run(self): rate = rospy.Rate(10) # 10hz speed = Twist() goto = GotoController() goto.set_max_linear_acceleration(.05) goto.set_max_angular_acceleration(.2) goto.set_forward_movement_only(True) while not rospy.is_shutdown(): speed_pose = goto.get_velocity(self.pose, self.goal, 0) # self.pid_pub.publish(goto.desiredAngVel) speed.linear.x = speed_pose.xVel speed.angular.z = speed_pose.thetaVel self.cmd_vel_pub.publish(speed) if goto.get_goal_distance(self.pose, self.goal) <= .05: # 5 cm precision self.next_pose() rate.sleep()
def h_euclid025(self, node): dx = self.h_x_distance(node) dy = self.h_y_distance(node) dist = 0.25 * np.sqrt(dx * dx + dy * dy) return int(dist) def h_euclid05(self, node): dx = self.h_x_distance(node) dy = self.h_y_distance(node) dist = 0.5 * np.sqrt(dx * dx + dy * dy) return int(dist) # This is the main part of the demo program map_loader = MapLoader( ) # Create a MapLoader to load the world map from a simple image base_image = "simple" # This is the base file name of the input image for map generation map_loader.addFrame(".", base_image + ".png") scale = 0.1 map_loader.createMap( scale) # Discretize the map based on the the scaling factor # Create a big version of discretized map for better visualization big_map = cv2.resize(map_loader.map, (0, 0), fx=(1.0 / scale), fy=(1.0 / scale), interpolation=cv2.INTER_NEAREST) cv2.imshow("Image", map_loader.image)
class ProSolver: def __init__(self): rospy.init_node('maze_pro_solver', anonymous=True) # Try to load parameters from launch file, otherwise set to None try: positions = rospy.get_param('~position') startX, startY = positions['startX'], positions['startY'] targetX, targetY = positions['targetX'], positions['targetY'] start = (startX, startY) target = (targetX, targetY) except: start, target = None, None # Load maze matrix self.map_loader = MapLoader( start, target) # do not crop if target outside of maze self.map_matrix = self.map_loader.loadMap() Cell.resolution = self.map_loader.occupancy_grid.info.resolution # Calculate path self.path_finder = PathFinder(self.map_matrix) raw_path = self.path_finder.calculate_path() self.path = [ Cell(r - self.path_finder.start.row, c - self.path_finder.start.column) for r, c in raw_path ] # move rows to correct starting position self.goal = self.path[0].pose() self.path_index = 0 self.pose = Pose(0, 0, 0) # Setup publishers self.cmd_vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=5) # Setup subscribers odom_sub = rospy.Subscriber("/odom", Odometry, self.odom_callback) def odom_callback(self, msg): self.pose.x = msg.pose.pose.position.x self.pose.y = msg.pose.pose.position.y rot_q = msg.pose.pose.orientation (_, _, self.pose.theta) = euler_from_quaternion( [rot_q.x, rot_q.y, rot_q.z, rot_q.w]) def next_pose(self): try: self.path_index += 1 self.goal = self.path[self.path_index].pose() except IndexError: rospy.logwarn("REACHED END OF PATH!") def run(self): rate = rospy.Rate(10) # 10hz speed = Twist() while not rospy.is_shutdown(): # Calculate command # Do other stuff inc_x = self.goal.x - self.pose.x inc_y = self.goal.y - self.pose.y angle_to_goal = atan2(inc_y, inc_x) if (inc_x**2 + inc_y**2)**0.5 < 0.05: speed.linear.x = 0.0 speed.angular.z = 0.0 self.next_pose() elif abs(angle_to_goal - self.pose.theta) > 0.02: speed.linear.x = 0.0 if (self.pose.theta < angle_to_goal): if (self.pose.theta < -0.2 and angle_to_goal > 0): if (abs(self.pose.theta > (pi / 2)) and abs(angle_to_goal > (pi / 2))): speed.angular.z = 0.3 else: speed.angular.z = -0.3 else: speed.angular.z = 0.3 elif (self.pose.theta > angle_to_goal): if (angle_to_goal < -0.2 and self.pose.theta > 0): if (abs(self.pose.theta > (pi / 2)) or abs(angle_to_goal > (pi / 2))): speed.angular.z = 0.3 else: speed.angular.z = -0.3 else: speed.angular.z = -0.3 else: speed.linear.x = 0.08 speed.angular.z = 0.0 self.cmd_vel_pub.publish(speed) rate.sleep()