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 = 200 # 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) def checkTarget(self, event): # 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 pixels [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\ ] # Find the distance between the robot pose and the next subtarget dist = math.hypot(\ rx - self.subtargets[self.next_subtarget][0], \ ry - self.subtargets[self.next_subtarget][1]) ######################### NOTE: QUESTION ############################## # What if a later subtarget or the end has been reached before the # next subtarget? Alter the code accordingly. # If the robot is in the starting point it immediately sets the next subtarget if self.next_subtarget == 0: self.next_subtarget += 1 self.counter_to_next_sub = self.count_limit else: # Check if there is a later subtarget, closer than the next one # If one is found, make it the next subtarget and update the time for i in range(self.next_subtarget + 1, len(self.subtargets) - 1): # Find the distance between the robot pose and the later subtarget dist_from_later = math.hypot(\ rx - self.subtargets[i][0], \ ry - self.subtargets[i][1]) if dist_from_later < 15: self.next_subtarget = i self.counter_to_next_sub = self.count_limit + 100 dist = dist_from_later break # If distance from subtarget is less than 5, go to the next one 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 ######################################################################## # 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 self.robot_perception.have_map == False: 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 g_robot_pose = self.robot_perception.getGlobalCoordinates(\ [self.robot_perception.robot_pose['x_px'],\ self.robot_perception.robot_pose['y_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] ######################### 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 weight_data = 0.5 # How much weight to update the data (a) weight_smooth = 0.1 # How much weight to smooth the coordinates (b) min_change = 0.0001 # Minimum change per iteration to keep iterating new_path = np.copy(np.array(self.path)) path_length = len(self.path[0]) change = min_change while change >= min_change: change = 0.0 for i in range(1, len(new_path) - 1): for j in range(path_length): # Initialize x, y x_i = self.path[i][j] y_i = new_path[i][j] y_prev = new_path[i - 1][j] y_next = new_path[i + 1][j] y_i_saved = y_i # Minimize the distance between coordinates of the original # path (y) and the smoothed path (x). Also minimize the # difference between the coordinates of the smoothed path # at time step i, and neighboring time steps. In order to do # all the minimizations, we use Gradient Ascent. y_i += weight_data * (x_i - y_i) + weight_smooth * ( y_next + y_prev - (2 * y_i)) new_path[i][j] = y_i change += abs(y_i - y_i_saved) self.path = new_path ######################################################################## # Break the path to subgoals 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" 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" ps.pose.position.x = 0 ps.pose.position.y = 0 ######################### 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. # Multiply subgoal's coordinates with resolution and # add robot.origin in order to translate between the (0,0) of the robot # pose and (0,0) of the map ps.pose.position.x = p[0] * self.robot_perception.resolution + \ self.robot_perception.origin['x'] ps.pose.position.y = p[1] * self.robot_perception.resolution + \ self.robot_perception.origin['y'] ######################################################################## ros_path.poses.append(ps) self.path_publisher.publish(ros_path) # Publish the subtargets for visualization purposes subtargets_mark = [] for s in self.subtargets: subt = [ s[0] * self.robot_perception.resolution + \ self.robot_perception.origin['x'], s[1] * self.robot_perception.resolution + \ self.robot_perception.origin['y'] ] subtargets_mark.append(subt) 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] [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\ ] theta = self.robot_perception.robot_pose['th'] ######################### 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_x = self.subtargets[self.next_subtarget][0] st_y = self.subtargets[self.next_subtarget][1] ######################### NOTE: QUESTION ############################## # angular and linear velocities are calculated upon the # math types presented at 9.exploration_target_selection.pdf dtheta = math.degrees(math.atan2(st_y - ry, st_x - rx)) - math.degrees(theta) if dtheta >= 0 and dtheta < 180: angular = dtheta / 180 elif dtheta > 0 and dtheta >= 180: angular = (dtheta - 2 * 180) / 180 elif dtheta <= 0 and dtheta > -180: angular = dtheta / 180 elif dtheta < 0 and dtheta < -180: angular = (dtheta + 2 * 180) / 180 #should be angular*0.3 but then the robot moves very slow #and cannot reach the targets within the time set from the timer if angular > 0.3: angular = 0.3 elif angular < -0.3: angular = -0.3 # **20 is used to avoid overshoot problem linear = ((1 - abs(angular))**20) * 0.3 return [linear, angular]
class Navigation: # Constructor def __init__(self): # Initializations self.robot_perception = RobotPerception() self.target_selection = TargetSelection() 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.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 # Check if subgoal is reached via a timer callback rospy.Timer(rospy.Duration(0.10), self.checkTarget) # 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) def checkTarget(self, event): # 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 # Get the robot pose in pixels [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\ ] # YOUR CODE HERE ------------------------------------------------------ # Here the next subtarget is checked for proximity. If the robot is too # close to the subtarget it is considered approached and the next # subtarget is assigned as next. However there are cases where the # robot misses one subtarget due to obstacle detection. Enhance the below # functionality by checking all the subgoals (and the final goal) for # proximity and assign the proper next subgoal # Find the distance between the robot pose and the next subtarget dist = math.hypot(\ rx - self.subtargets[self.next_subtarget][0], \ ry - self.subtargets[self.next_subtarget][1]) # Check if distance is less than 7 px (14 cm) if dist < 7: print "Sub target reached!" self.next_subtarget += 1 # Check if the final subtarget has been approached if self.next_subtarget == len(self.subtargets): print "Final goal reached!" self.target_exists = False # --------------------------------------------------------------------- # Publish the current target if self.next_subtarget == len(self.subtargets): return st = Marker() st.header.frame_id = "map" st.ns = 'as_curr_namespace' st.id = 0 st.header.stamp = rospy.Time(0) st.type = 1 # arrow st.action = 0 # add st.pose.position.x = self.subtargets[self.next_subtarget][0]\ * self.robot_perception.resolution + \ self.robot_perception.origin['x'] st.pose.position.y = self.subtargets[self.next_subtarget][1]\ * self.robot_perception.resolution + \ self.robot_perception.origin['y'] st.color.r = 0 st.color.g = 0 st.color.b = 0.8 st.color.a = 0.8 st.scale.x = 0.2 st.scale.y = 0.2 st.scale.z = 0.2 self.current_target_publisher.publish(st) # Function that seelects 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 self.robot_perception.have_map == False: return 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 # Manually update the coverage field self.robot_perception.updateCoverage() print "Navigation: Coverage updated" # Gets copies of the map and coverage local_ogm = self.robot_perception.getMap() local_coverage = self.robot_perception.getCoverage() print "Got the map and Coverage" # Call the target selection function to select the next best goal target = self.target_selection.selectTarget(\ local_ogm,\ local_coverage,\ self.robot_perception.robot_pose) print "Navigation: New target: " + str(target) # Once the target has been found, find the path to it # Get the global robot pose g_robot_pose = self.robot_perception.getGlobalCoordinates(\ [self.robot_perception.robot_pose['x_px'],\ self.robot_perception.robot_pose['y_px']]) # Need to reverse the path?? self.path = self.path_planning.createPath(\ local_ogm,\ g_robot_pose,\ target) print "Navigation: Path for target found with " + str(len(self.path)) +\ " points" # Reverse the path to start from the robot self.path = self.path[::-1] # Break the path to subgoals every 10 pixels (0.5m) step = 10 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" # 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" ps.pose.position.x = p[0] * self.robot_perception.resolution + \ self.robot_perception.origin['x'] ps.pose.position.y = p[1] * self.robot_perception.resolution + \ self.robot_perception.origin['y'] ros_path.poses.append(ps) self.path_publisher.publish(ros_path) # Publish the subtargets for visualization purposes ros_subgoals = MarkerArray() count = 0 for s in self.subtargets: st = Marker() st.header.frame_id = "map" st.ns = 'as_namespace' st.id = count st.header.stamp = rospy.Time(0) st.type = 2 # sphere st.action = 0 # add st.pose.position.x = s[0] * self.robot_perception.resolution + \ self.robot_perception.origin['x'] st.pose.position.y = s[1] * self.robot_perception.resolution + \ self.robot_perception.origin['y'] st.color.r = 0 st.color.g = 0.8 st.color.b = 0 st.color.a = 0.8 st.scale.x = 0.2 st.scale.y = 0.2 st.scale.z = 0.2 ros_subgoals.markers.append(st) count += 1 self.subtargets_publisher.publish(ros_subgoals) self.inner_target_exists = True def velocitiesToNextSubtarget(self): [linear, angular] = [0, 0] # YOUR CODE HERE ------------------------------------------------------ # 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 # --------------------------------------------------------------------- return [linear, angular]
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.time_limit = 20 # seconds # 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) # Timer Thread (initialization) self.timerThread = TimerThread(self.time_limit) def checkTarget(self, event): # 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 # Check if timer has expired if self.timerThread.expired == True: Print.art_print('\n~~~~ Time reset ~~~~',Print.RED) self.inner_target_exists = False self.target_exists = False return # Get the robot pose in pixels [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\ ] theta_robot = self.robot_perception.robot_pose['th'] # Clear achieved targets self.subtargets = self.subtargets[self.next_subtarget:] self.next_subtarget = 0 # Find the distance between the robot pose and all remaining subtargets dx = [rx - st[0] for st in self.subtargets] dy = [ry - st[1] for st in self.subtargets] dist = [math.hypot(v[0], v[1]) for v in zip(dx, dy)] # Try to optimize path only when there is more than one target available if len(self.subtargets) > 1: ogm = self.robot_perception.getMap() theta_goals = np.arctan2(-np.array(dy),-np.array(dx)) dtheta = theta_goals - theta_robot dtheta[dtheta > np.pi] -= 2*np.pi dtheta[dtheta < -np.pi] += 2*np.pi # Check if there are obstacles between robot and subtarget obstacles_subtarget = [] for st,i in zip(self.subtargets, range(len(self.subtargets))): # Find line in pixels between robot and goal line_pxls = list(bresenham(int(round(st[0])), int(round(st[1])),\ int(round(rx)), int(round(ry)))) # Find if there are any obstacles in line ogm_line = list(map(lambda pxl: ogm[pxl[0], pxl[1]], line_pxls)) N_occupied = len(list(filter(lambda x: x > 80, ogm_line))) obstacles_subtarget.append(N_occupied) # Check if one of next goals is aligned with the current robot theta # In this case plan immediately for this goal in order to improve motion behaviour for st, i in zip(self.subtargets, range(len(self.subtargets))): if np.fabs(dtheta[i]) < np.pi/10 and obstacles_subtarget[i] == 0: self.next_subtarget = i # Keep resetting timer, while moving towards target (without any obstacles inbetween) self.timerThread.reset() # Check if any target is in distance min_dist, min_idx = min(zip(dist, range(len(dist)))) if min_dist < 5: self.next_subtarget = min_idx + 1 # Reset timer if a goal has been reached self.timerThread.reset() # Check if the final subtarget has been approached 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): # Cancel previous goal timer self.timerThread.stop() # IMPORTANT: The robot must be stopped if you call this function until # it is over # Check if we have a map while self.robot_perception.have_map == False: 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 g_robot_pose = self.robot_perception.getGlobalCoordinates(\ [self.robot_perception.robot_pose['x_px'],\ self.robot_perception.robot_pose['y_px']]) # Call the target selection function to select the next best goal # Choose target function self.path = [] force_random = False # Potential Target path_altered = False previous_target = None 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) # ipdb.set_trace() # Check if there was any path alteration if target[0] == True: previous_target = target[2] target = target[1] path_altered = True else: target = target[1] 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 subgoals 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 ######################################################################## # Start timer thread self.timerThread.start() # 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" ps.pose.position.x = 0 ps.pose.position.y = 0 ######################### 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. # ipdb.set_trace() ps.pose.position.x = p[0] * self.robot_perception.resolution \ + self.robot_perception.origin['x'] ps.pose.position.y = p[1] * self.robot_perception.resolution \ + self.robot_perception.origin['y'] ######################################################################## ros_path.poses.append(ps) self.path_publisher.publish(ros_path) # Publish the subtargets for visualization purposes subtargets_mark = [] for s in self.subtargets: subt = [ s[0] * self.robot_perception.resolution + \ self.robot_perception.origin['x'], s[1] * self.robot_perception.resolution + \ self.robot_perception.origin['y'] ] subtargets_mark.append(subt) 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 ) # Print Old and Current Final Target if path_altered == True: subt = [ previous_target[0] * self.robot_perception.resolution + \ self.robot_perception.origin['x'], previous_target[1] * self.robot_perception.resolution + \ self.robot_perception.origin['y'] ] # ipdb.set_trace() RvizHandler.printMarker(\ [subt],\ 3, # Type: Sphere 0, # Action: Add "map", # Frame "art_subtargets", # Namespace [1.0, 0.0, 0.0, 0.8], # Color RGBA 0.2 # Scale ) # ipdb.set_trace() self.inner_target_exists = True def velocitiesToNextSubtarget(self): [linear, angular] = [0, 0] [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\ ] theta = self.robot_perception.robot_pose['th'] ######################### 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 l_max = 0.3 a_max = 0.3 if self.subtargets and self.next_subtarget <= len(self.subtargets) - 1: st_x = self.subtargets[self.next_subtarget][0] st_y = self.subtargets[self.next_subtarget][1] theta_rg = np.arctan2(st_y - ry, st_x - rx) dtheta = (theta_rg - theta) if dtheta > np.pi: omega = (dtheta - 2*np.pi)/np.pi elif dtheta < -np.pi: omega = (dtheta + 2*np.pi)/np.pi else: omega = (theta_rg - theta)/np.pi # Nonlinear relations derived from experimentation linear = l_max * ((1 - np.abs(omega)) ** 5) angular = a_max * np.sign(omega) * (abs(omega) ** (1/5)) ######################### NOTE: QUESTION ############################## return [linear, angular]
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 = 200 # 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) def checkTarget(self, event): # 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 pixels [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\ ] # Find the distance between the robot pose and the next subtarget dist = math.hypot(\ rx - self.subtargets[self.next_subtarget][0], \ ry - self.subtargets[self.next_subtarget][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 else: # Find the next subtarget with the minimum distance min = dist # minimum distance from a next subtarget jump_to_next_subtarget = self.next_subtarget # the number of the next subtarget to jump to next_check = self.next_subtarget + 1 # next subtarget to be checked for i in range(self.next_subtarget+1, len(self.subtargets)): temp_dist = math.hypot(\ rx - self.subtargets[next_check][0], \ ry - self.subtargets[next_check][1]) if temp_dist < min: min = temp_dist jump_to_next_subtarget = i next_check += 1 self.next_subtarget = jump_to_next_subtarget self.counter_to_next_sub = self.count_limit ######################################################################## # 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 self.robot_perception.have_map == False: 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 g_robot_pose = self.robot_perception.getGlobalCoordinates(\ [self.robot_perception.robot_pose['x_px'],\ self.robot_perception.robot_pose['y_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] ######################################### # Extra challenge #1 # A. Smooth path planner if len(self.path) > 3: x = np.array(self.path) y = np.copy(x) a = 0.5 b = 0.1 # FORMULA : y_i = y_i + a * (x_i - y_i) + b * (y_i+1 - 2 * y_i + y_i+1) epsilon = np.sum(np.abs(a * (x[1:-1, :] - y[1:-1, :]) + b * (y[2:, :] - 2*y[1:-1, :] + y[:-2, :]))) while epsilon > 1e-3: y[1:-1, :] += a * (x[1:-1, :] - y[1:-1, :]) + b * (y[2:, :] - 2*y[1:-1, :] + y[:-2, :]) epsilon = np.sum(np.abs(a * (x[1:-1, :] - y[1:-1, :]) + b * (y[2:, :] - 2*y[1:-1, :] + y[:-2, :]))) # Copy the smoother path self.path = y.tolist() # Break the path to subgoals 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" ps.pose.position.x = 0 ps.pose.position.y = 0 ######################### 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. ps.pose.position.x = p[0] * self.robot_perception.resolution + self.robot_perception.origin['x'] ps.pose.position.y = p[1] * self.robot_perception.resolution + self.robot_perception.origin['y'] ######################################################################## ros_path.poses.append(ps) self.path_publisher.publish(ros_path) # Publish the subtargets for visualization purposes subtargets_mark = [] for s in self.subtargets: subt = [ s[0] * self.robot_perception.resolution + \ self.robot_perception.origin['x'], s[1] * self.robot_perception.resolution + \ self.robot_perception.origin['y'] ] subtargets_mark.append(subt) 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] [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\ ] theta = self.robot_perception.robot_pose['th'] # robot orientation ######################### 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_x = self.subtargets[self.next_subtarget][0] st_y = self.subtargets[self.next_subtarget][1] PI = 3.14 MAX_SPEED = 0.3 st_x_rel = st_x - rx st_y_rel = st_y - ry st_th = math.atan2(st_y_rel, st_x_rel) delta = st_th - theta if delta >= 0 and delta < PI: angular = delta / PI elif delta > 0 and delta >= PI: angular = (delta - 2 * PI) / PI elif delta <= 0 and delta > -PI: angular = delta / PI elif delta < 0 and delta < -PI: angular = (delta + 2 * PI) / PI angular *= 5 #add more twisting sensitivity angular *= MAX_SPEED if angular > 0.3: angular = 0.3 elif angular < -0.3: angular = -0.3 linear = ((1 - abs(angular)) ** 10 ) * MAX_SPEED ######################### NOTE: QUESTION ############################## return [linear, angular]
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 = 400 # 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) def checkTarget(self, event): # 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 pixels [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\ ] for i in range(self.next_subtarget, len(self.subtargets)): # Find the distance between the robot pose and the next subtarget dist = math.hypot(\ rx - self.subtargets[i][0], \ ry - self.subtargets[i][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 < 7: self.next_subtarget = 1 + i 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 ######################################################################## # 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 self.robot_perception.have_map == False: 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 g_robot_pose = self.robot_perception.getGlobalCoordinates(\ [self.robot_perception.robot_pose['x_px'],\ self.robot_perception.robot_pose['y_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 subgoals 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" ps.pose.position.x = 0 ps.pose.position.y = 0 ######################### 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 ps.pose.position.x = self.robot_perception.origin['x'] /\ self.robot_perception.resolution ps.pose.position.y = self.robot_perception.origin['y'] /\ self.robot_perception.resolution ros_path.poses.append(ps) self.path_publisher.publish(ros_path) # Publish the subtargets for visualization purposes subtargets_mark = [] for s in self.subtargets: subt = [ s[0] * self.robot_perception.resolution + \ self.robot_perception.origin['x'], s[1] * self.robot_perception.resolution + \ self.robot_perception.origin['y'] ] subtargets_mark.append(subt) 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] [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\ ] theta = self.robot_perception.robot_pose['th'] ######################### 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_x = self.subtargets[self.next_subtarget][0] st_y = self.subtargets[self.next_subtarget][1] wmega = math.atan2((st_y - ry), (st_x - rx)) - theta print(wmega) print(math.atan2((st_y - ry), (st_x - rx))) print(theta) if abs(wmega) >= 0.1: angular = np.sign(wmega) * 0.3 #(wmega/(3.14))*0.3 linear = 0 #(1-(wmega/(3.14)))*0.3 else: angular = (wmega / (3.14)) * 0.3 linear = math.tanh( math.sqrt(pow((st_y - ry), 2) + pow((st_x - rx), 2))) * 0.3 ######################### NOTE: QUESTION ############################## print(angular) return [linear, angular]
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]
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 = 200 # 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) def checkTarget(self, event): # 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 pixels [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 ] # # Find the distance between the robot pose and the next subtarget # dist = math.hypot( # rx - self.subtargets[self.next_subtarget][0], # ry - self.subtargets[self.next_subtarget][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 # find the distance between robot and ALL subtargets dists = (math.hypot(rx - self.subtargets[subtarget][0], ry - self.subtargets[subtarget][1]) for subtarget, _ in enumerate(self.subtargets)) # check the distance of each subtarget from the robot # go towards the nearest and change index of next_subtarget # accordingly for idx, dist in enumerate(dists): if dist < 5: self.next_subtarget = idx + 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 ######################################################################## # 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 g_robot_pose = self.robot_perception.getGlobalCoordinates([ self.robot_perception.robot_pose['x_px'], self.robot_perception.robot_pose['y_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() print(self.robot_perception.resolution) 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 subgoals 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" ps.pose.position.x = 0 ps.pose.position.y = 0 ######################### 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. ps.pose.position.x = p[ 0] * self.robot_perception.resolution + self.robot_perception.origin[ 'x'] ps.pose.position.y = p[ 1] * self.robot_perception.resolution + self.robot_perception.origin[ 'y'] ######################################################################## ros_path.poses.append(ps) self.path_publisher.publish(ros_path) # Publish the subtargets for visualization purposes subtargets_mark = [] for s in self.subtargets: subt = [ s[0] * self.robot_perception.resolution + \ self.robot_perception.origin['x'], s[1] * self.robot_perception.resolution + \ self.robot_perception.origin['y'] ] subtargets_mark.append(subt) 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] [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 ] theta = self.robot_perception.robot_pose['th'] ######################### 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_x = self.subtargets[self.next_subtarget][0] st_y = self.subtargets[self.next_subtarget][1] # Find the distance between the robot and the next subtarget # dist = math.hypot(rx - st_x, ry - st_y) # phi is the angle between the robot and the next subtarget # we calculate that angle using atan2, which returns the angle # between a point [x,y] and the x'x axis, with origin on 0,0 # So, we move the origin to the robots position by subtracting # its coordinates from a given point phi = math.atan2(st_y - ry, st_x - rx) if 3.2 >= ( phi - theta ) >= 0.1 and phi > theta: # if phi is bigger than theta and their difference is # lower than pi (they are anti-parallel), turn left linear = 0 angular = 1 elif 3.2 >= ( theta - phi ) >= 0.1 and phi < theta: # if phi is smaller than theta and their difference is # lower than pi (they are anti-parallel), turn right linear = 0 angular = -1 else: # otherwise, move ahead linear = 1 angular = 0 ######################### NOTE: QUESTION #############-################# return [linear, angular]
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 = 200 # 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) def checkTarget(self, event): # 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 pixels [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\ ] ######################### NOTE: QUESTION ############################## # What if a later subtarget or the end has been reached before the # next subtarget? Alter the code accordingly. # Challenge 5 # Instead of checking the distance of the next target, # check the distance of the remaining targets, therefore you may reach to a next subject # bypassing current. for i in range(self.next_subtarget, len(self.subtargets)): # Find the distance between the robot pose and the next subtarget dist = math.hypot( rx - self.subtargets[i][0], ry - self.subtargets[i][1]) # check distance with the i_th target if i != (len(self.subtargets)-1): if dist < 10: #if distance found to be small from a terget set as next the target i + 1 self.next_subtarget = i + 1 self.counter_to_next_sub = self.count_limit else: if dist < 5: #if distance found to be small from a terget set as next the target i + 1 self.next_subtarget = i + 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 ######################################################################## # 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 self.robot_perception.have_map == False: 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 g_robot_pose = self.robot_perception.getGlobalCoordinates(\ [self.robot_perception.robot_pose['x_px'],\ self.robot_perception.robot_pose['y_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] ################################################### # Extra Challenge 1 # Smooth path if len(self.path) > 3: x = np.array(self.path) y = np.copy(x) a = 0.5 b = 0.1 epsilon = np.sum(np.abs(a * (x[1:-1, :] - y[1:-1, :]) + b * (y[2:, :] - 2*y[1:-1, :] + y[:-2, :]))) while epsilon > 1e-3: y[1:-1, :] += a * (x[1:-1, :] - y[1:-1, :]) + b * (y[2:, :] - 2*y[1:-1, :] + y[:-2, :]) epsilon = np.sum(np.abs(a * (x[1:-1, :] - y[1:-1, :]) + b * (y[2:, :] - 2*y[1:-1, :] + y[:-2, :]))) # Copy the smoother path self.path = y.tolist() #################################################### # Break the path to subgoals 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" ps.pose.position.x = 0 ps.pose.position.y = 0 ######################### 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. # Challenge 2: ps.pose.position.x = p[0] * self.robot_perception.resolution + self.robot_perception.origin['x'] ps.pose.position.y = p[1] * self.robot_perception.resolution + self.robot_perception.origin['y'] ######################################################################## ros_path.poses.append(ps) self.path_publisher.publish(ros_path) # Publish the subtargets for visualization purposes subtargets_mark = [] for s in self.subtargets: subt = [ s[0] * self.robot_perception.resolution + \ self.robot_perception.origin['x'], s[1] * self.robot_perception.resolution + \ self.robot_perception.origin['y'] ] subtargets_mark.append(subt) 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] [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\ ] theta = self.robot_perception.robot_pose['th'] ######################### 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 # Challenge 3 if self.subtargets and self.next_subtarget <= len(self.subtargets) - 1: st_x = self.subtargets[self.next_subtarget][0] st_y = self.subtargets[self.next_subtarget][1] # We know goals position (x', y') and robot's position (x,y) # It is tan(Theta_RG) = (y'-y)/(x'-x) = lamda # Theta_RG = atan(lamda) # Theta_RG is the angle between vector RobotGoal and Ox axis Theta_RG = math.atan2(st_y - ry, st_x - rx) # atan2 return the result in rads # We can calculate the angle between the direction of robot's movement # and the RG vector. The angle will be the difference of RGangle - theta(direction angle of robot's movement) D_Theta = Theta_RG - theta # Based on presentation 9, D_Theta # has to be readjusted in [-pi, pi] if (D_Theta < math.pi) and (D_Theta > -math.pi): omega = D_Theta / math.pi elif D_Theta >= math.pi: omega = (D_Theta - 2 * math.pi) / math.pi elif D_Theta <= -math.pi: omega = (D_Theta + 2 * math.pi) / math.pi # Linear Speed Calculation # presentation 9: u = umax(1- |omega|)^n # larger n -> lower speed # max speed = 0.3 u = ((1 - abs(omega))**(6.00)) linear = 0.3 * u # Robot is steering slowly # therefore we had to make steer faster # max speed = 0.3 omega = (math.copysign(abs(omega)**(1.0/6), omega)) angular = 0.3 * omega ######################### NOTE: QUESTION ############################## return [linear, angular]
class Navigation(object): MAX_LINEAR_VELOCITY = rospy.get_param('max_linear_velocity') MAX_ANGULAR_VELOCITY = rospy.get_param('max_angular_velocity') # 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 self.force_random = 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 = 200 # 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) def checkTarget(self, event): # Check if we have a target or if the robot just wanders if not self.inner_target_exists or not self.move_with_target 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 self.force_random = True return # Get the robot pose in pixels [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 ] ######################### 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). Slice the list of subtargets and then reverse it. for idx, st in enumerate(self.subtargets[self.next_subtarget::][::-1]): # Right now, idx refers to the sliced & reversed array, fix it. idx = len(self.subtargets) - 1 - idx assert idx >= self.next_subtarget dist = math.hypot(rx - st[0], ry - st[1]) if dist < 7: self.next_subtarget = idx + 1 self.counter_to_next_sub = self.count_limit if self.next_subtarget == len(self.subtargets): self.target_exists = False break ######################################################################## # 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 g_robot_pose = self.robot_perception.getGlobalCoordinates([ self.robot_perception.robot_pose['x_px'], self.robot_perception.robot_pose['y_px'] ]) # Call the target selection function to select the next best goal # Choose target function self.path = [] 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, self.force_random) self.force_random = False 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) self.force_random = True # Reverse the path to start from the robot self.path = self.path[::-1] # Break the path to subgoals 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. ps.pose.position.x = p[ 0] * self.robot_perception.resolution + self.robot_perception.origin[ 'x'] ps.pose.position.y = p[ 1] * self.robot_perception.resolution + self.robot_perception.origin[ 'y'] ######################################################################## ros_path.poses.append(ps) self.path_publisher.publish(ros_path) # Publish the subtargets for visualization purposes subtargets_mark = [] for s in self.subtargets: subt = [ s[0] * self.robot_perception.resolution + self.robot_perception.origin['x'], s[1] * self.robot_perception.resolution + self.robot_perception.origin['y'] ] subtargets_mark.append(subt) 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): rx = self.robot_perception.robot_pose['x_px'] - \ self.robot_perception.origin['x'] / self.robot_perception.resolution ry = self.robot_perception.robot_pose['y_px'] - \ self.robot_perception.origin['y'] / self.robot_perception.resolution theta = self.robot_perception.robot_pose['th'] ######################### 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_x = self.subtargets[self.next_subtarget][0] st_y = self.subtargets[self.next_subtarget][1] theta_g = math.atan2(st_y - ry, st_x - rx) delta_theta = theta_g - theta # All thetas in radians. angular_coeff = delta_theta / math.pi + 2 * ( delta_theta < -math.pi) - 2 * (delta_theta >= math.pi) linear_coeff = (1 - abs(angular_coeff))**6 # Experimental: Recalculate angular speed according to linear. angular_coeff = (1 - linear_coeff) * np.sign(angular_coeff) linear = self.MAX_LINEAR_VELOCITY * linear_coeff angular = self.MAX_ANGULAR_VELOCITY * angular_coeff assert abs(angular_coeff ) <= 1, "Angular coefficient outside of range [-1, 1]." assert 0 <= linear_coeff <= 1, "Linear coefficient outside of range [0, 1]." return linear, angular else: return 0, 0
class Navigation: # Constructor def __init__(self): # Initializations of Objects Needed self.robot_perception = RobotPerception() self.path_planning = PathPlanning() # Parameters from YAML File self.move_with_target = rospy.get_param("calculate_target") self.target_selector = rospy.get_param("target_selector") self.turtlebot_path_topic = rospy.get_param('turtlebot_path_topic') self.turtlebot_subgoals_topic = rospy.get_param( 'turtlebot_subgoals_topic') self.turtlebot_curr_target_topic = rospy.get_param( 'turtlebot_curr_target_topic') self.map_frame = rospy.get_param('map_frame') self.debug = rospy.get_param('debug') self.turtlebot_save_progress_images = rospy.get_param( 'turtlebot_save_progress_images') self.limit_exploration_flag = rospy.get_param( 'limit_exploration') # Flag to Enable/ Disable OGM Enhancement # Flag to check if the vehicle has a target or not self.target_exists = False 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 = 100 # 20 sec self.counter_to_next_sub = self.count_limit # Timer Thread (initialization) self.time_limit = 150 # in seconds self.timerThread = TimerThread(self.time_limit) # Check if subgoal is reached via a timer callback rospy.Timer(rospy.Duration(0.10), self.checkTarget) # Read the target function self.target_selection = TargetSelection(self.target_selector) # ROS Publisher for the path self.path_publisher = rospy.Publisher(self.turtlebot_path_topic, Path, queue_size=10) # ROS Publisher for the subtargets self.subtargets_publisher = rospy.Publisher( self.turtlebot_subgoals_topic, MarkerArray, queue_size=10) # ROS Publisher for the current target self.current_target_publisher = rospy.Publisher( self.turtlebot_curr_target_topic, Marker, queue_size=10) # For Testing Purpose self.timer_flag = True # True to Enable Timer for resetting Navigation # Speed Parameter self.max_linear_velocity = rospy.get_param('max_linear_speed') self.max_angular_velocity = rospy.get_param('max_angular_speed') # Parameters of OGM Enhancement self.first_run_flag = True # Flag to Control first Enhancement in OGM Map # Map Container Visualize Message self.map_size = np.array( [[rospy.get_param('x_min_size'), rospy.get_param('y_min_size')], [rospy.get_param('x_min_size'), rospy.get_param('y_max_size')], [rospy.get_param('x_max_size'), rospy.get_param('y_min_size')], [rospy.get_param('x_max_size'), rospy.get_param('y_max_size')]], dtype=np.float64) if self.limit_exploration_flag: # Create CV Bridge Object self.bridge = CvBridge() # Parameters self.drone_image = [] # Hold the Drone OGM Image converted in CV! self.ogm_in_cv = [] # Hold the OGM in CV Compatible Version self.drone_yaw = 0.0 # Holds the current Drone Yaw self.drone_map_pose = [] # Holds the current Drone Position in Map # Service self.drone_explore_limit_srv_name = rospy.get_param( 'drone_explore_limit_srv') self.limits_exploration_service = rospy.ServiceProxy( self.drone_explore_limit_srv_name, OgmLimits) # Subscriber self.drone_pub_image_topic = rospy.get_param( 'drone_pub_image_topic') self.drone_image_sub = rospy.Subscriber(self.drone_pub_image_topic, Image, self.drone_image_cb) # Flags self.done_enhancement = False # Flag for Controlling OGM Enhancement Loop self.drone_take_image = False # Flag for Controlling When to Read Image From Drone Camera # FIXME: Testing self.match_with_limit_pose = True # True/False to Match with Known Location/Template Matching self.match_with_drone_pose = False # True if you match with Drone Real Pose def checkTarget(self, event): # Check if we have a target or if the robot just wanders if not self.inner_target_exists or not self.move_with_target or self.next_subtarget == len( self.subtargets): return # Check if timer has expired if self.timer_flag: if self.timerThread.expired: Print.art_print('\n~~~~ Time reset ~~~~', Print.RED) self.inner_target_exists = False self.target_exists = False return # Get the robot pose in pixels [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 ] theta_robot = self.robot_perception.robot_pose['th'] # Clear achieved targets self.subtargets = self.subtargets[self.next_subtarget:] self.next_subtarget = 0 # If distance between the robot pose and the next subtarget is < 7 pixel consider that Target is Reached dist = math.hypot(rx - self.subtargets[self.next_subtarget][0], ry - self.subtargets[self.next_subtarget][1]) if dist < 7: 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 # 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, 0, "map", "art_next_subtarget", [0, 0, 0.8, 0.8], 0.2) # 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 # Cancel previous goal timer self.timerThread.stop() # Check if we have a map while not self.robot_perception.have_map: Print.art_print("Navigation: No map yet", Print.RED) return print "Clearing all markers !\n\n" RvizHandler.printMarker([[0, 0]], 1, 3, "map", "null", [0, 0, 0, 0], 0.1) # Visualize Map Container map_container_mark = [] for s in self.map_size: map_container_mark.append([ s[0] * self.robot_perception.resolution + self.robot_perception.origin['x'], s[1] * self.robot_perception.resolution + self.robot_perception.origin['y'] ]) RvizHandler.printMarker(map_container_mark, 1, 0, "map", "art_map_container", [1.0, 0.0, 0.0, 1.0], 0.3) print '----------------------------------------------------------' 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 ######################################################################## # Get OGM Map and Coverage start_ = time.time() # Gets copies of the map and coverage start = time.time() local_ogm = self.robot_perception.getMap() if self.debug: print str('Robot Perception: Got the map in ') + str( time.time() - start) + str(' seconds.') start = time.time() local_ros_ogm = self.robot_perception.getRosMap() if self.debug: print str('Robot Perception: Got the ros map in ') + str( time.time() - start) + str(' seconds.') start = time.time() local_coverage = self.robot_perception.getCoverage() if self.debug: print str('Robot Perception: Got the coverage in ') + str( time.time() - start) + str(' seconds.') print str('Navigation: Got the map and Coverage in ') + str( time.time() - start_) + str(' seconds.') ######################################################################## # Part 1 - OGM Enhancement no_points = False ogm_limits_before = [] if self.turtlebot_save_progress_images: ####################### Save OGM and Coverage ########################## scipy.misc.imsave( '/home/vvv/pictures_slam/ogm.png', cv2.bitwise_not( exposure.rescale_intensity(np.array(local_ogm, dtype=np.uint8), in_range=(0, 100), out_range=(0, 255)))) scipy.misc.imsave('/home/vvv/pictures_slam/coverage.png', local_coverage) start_ = time.time() print str(self.robot_perception.percentage_explored) if self.limit_exploration_flag and self.robot_perception.percentage_explored < 1.0 and not self.first_run_flag: ############################################################################# # Subpart 1 - Find the Useful boundaries of OGM ############################################################################# start = time.time() (points, ogm_limits_before) = self.ogm_limit_calculation( local_ogm, self.robot_perception.resolution, self.robot_perception.origin, 20, 20, 12, self.map_size) if points is None or len(points) == 0: print str('No Limits Points Calculated ') no_points = True else: print str('Number of Limit Points Calculated is ') + str(len(points)) + str(' in ')\ + str(time.time() - start) + str(' seconds.') no_points = False if self.debug: print str('\n') + str(points) + str('\n') ############################################################################# ######################################################################### # Subpart 2 - Send Drone to Limits and Match the to OGM Map ######################################################################### startt = time.time() if not no_points: index = 0 p = points[index, :] while len(points): points = np.delete( points, [index], axis=0) # Delete Point thats Going to be Explored # Send Drone to OGM Limits start = time.time() while not self.sent_drone_to_limit(p[0], p[1]): pass if self.debug: Print.art_print( str('Navigation: Get Image From Drone took ') + str(time.time() - start) + str(' seconds.'), Print.ORANGE) # Image Matching start = time.time() if self.match_with_limit_pose: if self.match_with_drone_pose: [x_p, y_p] = [ int((self.drone_map_pose[0] - self.robot_perception.origin['x']) / self.robot_perception.resolution), int((self.drone_map_pose[1] - self.robot_perception.origin['y']) / self.robot_perception.resolution) ] local_ogm = ImageMatching.ogm_match_with_known_image_pose( ogm=local_ogm, new_data=self.drone_image, coordinates=[x_p, y_p, self.drone_yaw], map_boundries=[ self.map_size[0][0], self.map_size[3][0], self.map_size[0][1], self.map_size[3][1] ], debug=self.debug) else: local_ogm = ImageMatching.ogm_match_with_known_image_pose( ogm=local_ogm, new_data=self.drone_image, coordinates=[ int(p[2]), int(p[3]), self.drone_yaw ], map_boundries=[ int(self.map_size[0][0]), int(self.map_size[3][0]), int(self.map_size[0][1]), int(self.map_size[3][1]) ], debug=self.debug) else: local_ogm = ImageMatching.template_matching( ogm=local_ogm, drone_image=self.drone_image, lim_x=int(p[2]), lim_y=int(p[3]), drone_yaw=self.drone_yaw, window=200, s_threshold=0.8, debug=self.debug) if self.debug: Print.art_print( str('Navigation: OGM Matching Function took ') + str(time.time() - start) + str(' seconds.\n'), Print.ORANGE) # Calculate Point for Next Loop! if len(points) > 0: index = self.closest_limit_point(p[:2], points[:, :2]) p = points[index, :] ''' print('\x1b[35;1m' + str('Navigation: Taking Image and Matching took ') + str(time.time() - startt) + str(' seconds.') + '\x1b[0m') ''' ######################################################################## ######################################################################## # Subpart 3 - -Copy Enhanced Data to ROS OGM ######################################################################## if not no_points: start = time.time() local_ros_ogm.data = cp_data_to_ros_ogm( np.array(local_ros_ogm.data), local_ogm, local_ros_ogm.info.width, local_ros_ogm.info.height) if self.debug: print('\x1b[35;1m' + str('Navigation: Copying OGM Data took ') + str(time.time() - start) + str(' seconds.') + '\x1b[0m') ######################################################################## print('\x1b[38;1m' + str('Navigation: OGM Enhancement took ') + str(time.time() - start_) + str(' seconds.') + '\x1b[0m') if self.turtlebot_save_progress_images and self.limit_exploration_flag and not no_points: ########################## Save Enhance OGM ############################ scipy.misc.imsave( '/home/vvv/pictures_slam/ogm_enhanced.png', cv2.bitwise_not( exposure.rescale_intensity(np.array(local_ogm, dtype=np.uint8), in_range=(0, 100), out_range=(0, 255)))) ######################################################################## # Part 2 - Set Map, Get Robot Position and Choose Target start = time.time() self.target_selection.path_planning.setMap(local_ros_ogm) print str('Navigation: Set Map in ') + str(time.time() - start) + str(' seconds.') # Once the target has been found, find the path to it # Get the global robot pose start = time.time() g_robot_pose = self.robot_perception.getGlobalCoordinates([ self.robot_perception.robot_pose['x_px'], self.robot_perception.robot_pose['y_px'] ]) print str('Navigation: Got Robot Pose in ') + str( time.time() - start) + str(' seconds.') # Call the target selection function to select the next best goal self.path = [] force_random = False while len(self.path) == 0: # Get Target and Path to It! start = time.time() self.path = self.target_selection.selectTarget( local_ogm, local_coverage, self.robot_perception.robot_pose, self.robot_perception.origin, self.robot_perception.resolution, g_robot_pose, ogm_limits_before, force_random) #self.target_selection.path_planning.createPath(g_robot_pose, target, self.robot_perception.resolution) if len(self.path) == 0: Print.art_print( 'Navigation: Path planning failed. Fallback to random target selection', Print.RED) print str('\n') force_random = True else: print str('Navigation: Target Selected and Path to Target found with ') + str(len(self.path)) \ + str(' points in ') + str(time.time() - start) + str(' seconds.') ######################################################################## # Part 3 - Create Subgoals and Print Path to RViz start = time.time() # Reverse the path to start from the robot self.path = self.path[::-1] # Break the path to subgoals 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 if self.debug: print str('Navigation: The path produced ') + str(len(self.subtargets)) + str(' subgoals in ') \ + str(time.time() - start) + str('seconds.') # Start timer thread self.timerThread.start() # Publish the path for visualization purposes ros_path = Path() ros_path.header.frame_id = self.map_frame for p in self.path: ps = PoseStamped() ps.header.frame_id = self.map_frame ps.pose.position.x = 0 ps.pose.position.y = 0 ps.pose.position.x = p[ 0] * self.robot_perception.resolution + self.robot_perception.origin[ 'x'] ps.pose.position.y = p[ 1] * self.robot_perception.resolution + self.robot_perception.origin[ 'y'] ros_path.poses.append(ps) self.path_publisher.publish(ros_path) # Publish the subtargets for visualization purposes subtargets_mark = [] for s in self.subtargets: subtargets_mark.append([ s[0] * self.robot_perception.resolution + self.robot_perception.origin['x'], s[1] * self.robot_perception.resolution + self.robot_perception.origin['y'] ]) RvizHandler.printMarker(subtargets_mark, 2, 0, 'map', 'art_subtargets', [0, 0.8, 0.0, 0.8], 0.2) # Update NUmber of Targets self.robot_perception.num_of_explored_targets += 1 self.inner_target_exists = True # Reset First Run Flag if self.first_run_flag: self.first_run_flag = False # Function that calculates Velocity to next Target def velocitiesToNextSubtarget(self): # Initialize Speeds [linear, angular] = [0, 0] # Get Robot Position in Map [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] theta = self.robot_perception.robot_pose['th'] # If Target Exists Calculate Speeds if self.subtargets and self.next_subtarget <= len(self.subtargets) - 1: # Calculate dx, dy, dth st_x = self.subtargets[self.next_subtarget][0] st_y = self.subtargets[self.next_subtarget][1] th_rg = np.arctan2(st_y - ry, st_x - rx) dth = (th_rg - theta) if dth > np.pi: omega = (dth - 2 * np.pi) / np.pi elif dth < -np.pi: omega = (dth + 2 * np.pi) / np.pi else: omega = (th_rg - theta) / np.pi # Nonlinear Relations Derived from Experimentation linear = self.max_linear_velocity * ((1 - np.abs(omega))**5) angular = self.max_angular_velocity * np.sign(omega) * (abs(omega) **(1 / 5)) ''' new_theta = atan2(st_y - ry, st_x - rx) if new_theta <= -3: new_theta = -new_theta if theta <= -3: theta = -theta delta_theta = new_theta - theta if abs(delta_theta) > 3.15: delta_theta = -0.2 * np.sign(delta_theta) # Assign Speeds Accordingly if abs(delta_theta) < 0.1: linear = self.max_linear_velocity angular = 0 else: linear = 0 angular = self.max_angular_velocity*np.sign(delta_theta) ''' # Return Speeds return [linear, angular] # Send SIGNAL to Drone to Explore Limit Function def sent_drone_to_limit(self, x, y): # Set Flag to Make Drone Image to be Received self.drone_take_image = True # Create Request with Points in Map of OGM Limits request = OgmLimitsRequest() request.x = x request.y = y # Call Service rospy.wait_for_service(self.drone_explore_limit_srv_name) try: response = self.limits_exploration_service(request) except rospy.ServiceException, e: print "Service call failed: %s" % e # Wait for Drone Image to be Received to Return while self.drone_take_image: pass # Return Once Image has been Received return response
class Navigation: # Constructor def __init__(self): # Initializations self.robot_perception = RobotPerception() self.target_selection = TargetSelection() 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.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 # Check if subgoal is reached via a timer callback rospy.Timer(rospy.Duration(0.10), self.checkTarget) # 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) def checkTarget(self, event): # 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 # Get the robot pose in pixels [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\ ] # YOUR CODE HERE ------------------------------------------------------ # Here the next subtarget is checked for proximity. If the robot is too # close to the subtarget it is considered approached and the next # subtarget is assigned as next. However there are cases where the # robot misses one subtarget due to obstacle detection. Enhance the below # functionality by checking all the subgoals (and the final goal) for # proximity and assign the proper next subgoal # Find the distance between the robot pose and the next subtarget dist = math.hypot(\ rx - self.subtargets[self.next_subtarget][0], \ ry - self.subtargets[self.next_subtarget][1]) # Check if distance is less than 7 px (14 cm) if dist < 7: print "Sub target reached!" self.next_subtarget += 1 # Check if the final subtarget has been approached if self.next_subtarget == len(self.subtargets): print "Final goal reached!" self.target_exists = False # --------------------------------------------------------------------- # Publish the current target if self.next_subtarget == len(self.subtargets): return st = Marker() st.header.frame_id = "map" st.ns = 'as_curr_namespace' st.id = 0 st.header.stamp = rospy.Time(0) st.type = 1 # arrow st.action = 0 # add st.pose.position.x = self.subtargets[self.next_subtarget][0]\ * self.robot_perception.resolution + \ self.robot_perception.origin['x'] st.pose.position.y = self.subtargets[self.next_subtarget][1]\ * self.robot_perception.resolution + \ self.robot_perception.origin['y'] st.color.r = 0 st.color.g = 0 st.color.b = 0.8 st.color.a = 0.8 st.scale.x = 0.2 st.scale.y = 0.2 st.scale.z = 0.2 self.current_target_publisher.publish(st) # Function that seelects 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 self.robot_perception.have_map == False: return 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 # Manually update the coverage field self.robot_perception.updateCoverage() print "Navigation: Coverage updated" # Gets copies of the map and coverage local_ogm = self.robot_perception.getMap() local_coverage = self.robot_perception.getCoverage() print "Got the map and Coverage" # Call the target selection function to select the next best goal target = self.target_selection.selectTarget(\ local_ogm,\ local_coverage,\ self.robot_perception.robot_pose) print "Navigation: New target: " + str(target) # Once the target has been found, find the path to it # Get the global robot pose g_robot_pose = self.robot_perception.getGlobalCoordinates(\ [self.robot_perception.robot_pose['x_px'],\ self.robot_perception.robot_pose['y_px']]) # Need to reverse the path?? self.path = self.path_planning.createPath(\ local_ogm,\ g_robot_pose,\ target) print "Navigation: Path for target found with " + str(len(self.path)) +\ " points" # Reverse the path to start from the robot self.path = self.path[::-1] # Break the path to subgoals every 10 pixels (0.5m) step = 10 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" # 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" ps.pose.position.x = p[0] * self.robot_perception.resolution + \ self.robot_perception.origin['x'] ps.pose.position.y = p[1] * self.robot_perception.resolution + \ self.robot_perception.origin['y'] ros_path.poses.append(ps) self.path_publisher.publish(ros_path) # Publish the subtargets for visualization purposes ros_subgoals = MarkerArray() count = 0 for s in self.subtargets: st = Marker() st.header.frame_id = "map" st.ns = 'as_namespace' st.id = count st.header.stamp = rospy.Time(0) st.type = 2 # sphere st.action = 0 # add st.pose.position.x = s[0] * self.robot_perception.resolution + \ self.robot_perception.origin['x'] st.pose.position.y = s[1] * self.robot_perception.resolution + \ self.robot_perception.origin['y'] st.color.r = 0 st.color.g = 0.8 st.color.b = 0 st.color.a = 0.8 st.scale.x = 0.2 st.scale.y = 0.2 st.scale.z = 0.2 ros_subgoals.markers.append(st) count += 1 self.subtargets_publisher.publish(ros_subgoals) self.inner_target_exists = True def velocitiesToNextSubtarget(self): [linear, angular] = [0, 0] # YOUR CODE HERE ------------------------------------------------------ # 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 # --------------------------------------------------------------------- return [linear, angular]
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 self.TimeOut = False self.Reset = 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 = 230 # 23 sec self.counter_to_next_sub = self.count_limit # Check if subgoal is reached via a timer callback rospy.Timer(rospy.Duration(0.1), 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) def checkTarget(self, event): # 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 if self.Reset == False: self.TimeOut = True else: self.Reset = False return # Get the robot pose in pixels [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\ ] # remove visited sub targets self.subtargets = self.subtargets[self.next_subtarget:] # set the next sub target as the first one that should be visited self.next_subtarget = 0 dis_x = [rx - target[0] for target in self.subtargets] dis_y = [ry - target[1] for target in self.subtargets] #for i in range(0,len(dis_x)): # print " x dis " + str(dis_x[i]) # print " y dis " + str(dis_y[i]) # target distances dist_t = [math.hypot(dis[0],dis[1]) for dis in zip(dis_x,dis_y) ] # find the closest target min_target = 0 min_dis = dist_t[0] for i in range(0,len(dist_t)): if min_dis > dist_t[i]: min_dis = dist_t[i] min_target = i ######################### NOTE: QUESTION ############################## # What if a later subtarget or the end has been reached before the # next subtarget? Alter the code accordingly. # print " min target " + str(min_target) + " min dis " + str(min_dis) # Check if distance is less than 7 px (14 cm) if min_dis < 5: self.next_subtarget = min_target + 1 self.counter_to_next_sub = self.count_limit # print "REACHED" # print " next target " + str(self.next_subtarget) + " len " +str(len(self.subtargets)) # Check if the final subtarget has been approached 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 self.robot_perception.have_map == False: 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 g_robot_pose = self.robot_perception.getGlobalCoordinates(\ [self.robot_perception.robot_pose['x_px'],\ self.robot_perception.robot_pose['y_px']]) # Call the target selection function to select the next best goal # Choose target function print "TimeOut Flag " +str(self.TimeOut) 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] tolerance = 0.000001 weight_data = 0.5 weight_smooth = 0.1 new = deepcopy(self.path) dims = 2 change = tolerance # Path smoothing using gradient descent if len(self.path) > 3: print "PATH SMOOTHING" while change >= tolerance: change = 0.0 for i in range(1, len(new) - 1): for j in range(dims): x_i = self.path[i][j] y_i, y_prev, y_next = new[i][j], new[i - 1][j], new[i + 1][j] y_i_saved = y_i y_i += weight_data * (x_i - y_i) + weight_smooth * (y_next + y_prev - (2 * y_i)) new[i][j] = y_i change += abs(y_i - y_i_saved) self.path = new # Break the path to subgoals 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. #p[0] * resolution => gives the path's point x-coordinate expressed in the map's coordinate system (meters) # As the path's(p) first point-set is the pixels that are occupied by the robot, multyplying it by resolution (meter/pixel) # gives the values of that point expressed in meters. # Adding the robot's initial position we have a path starting from the robot's position expressed in the robot's frame. ps.pose.position.x = p[0]*0.05 + self.robot_perception.origin['x'] ps.pose.position.y = p[1]*0.05 + self.robot_perception.origin['y'] # print " x val " + str(ps.pose.position.x) # print " y val " + str(ps.pose.position.y ) # print " origin x " + str(self.robot_perception.origin['x']) # print " origin y " + str(self.robot_perception.origin['y']) # print " p0 " +str(p[0]) # print " p1 " +str(p[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: subt = [ s[0] * self.robot_perception.resolution + \ self.robot_perception.origin['x'], s[1] * self.robot_perception.resolution + \ self.robot_perception.origin['y'] ] subtargets_mark.append(subt) 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] [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\ ] theta = self.robot_perception.robot_pose['th'] ######################### 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 there is another subtarget , acquire its position if self.subtargets and self.next_subtarget <= len(self.subtargets) - 1: st_x = self.subtargets[self.next_subtarget][0] st_y = self.subtargets[self.next_subtarget][1] # align robot with the target theta_target = np.arctan2(st_y - ry, st_x - rx) delta_theta = (theta_target - theta) # find omega if delta_theta > np.pi: omega = (delta_theta - 2*np.pi)/np.pi elif delta_theta < -np.pi: omega = (delta_theta + 2*np.pi)/np.pi else: omega = delta_theta/np.pi # maximum magnitude for both velocities is 0.3 linear = 0.3 * ((1 - np.abs(omega)) ** 5) angular = 0.3 * np.sign(omega) ######################### NOTE: QUESTION ############################## return [linear, angular]
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 = 200 # 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) def checkTarget(self, event): # 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 pixels [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\ ] # Clear visited subtargets self.subtargets = self.subtargets[self.next_subtarget:] self.next_subtarget = 0 # Calculate distance between the robot pose and all remaining subtargets dx = [rx - st[0] for st in self.subtargets] dy = [ry - st[1] for st in self.subtargets] dist = [math.hypot(v[0], v[1]) for v in zip(dx, dy)] # Check if any target is in distance min_dist, min_idx = min(zip(dist, range(len(dist)))) ######################### 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 min_dist < 5: self.next_subtarget = min_idx + 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 ######################################################################## # 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 self.robot_perception.have_map == False: 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 g_robot_pose = self.robot_perception.getGlobalCoordinates(\ [self.robot_perception.robot_pose['x_px'],\ self.robot_perception.robot_pose['y_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_ros_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] # Smooth path if len(self.path) > 3: x = np.array(self.path) y = np.copy(x) a = 0.5 b = 0.2 e = np.sum( np.abs(a * (x[1:-1, :] - y[1:-1, :]) + b * (y[2:, :] - 2 * y[1:-1, :] + y[:-2, :]))) while e > 1e-3: y[1:-1, :] += a * (x[1:-1, :] - y[1:-1, :]) + b * ( y[2:, :] - 2 * y[1:-1, :] + y[:-2, :]) e = np.sum( np.abs(a * (x[1:-1, :] - y[1:-1, :]) + b * (y[2:, :] - 2 * y[1:-1, :] + y[:-2, :]))) self.path = y.tolist() # Break the path to subgoals 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 (answer is just above) # 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. ps.pose.position.x = p[ 0] * self.robot_perception.resolution + self.robot_perception.origin[ 'x'] ps.pose.position.y = p[ 1] * self.robot_perception.resolution + self.robot_perception.origin[ 'y'] ######################################################################## ros_path.poses.append(ps) self.path_publisher.publish(ros_path) # Publish the subtargets for visualization purposes subtargets_mark = [] for s in self.subtargets: subt = [ s[0] * self.robot_perception.resolution + \ self.robot_perception.origin['x'], s[1] * self.robot_perception.resolution + \ self.robot_perception.origin['y'] ] subtargets_mark.append(subt) 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] [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\ ] theta = self.robot_perception.robot_pose['th'] ######################### 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 l_max = 0.3 a_max = 0.3 if self.subtargets and self.next_subtarget <= len(self.subtargets) - 1: st_x = self.subtargets[self.next_subtarget][0] st_y = self.subtargets[self.next_subtarget][1] # Fix robot turning problem theta_r = np.arctan2(st_y - ry, st_x - rx) dtheta = (theta_r - theta) if dtheta > np.pi: omega = (dtheta - 2 * np.pi) / np.pi elif dtheta < -np.pi: omega = (dtheta + 2 * np.pi) / np.pi else: omega = (theta_r - theta) / np.pi # Velocities in [-0.3, 0.3] range linear = l_max * ((1 - np.abs(omega))**4) angular = a_max * np.sign(omega) * (abs(omega)**(1 / 3)) # omega = (theta_r - theta)/np.pi # angular = omega * 0.3 # max angular = 0.3 rad/s # linear_r = 1 - np.abs(omega) #may need square # linear = linear_r * 0.3 # max linear = 0.3 m/s ######################### NOTE: QUESTION ############################## return [linear, angular]
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 = 200 # 20 sec self.counter_to_next_sub = self.count_limit self.laser_aggregation = LaserDataAggregator() # 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) def checkTarget(self, event): # 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 pixels [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\ ] # Find the distance between the robot pose and the next subtarget dist = math.hypot(\ rx - self.subtargets[self.next_subtarget][0], \ ry - self.subtargets[self.next_subtarget][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) count = 1 for i in range( self.next_subtarget, len(self.subtargets) - 2 ): # compare the slope of the points till the end in order to check if there are more than one points at the same line try: # use try except to avoid zerodivisior error slope1 = (self.subtargets[i + 1][1] - self.subtargets[i][1]) / (self.subtargets[i + 1][0] - self.subtargets[i][0]) except ZeroDivisionError: slope1 = float('Inf') try: slope2 = (self.subtargets[i + 2][1] - self.subtargets[i + 1][1] ) / (self.subtargets[i + 2][0] - self.subtargets[i + 1][0]) except ZeroDivisionError: slope2 = float('Inf') if abs(slope1 - slope2) < 0.3 or ( slope1 == float('inf') and slope2 == float('inf') ): # if difference in slopes is less than 0.3 then we assume that the points are in the same line count += 1 else: break scan = self.laser_aggregation.laser_scan # take scan measurements count_scan = 0 for i in range( 250, 450 ): # count scanlines with measure more than 2. The range has calculated after experiments. if scan[i] > 2: count_scan += 1 if dist < 5: if count != 1: self.next_subtarget += count # if there are more than one subtargets that belongs to the same line then the robot goes to the last target of the line self.counter_to_next_sub = self.count_limit + count * 100 # increase time limit proportionally to the count in order to avoid time reset elif count_scan > 10: # check if obstacle closer than 4 cm exists, if obstacle does not exist then self.next_subtarget += 2 # increase subtarget by 2. This case happens when count = 1 in order to accelerate the process in case of not having subtargets in the same line self.counter_to_next_sub = self.count_limit + 300 # increase time limit in order to avoid time reset if self.next_subtarget >= len( self.subtargets ): # check if the final subtarget has been approached self.next_subtarget = len(self.subtargets) else: self.next_subtarget += 1 # else if obstacle exists then increase subtarget by 1 self.counter_to_next_sub = self.count_limit + 300 # increase time limit in order to avoid time reset if self.next_subtarget >= len( self.subtargets ): # check if the final subtarget has been approached self.next_subtarget = len(self.subtargets) 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 self.robot_perception.have_map == False: 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 g_robot_pose = self.robot_perception.getGlobalCoordinates(\ [self.robot_perception.robot_pose['x_px'],\ self.robot_perception.robot_pose['y_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 subgoals 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" ps.pose.position.x = 0 ps.pose.position.y = 0 ######################### 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. # p[0] and p[1] are the global coordinates so we multiply these values with resolution and add the origin ps.pose.position.x = p[ 0] * self.robot_perception.resolution + self.robot_perception.origin[ 'x'] ps.pose.position.y = p[ 1] * self.robot_perception.resolution + self.robot_perception.origin[ 'y'] ######################################################################## ros_path.poses.append(ps) self.path_publisher.publish(ros_path) # Publish the subtargets for visualization purposes subtargets_mark = [] for s in self.subtargets: subt = [ s[0] * self.robot_perception.resolution + \ self.robot_perception.origin['x'], s[1] * self.robot_perception.resolution + \ self.robot_perception.origin['y'] ] subtargets_mark.append(subt) 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] [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\ ] theta = self.robot_perception.robot_pose['th'] ######################### 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_x = self.subtargets[self.next_subtarget][0] st_y = self.subtargets[self.next_subtarget][1] ######################### NOTE: QUESTION ############################## # theta contains yaw angle, hence we convert it to x,y coordinates in order to find the angle in degrees x = math.cos(theta) y = math.sin(theta) deg = math.degrees(math.atan2(y, x)) delta_theta = math.degrees(math.atan2(st_y - ry, st_x - rx)) - deg if delta_theta >= 0 and delta_theta < 180: angular = delta_theta / 180 elif delta_theta > 0 and delta_theta >= 180: angular = (delta_theta - 2 * 180) / 180 elif delta_theta <= 0 and delta_theta > -180: angular = delta_theta / 180 elif delta_theta < 0 and delta_theta < -180: angular = (delta_theta + 2 * 180) / 180 linear = ( (1 - abs(angular))**25 ) # power calculated after experiments and we use it in order to avoid overshoot problem linear = linear * 0.3 # use tanh to limit the range if angular > 0.3: # angular = angular * 0.3. angular velocity should be multiplied by max angular velocity according to notes angular = 0.3 elif angular < -0.3: angular = -0.3 # but the result was much slower so we didn't multiply it by 0.3 ######################### NOTE: QUESTION ############################## return [linear, angular]