class Navigation: # Constructor def __init__(self): # Initializations self.robot_perception = RobotPerception() self.path_planning = PathPlanning() # Check if the robot moves with target or just wanders self.move_with_target = rospy.get_param("calculate_target") # Flag to check if the vehicle has a target or not self.target_exists = False self.select_another_target = 0 self.inner_target_exists = False # Container for the current path self.path = [] # Container for the subgoals in the path self.subtargets = [] # Container for the next subtarget. Holds the index of the next subtarget self.next_subtarget = 0 self.count_limit = 500 # 20 sec self.counter_to_next_sub = self.count_limit # Check if subgoal is reached via a timer callback rospy.Timer(rospy.Duration(0.10), self.checkTarget) # Read the target function self.target_selector = rospy.get_param("target_selector") print "The selected target function is " + self.target_selector self.target_selection = TargetSelection(self.target_selector) # ROS Publisher for the path self.path_publisher = rospy.Publisher( rospy.get_param('path_pub_topic'), Path, queue_size=10) # ROS Publisher for the subtargets self.subtargets_publisher = rospy.Publisher( rospy.get_param('subgoals_pub_topic'), MarkerArray, queue_size=10) # ROS Publisher for the current target self.current_target_publisher = rospy.Publisher( rospy.get_param('curr_target_pub_topic'), Marker, queue_size=10) self.previous_next_subtarget = None def checkTarget(self, event): """ Check if target/sub-target reached. :param event: :return: """ if self.previous_next_subtarget is None or self.previous_next_subtarget < self.next_subtarget: Print.art_print("next_subtarget = %d" % self.next_subtarget, Print.ORANGE) self.previous_next_subtarget = self.next_subtarget # Check if we have a target or if the robot just wanders if self.inner_target_exists == False \ or self.move_with_target == False \ or self.next_subtarget == len(self.subtargets): return self.counter_to_next_sub -= 1 if self.counter_to_next_sub == 0: Print.art_print('\n~~~~ Time reset ~~~~', Print.RED) self.inner_target_exists = False self.target_exists = False return # Get the robot pose in: pixel units & global (map's) coordinate system rp_l_px = [ self.robot_perception.robot_pose['x_px'], self.robot_perception.robot_pose['y_px'] ] # rp_g_px = self.robot_perception.toGlobalCoordinates(rp_l_px) [rx, ry] = [ self.robot_perception.robot_pose['x_px'] - self.robot_perception.origin['x'] / self.robot_perception.resolution, self.robot_perception.robot_pose['y_px'] - self.robot_perception.origin['y'] / self.robot_perception.resolution ] rp_g_px = [rx, ry] # Find the distance between the robot pose and the next subtarget v_g_px = map(operator.sub, rp_g_px, self.subtargets[self.next_subtarget]) dist = math.hypot(v_g_px[0], v_g_px[1]) ######################### NOTE: QUESTION ############################## # What if a later subtarget or the end has been reached before the # next subtarget? Alter the code accordingly. # Check if distance is less than 7 px (14 cm) # if dist < 5: # self.next_subtarget += 1 # self.counter_to_next_sub = self.count_limit # # Check if the final subtarget has been approached # if self.next_subtarget == len(self.subtargets): # self.target_exists = False # Instead of checking only next sub-target check all remaining sub-targets (starting from end up to next # sub-target) and decide if robot is closer to another sub-target at later point in path for st_i in range( len(self.subtargets) - 1, self.next_subtarget - 1, -1): # @v_st_g_px: Difference of Global coordinates in Pixels between robot's position # and i-th" sub-target's position v_st_g_px = map(operator.sub, rp_g_px, self.subtargets[st_i]) if math.hypot(v_st_g_px[0], v_st_g_px[1]) < 5: # reached @st_i, set next sub-target in path as robot's next sub-target self.next_subtarget = st_i + 1 self.counter_to_next_sub = self.count_limit if self.next_subtarget == len(self.subtargets): self.target_exists = False ######################################################################## # Publish the current target if self.next_subtarget == len(self.subtargets): return subtarget = [ self.subtargets[self.next_subtarget][0] * self.robot_perception.resolution + self.robot_perception.origin['x'], self.subtargets[self.next_subtarget][1] * self.robot_perception.resolution + self.robot_perception.origin['y'] ] RvizHandler.printMarker( [subtarget], 1, # Type: Arrow 0, # Action: Add "map", # Frame "art_next_subtarget", # Namespace [0, 0, 0.8, 0.8], # Color RGBA 0.2 # Scale ) # Function that selects the next target, produces the path and updates # the coverage field. This is called from the speeds assignment code, since # it contains timer callbacks def selectTarget(self): # IMPORTANT: The robot must be stopped if you call this function until # it is over # Check if we have a map while not self.robot_perception.have_map: Print.art_print("Navigation: No map yet", Print.RED) return print "\nClearing all markers" RvizHandler.printMarker( [[0, 0]], 1, # Type: Arrow 3, # Action: delete all "map", # Frame "null", # Namespace [0, 0, 0, 0], # Color RGBA 0.1 # Scale ) print '\n\n----------------------------------------------------------' print "Navigation: Producing new target" # We are good to continue the exploration # Make this true in order not to call it again from the speeds assignment self.target_exists = True # Gets copies of the map and coverage local_ogm = self.robot_perception.getMap() local_ros_ogm = self.robot_perception.getRosMap() local_coverage = self.robot_perception.getCoverage() print "Got the map and Coverage" self.path_planning.setMap(local_ros_ogm) # Once the target has been found, find the path to it # Get the global robot pose # Get robot pose in global (map's) coordinates rp_l_px = [ self.robot_perception.robot_pose['x_px'], self.robot_perception.robot_pose['y_px'] ] rp_g_px = self.robot_perception.toGlobalCoordinates( rp_l_px, with_resolution=True) g_robot_pose = rp_g_px # Call the target selection function to select the next best goal # Choose target function self.path = [] force_random = False while len(self.path) == 0: start = time.time() target = self.target_selection.selectTarget( local_ogm, local_coverage, self.robot_perception.robot_pose, self.robot_perception.origin, self.robot_perception.resolution, force_random) self.path = self.path_planning.createPath( g_robot_pose, target, self.robot_perception.resolution) print "Navigation: Path for target found with " + str(len(self.path)) + \ " points" if len(self.path) == 0: Print.art_print( "Path planning failed. Fallback to random target selection", Print.RED) force_random = True # Reverse the path to start from the robot self.path = self.path[::-1] # Break the path to sub-goals every 2 pixels (1m = 20px) step = 1 n_subgoals = int(len(self.path) / step) self.subtargets = [] for i in range(0, n_subgoals): self.subtargets.append(self.path[i * step]) self.subtargets.append(self.path[-1]) self.next_subtarget = 0 print "The path produced " + str(len(self.subtargets)) + " subgoals" ######################### NOTE: QUESTION ############################## # The path is produced by an A* algorithm. This means that it is # optimal in length but 1) not smooth and 2) length optimality # may not be desired for coverage-based exploration ######################################################################## self.counter_to_next_sub = self.count_limit # Publish the path for visualization purposes ros_path = Path() ros_path.header.frame_id = "map" for p in self.path: ps = PoseStamped() ps.header.frame_id = "map" ######################### NOTE: QUESTION ############################## # Fill the ps.pose.position values to show the path in RViz # You must understand what self.robot_perception.resolution # and self.robot_perception.origin are. # Convert p from pixel-units to meter p_m = self.robot_perception.toMeterUnits(p, False) # Convert p_m from global (map's) coordinates to relative (image's) coordinates (using resolution = 1) p_m_r = self.robot_perception.toRelativeCoordinates(p_m, False) # Fill in $ps object ps.pose.position.x = p_m_r[0] ps.pose.position.y = p_m_r[1] ######################################################################## ros_path.poses.append(ps) self.path_publisher.publish(ros_path) # Publish the subtargets for visualization purposes subtargets_mark = [] for s in self.subtargets: st_m = self.robot_perception.toMeterUnits(s, False) st_m_r = self.robot_perception.toRelativeCoordinates(st_m, False) subtargets_mark.append(st_m_r) RvizHandler.printMarker( subtargets_mark, 2, # Type: Sphere 0, # Action: Add "map", # Frame "art_subtargets", # Namespace [0, 0.8, 0.0, 0.8], # Color RGBA 0.2 # Scale ) self.inner_target_exists = True def velocitiesToNextSubtarget(self): [linear, angular] = [0, 0] # Get global robot coordinates rp_l_px = [ self.robot_perception.robot_pose['x_px'], self.robot_perception.robot_pose['y_px'] ] rp_g_px = self.robot_perception.toGlobalCoordinates(rp_l_px) theta_r = self.robot_perception.robot_pose['th'] theta_r_deg = math.degrees(theta_r) if theta_r_deg < 0: theta_r_deg += 360 ######################### NOTE: QUESTION ############################## # The velocities of the robot regarding the next subtarget should be # computed. The known parameters are the robot pose [x,y,th] from # robot_perception and the next_subtarget [x,y]. From these, you can # compute the robot velocities for the vehicle to approach the target. # Hint: Trigonometry is required if self.subtargets and self.next_subtarget <= len(self.subtargets) - 1: # st: in pixel units & on global (map's) coordinate system st_g_px = self.subtargets[self.next_subtarget] # Both, st and rp are in the same coordinate system (global - map's) and in the same units (pixels) # So, the vector difference of these two vectors (st - rp) yields the vector that joins these two points v_g_px = map(operator.sub, st_g_px, rp_g_px) # Robot's current orientations forms $theta_r angle from global coordinate system's x-axis, whereas the # vector calculated above forms $theta_rg angle. The latter is defined next: theta_rg = math.atan2(v_g_px[1], v_g_px[0]) # FIX: use degrees to correctly address negative angles theta_rg_deg = math.degrees(theta_rg) if theta_rg_deg < 0: theta_rg_deg += 360 # Find angle difference, $delta_theta delta_theta_deg = theta_rg_deg - theta_r_deg # Calculate new angular velocity based on delta_theta # (slide #93 - 9th presentation, IRS Course ECE AUTH, [email protected]) if 0 <= delta_theta_deg < 180: angular = delta_theta_deg / 180 elif delta_theta_deg > 0 and delta_theta_deg >= 180: angular = (delta_theta_deg - 360) / 180 elif -180 < delta_theta_deg <= 0: angular = delta_theta_deg / 180 elif delta_theta_deg < 0 and delta_theta_deg < -180: angular = (delta_theta_deg + 360) / 180 # Calculate new linear velocity based on new angular velocity and relation in slide #97 (for n = 6) linear = math.pow(1 - abs(angular), 6) ######################### NOTE: QUESTION ############################## return [linear, angular]