def map_callback(self, map_msg): laser_sigma = self.laser_sigma_hit self.map = map_msg self.grid = np.zeros((map_msg.info.height, map_msg.info.width)) rospy.loginfo('building Likelihood map... ') rospy.logwarn('using laser_sigma = %f', laser_sigma) world_map = map_utils.Map(map_msg) rospy.loginfo('building KDTree') from sklearn.neighbors import KDTree occupied_points = [] all_positions = [] for i in range(world_map.grid.shape[0]): for j in range(world_map.grid.shape[1]): all_positions.append(world_map.cell_position(i, j)) if world_map.grid[i, j] > .9: occupied_points.append(world_map.cell_position(i, j)) kdt = KDTree(occupied_points) rospy.loginfo('Constructing likelihood field from KDTree.') likelihood_field = map_utils.Map(world_map.to_message()) dists = kdt.query(all_positions, k=1)[0][:] probs = np.exp(-(dists**2) / (2 * laser_sigma**2)) likelihood_field.grid = probs.reshape(likelihood_field.grid.shape) rospy.logwarn('Done building likelihood field') self.likelihood_pub.publish(likelihood_field.to_message())
def map_callback(self, map_msg): """ Create and publish the costmap. """ world_map = map_utils.Map(map_msg) cost_map = map_utils.Map(map_msg) # first inflate the obstacles... dilation_size = int(self.robot_radius / world_map.resolution) * 2 + 1 rospy.loginfo("{}".format(dilation_size)) kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (dilation_size, dilation_size)) grid_occupied = np.zeros(world_map.grid.shape) occupied_indices = world_map.grid == 100 grid_occupied[occupied_indices] = 1 rospy.loginfo("dilating costmap...") grid_dilated = cv2.dilate(grid_occupied, kernel) indices = np.logical_and(grid_dilated == 1, world_map.grid != -1) # fully dilated grid... cost_map.grid[grid_dilated == 1] = 100 rospy.loginfo('building KDTree') from sklearn.neighbors import KDTree occupied_points = [] all_positions = [] for i in range(cost_map.grid.shape[0]): for j in range(cost_map.grid.shape[1]): all_positions.append(cost_map.cell_position(i, j)) if cost_map.grid[i, j] == 100: occupied_points.append(cost_map.cell_position(i, j)) kdt = KDTree(occupied_points) dists = kdt.query(all_positions, k=1)[0][:] probs = np.exp(-(dists**2) / (2 * self.sigma**2)) dist_costs = probs.reshape(cost_map.grid.shape) * 100 dist_costs = np.array(dist_costs, dtype='int8') indices = np.logical_and(dist_costs > cost_map.grid, dist_costs > 0) cost_map.grid[indices] = dist_costs[indices] # no cell should have zero cost... cost_map.grid[cost_map.grid == 0] = 1 # import matplotlib.pyplot as plt # plt.imshow(world_map.grid,interpolation='none') # plt.show() rospy.loginfo("publishing costmap...") self.costmap_pub.publish(cost_map.to_message())
def __init__(self): rospy.init_node('map_node') self.map_msg = None self.pos = None rospy.Subscriber('map', OccupancyGrid, self.map_callback) rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, self.position_callback) seenmap_pub = rospy.Publisher('/seenmap', OccupancyGrid, queue_size=10) self.tf_listener = tf.TransformListener() while self.map_msg is None and not rospy.is_shutdown(): rospy.loginfo("Waiting for map...") rospy.sleep(1) occmap = map_utils.Map(self.map_msg) seenmap = map_utils.Map(width=self.map_msg.info.width, height=self.map_msg.info.height, resolution=self.map_msg.info.resolution, origin_x=self.map_msg.info.origin.position.x, origin_y=self.map_msg.info.origin.position.y) while (self.pos is None and not rospy.is_shutdown()): rospy.loginfo("Waiting for position...") rospy.sleep(.1) # set initial values of seenmap. -1 = unknown, 0 = unseen, 1 = seen it = np.nditer(occmap.grid, flags=['multi_index']) while not it.finished: if it[0] < 0: seenmap.grid[it.multi_index] = -1 elif it[0] == 0: seenmap.grid[it.multi_index] = 0 else: seenmap.grid[it.multi_index] = 1 it.iternext() while not rospy.is_shutdown(): # draw triangle pts = self.calc_triangle() one_row, one_col = occmap._cell_index(pts[0], pts[1]) two_row, two_col = occmap._cell_index(pts[2], pts[3]) three_row, three_col = occmap._cell_index(self.pos.position.x, self.pos.position.y) triangle = np.array([[one_col, one_row], [two_col, two_row], [three_col, three_row]], np.int32) cv2.fillConvexPoly(seenmap.grid, triangle, 1) # publish to /seenmap seenmap_pub.publish(seenmap.to_message())
def __init__(self): rospy.init_node('seenmap_server_a') seenmap_pub = rospy.Publisher('/seenmap_a', OccupancyGrid, queue_size=5) self.map_msg = None rospy.Subscriber('map', OccupancyGrid, self.map_callback) while self.map_msg is None and not rospy.is_shutdown(): rospy.loginfo("Waiting for map...") rospy.sleep(1) try: sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) sock.bind(("", 13008)) sock.listen(1) sock, addr = sock.accept() except Exception as e: sock.close() rospy.loginfo("SEENMAP_SERVER ERROR:") rospy.loginfo(e) sys.exit() seenmap = map_utils.Map(width = self.map_msg.info.width, height = self.map_msg.info.height, resolution = self.map_msg.info.resolution, origin_x = self.map_msg.info.origin.position.x, origin_y = self.map_msg.info.origin.position.y) while True: compressed_message = self.recv_msg(sock) message = zlib.decompress(compressed_message) if not message is None: seenmap.grid = pickle.loads(message) og = seenmap.to_message() seenmap_pub.publish(og)
def __init__(self): rospy.init_node('random_nav') rospy.Subscriber('map', OccupancyGrid, self.map_callback) self.map_msg = None while self.map_msg is None and not rospy.is_shutdown(): rospy.loginfo("Waiting for map...") rospy.sleep(.1) self.map = map_utils.Map(self.map_msg) self.demo_map()
def __init__(self): rospy.init_node('goal_client') self.og = None rospy.Subscriber('/seenmap', OccupancyGrid, self.seenmap_callback) host = "134.126.125.237" # ip of server port = 13006 BUFFER_SIZE = 1024 self.goal_reached = True # set up socket connected = False count = 0 while not connected: try: s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) s.connect((host, port)) connected = True except Exception as e: if count <= 10: rospy.loginfo("FAILED TO CONNECT. RETRYING...") connected = False count += 1 rospy.sleep(1) else: rospy.loginfo("I QUIT") sys.exit() while self.og is None and not rospy.is_shutdown(): rospy.loginfo("Waiting for seen_map...") rospy.sleep(1) rospy.loginfo("GOAL CLIENT ONLINE") s.send("CLIENT READY") while not rospy.is_shutdown(): self.seenmap = map_utils.Map(self.og) try: goal = pickle.loads(s.recv(BUFFER_SIZE)) if self.seen(goal): s.send("SEEN") else: s.send("UNSEEN") except Exception as e: s.close() rospy.loginfo("TCP_TEST ERROR:") rospy.loginfo(e) sys.exit()
def __init__(self): super().__init__('random_nav') self.subscription = self.create_subscription(OccupancyGrid, 'map', self.map_callback, 10) self.map_msg = None # Need help while self.map_msg is None: rospy.loginfo("Waiting for map...") rospy.sleep(.1) self.map = map_utils.Map(self.map_msg) self.demo_map()
def __init__(self): rospy.init_node('random_nav') self.ac = actionlib.SimpleActionClient("move_base", MoveBaseAction) self.state_names = {} self.state_names[GoalStatus.PENDING] = "PENDING" self.state_names[GoalStatus.ACTIVE] = "ACTIVE" self.state_names[GoalStatus.PREEMPTED] = "PREEMPTED" self.state_names[GoalStatus.SUCCEEDED] = "SUCCEEDED" self.state_names[GoalStatus.ABORTED] = "ABORTED" self.state_names[GoalStatus.REJECTED] = "REJECTED" self.state_names[GoalStatus.RECALLED] = "RECALLED" self.state_names[GoalStatus.LOST] = "LOST" rospy.Subscriber('map', OccupancyGrid, self.map_callback) self.map_msg = None while self.map_msg is None and not rospy.is_shutdown(): rospy.loginfo("Waiting for map...") rospy.sleep(.1) self.map = map_utils.Map(self.map_msg) self.demo_map() # REPEAT THE FOLLOWING UNTIL ROSPY IS SHUT DOWN: # # GENERATE A RANDOM GOAL LOCATION: # *GENERATE RANDOM LOCATIONS WHERE X AND Y ARE BOTH # IN THE RANGE [-10, 10]. # *CONTINUE GENERATING RANDOM GOAL LOCATIONS UNTIL ONE IS # AT A FREE LOCATION IN THE MAP (see demo_map below) # # ATTEMPT TO NAVIGATE TO THE GOAL: # * SEND THE DESIRED GOAL IN THE MAP COORDINATE FRAME # (see provided action_nav.py file) x_target = 0 y_target = 0 while not rospy.is_shutdown(): #while not self.map.get_cell(x_target, y_target) == 0: x_target = random.uniform(-10, 10) y_target = random.uniform(-10, 10) if self.map.get_cell(x_target, y_target) == 0: self.goto_point(x_target, y_target)
def __init__(self): rospy.init_node('random_nav') self.ac = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.Subscriber('map', OccupancyGrid, self.map_callback) self.map_msg = None while self.map_msg is None and not rospy.is_shutdown(): rospy.loginfo("Waiting for map...") rospy.sleep(.1) self.map = map_utils.Map(self.map_msg) while not rospy.is_shutdown(): x_goal = 100000 y_goal = 100000 while self.map.get_cell(x_goal, y_goal) != 0: x_goal = random.random() * 20.0 - 10.0 y_goal = random.random() * 20.0 - 10.0 rospy.loginfo("Random goal found: ({}, {})".format(x_goal, y_goal)) goal = self.goal_message(x_goal, y_goal, 0) rospy.loginfo("Waiting for server.") self.ac.wait_for_server() rospy.loginfo("Sending goal.") self.ac.send_goal(goal) rospy.loginfo("Goal Sent.") # Wait until the server reports a result. self.ac.wait_for_result() rospy.loginfo(self.ac.get_goal_status_text())
def callback(self, og): seenmap = map_utils.Map(og) message = pickle.dumps(seenmap.grid) compressed_message = zlib.compress(message) self.send_msg(compressed_message)
def __init__(self): rospy.init_node("detect_node") mins = 4 self.closeToVictimforPicture = False self.takeOnePicture = False self.finding = True self.doNewVictim = True self.finishedGoingHome = False self.time = 60 * mins self.startTime = None self.ranTime = None self.tf_listener = tf.TransformListener() self.listOfVictims = [] self.bridge = CvBridge() self.map_msg = None self.imgCounter = 1 self.victim = Victim() self.thresh_hold = 1 self.victim_hold = .3 self.curX = 2.86 #hardcoded based off of current intital_pose.yaml self.curY = -1.77 #hardcoded based off of current intital_pose.yaml self.listOfPoints = [(self.curX, self.curY)] self.first = True self.xpostive = 1 self.ypostive = 1 self.victim.id = 1 self.ID = 1 self.ac = actionlib.SimpleActionClient("move_base", MoveBaseAction) self.state_names = {} self.state_names[GoalStatus.PENDING] = "PENDING" self.state_names[GoalStatus.ACTIVE] = "ACTIVE" self.state_names[GoalStatus.PREEMPTED] = "PREEMPTED" self.state_names[GoalStatus.SUCCEEDED] = "SUCCEEDED" self.state_names[GoalStatus.ABORTED] = "ABORTED" self.state_names[GoalStatus.REJECTED] = "REJECTED" self.state_names[GoalStatus.RECALLED] = "RECALLED" self.state_names[GoalStatus.LOST] = "LOST" self.pub = rospy.Publisher('victim', Victim, queue_size=10) self.twistPub = rospy.Publisher('/cmd_vel_mux/input/navi', Twist, queue_size=1) rospy.Subscriber('map', OccupancyGrid, self.map_callback) rospy.Subscriber('/camera/rgb/image_raw', Image, self.icallback) rospy.Subscriber('/visualization_marker', Marker, self.detect_callback, queue_size=1) while self.map_msg is None and not rospy.is_shutdown(): #print "in while-loop1/init/Detector_Alvar/detect_alvar.py" rospy.loginfo("Waiting for map...") rospy.sleep(.1) self.map = map_utils.Map(self.map_msg) while self.startTime is None: self.startTime = self.map_msg.header.stamp self.count = 0 x_target = 0 y_target = 0 while not rospy.is_shutdown() and self.finding and len( self.listOfVictims) != 4 and ( self.time - (self.ranTime.secs - self.startTime.secs)) > 30: x_target = random.uniform(-10, 10) y_target = random.uniform(-10, 10) if self.map.get_cell(x_target, y_target) == 0: if self.checkPoint(x_target, y_target): self.goto_point(x_target, y_target) while (not self.finding and not rospy.is_shutdown()): rospy.sleep(.1) self.finishedGoingHome = True self.goto_point(self.curX, self.curY) print "done victims are here \n" + str(self.listOfVictims)
def costmap_callback(self, costmap): """ Get the costmap and covert it to a Map object. """ self.costmap = map_utils.Map(costmap)
def __init__(self): rospy.init_node('goal_server') teammate_marker_pub = rospy.Publisher('teammate_marker', Marker, queue_size=10) self.map_msg = None rospy.Subscriber('map', OccupancyGrid, self.map_callback) goal_pub = rospy.Publisher('/new_goal', MoveBaseGoal, queue_size=10) rospy.Subscriber('/goal_reached', Bool, self.goal_reached_callback) self.og = None self.seenmap = None rospy.Subscriber('/seenmap', OccupancyGrid, self.seenmap_callback) # set up map for choosing goals while self.map_msg is None and not rospy.is_shutdown(): rospy.loginfo("Waiting for map...") rospy.sleep(1) self.map = map_utils.Map(self.map_msg) self.goal_reached = True # Set up port for incomming traffic host = "" port = 13005 BUFFER_SIZE = 1024 goal_msg = MoveBaseGoal() goal_msg.target_pose.header.frame_id = "map" try: s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) except: rospy.loginfo("FAILED TO CREATE SOCKET") sys.exit() try: s.bind(("", port)) s.listen(1) conn, addr = s.accept() #rospy.loginfo("WAITING FOR CONNECTION") except Exception as e: rospy.loginfo("FAILED TO BIND") rospy.loginfo(e) sys.exit() rospy.loginfo("GOAL SERVER ONLINE") # main loop while not rospy.is_shutdown(): self.seenmap = map_utils.Map(self.og) try: goal = self.random_goal() conn.send(pickle.dumps(goal)) response = conn.recv(BUFFER_SIZE) if response == "UNSEEN" and self.goal_reached == True: rospy.loginfo("GOAL CHOSEN (%f, %f)", goal[0], goal[1]) goal_msg.target_pose.header.stamp = rospy.get_rostime() goal_msg.target_pose.pose.position.x = goal[0] goal_msg.target_pose.pose.position.y = goal[1] goal_pub.publish(goal_msg) except Exception as e: s.close() rospy.loginfo("SEENMAP SERVER ERROR:") rospy.loginfo(e) sys.exit() rospy.sleep(5)
def __init__(self): rospy.init_node('robo_cleanup') # Subscribers/Publishers self.map_msg = None rospy.Subscriber('map', OccupancyGrid, self.map_callback) rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, self.position_callback) rospy.Subscriber('new_goal', MoveBaseGoal, self.new_goal_callback) rospy.Subscriber('messes_to_clean', Float32MultiArray, self.mess_arr_callback) self.mv_base = rospy.Publisher('/cmd_vel_mux/input/teleop', Twist, queue_size=10) self.goal_reached = rospy.Publisher('/goal_reached', Bool, queue_size=10) # Other globals self.ac = actionlib.SimpleActionClient("move_base", MoveBaseAction) self.position = None self.goal = None self.searching = True self.cleaning = False self.cur_mess = None self.mess_id = 1 self.tf_listener = tf.TransformListener() self.old_marker = Point() self.old_marker.x = 0 self.old_marker.y = 0 # Wait for the map and the robot's position to be initialized while self.map_msg is None and not rospy.is_shutdown(): rospy.loginfo("Waiting for map...") rospy.sleep(1) self.map = map_utils.Map(self.map_msg) while (self.position is None and not rospy.is_shutdown()): rospy.loginfo("Waiting for position...") rospy.sleep(.1) # Initially set the safezone to the start location self.safezone = self.position counter = 0 # Main execution loop while not rospy.is_shutdown(): if (self.searching and self.goal is not None): # if in searching state AND has a valid goal # if first goal if counter == 0: goal = self.goal_message(self.goal.x, self.goal.y, 0) self.ac.send_goal(goal) counter += 1 # if close enough to the goal OR the goal has been reached OR the goal has been abandoned if (self.close_enough(self.position.position.x,self.position.position.y, self.goal.x, self.goal.y, .7) or self.ac.get_state() is 4 or self.ac.get_state() is 3): goal = self.goal_message(self.goal.x, self.goal.y, 0) self.ac.send_goal(goal) self.goal = None if self.cleaning: # if in cleaning state length = len(self.mess_arr)/2 count = 0 # retreive each mess in self.mess_arr for i in range(0, length): self.drive_to_mess(self.mess_arr[i+count], self.mess_arr[i+1+count]) self.take_to_safezone() count += 1 self.cleaning = False
def __init__(self): """ The main control loop for searching and recording victim locations """ # States for the state machine self.state = enum(SEARCHING=1, INVESTIGATING=2, GO_HOME=3) rospy.init_node('random_nav') # setup publishers and subscribers self.ac = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.Subscriber('/visualization_marker', Marker, self.marker_callback) rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, self.position_callback) rospy.Subscriber('/camera/rgb/image_raw', Image, self.image_callback) self.marker_pub = rospy.Publisher('victim_marker', Marker, queue_size=10) self._talk_pub = rospy.Publisher('/robotsound', SoundRequest, queue_size=10) rospy.Subscriber('map', OccupancyGrid, self.map_callback) self.tf_listener = tf.TransformListener() self.current_goal = (1000,1000) # current nav goal self.TIME = 450 # search time for robot self.old_goals = [] # list of previous nav goals self.cv_bridge = CvBridge() # for image conversion self.image = None # raw rgb image self.map_msg = None # sets up map self.victims = [] # list of found victims self.start_time = rospy.get_time() # start time of the robot self.home = False # signals when to go home self.position = None # robot's current position self.cur_state = self.state.SEARCHING # current state of the robot self.cur_marker = Point() # ar marker currently seen self.first_goal = True # signals if it is the 1st goal self.goals = 0 # diagnostic count of old goals old_state = None # previous state of the robot # Set up a SoundRequest message: sound_request = SoundRequest() sound_request.sound = sound_request.SAY sound_request.command = sound_request.PLAY_ONCE sound_request.arg = "Report is Ready!" while self.map_msg is None and not rospy.is_shutdown(): rospy.loginfo("Waiting for map...") rospy.sleep(.1) self.map = map_utils.Map(self.map_msg) while (self.position is None and not rospy.is_shutdown()): rospy.loginfo("Waiting for position...") rospy.sleep(.1) # sets the home postion home_pos = self.position # generate the first random goal x_goal = 100000 y_goal = 100000 while self.map.get_cell(x_goal, y_goal) != 0: x_goal = random.random() * 20.0 - 10.0 y_goal = random.random() * 20.0 - 10.0 rospy.loginfo("Random goal found: ({}, {})".format(x_goal, y_goal)) # main execution loop while not rospy.is_shutdown(): local_marker = self.cur_marker local_state = self.cur_state # Go back to start after certain time limit local_state = self.go_home(local_state) # Set the goal to a random point if local_state is self.state.SEARCHING: goals = self.search(x_goal, y_goal, old_state) x_goal = goals[0] y_goal = goals[1] # Set the goal to the starting position elif local_state is self.state.GO_HOME: x_goal = home_pos.position.x y_goal = home_pos.position.y # Set the goal to the victim's position elif local_state is self.state.INVESTIGATING: local_state = self.investigate(local_state, local_marker) # Only set a new goal if the state has changed if (old_state is not local_state): goal = self.goal_message(x_goal, y_goal, 0) self.go_to_point(goal) self.old_goals.append((x_goal, y_goal)) self.current_goal = (x_goal,y_goal) old_state = local_state self.cur_state = local_state # if going home, wait to reach goal then break out of loop if self.home: self.ac.wait_for_result() self._talk_pub.publish(sound_request) rospy.sleep(3) break