diff --git a/waypoint_nav/scripts/tandem_run_manager.py b/waypoint_nav/scripts/tandem_run_manager.py index 85cbf06..c402e00 100755 --- a/waypoint_nav/scripts/tandem_run_manager.py +++ b/waypoint_nav/scripts/tandem_run_manager.py @@ -36,9 +36,10 @@ self.costmap_client1 = dynamic_reconfigure.client.Client("/move_base/global_costmap/obstacle_layer1") self.costmap_client2 = dynamic_reconfigure.client.Client("/move_base/global_costmap/obstacle_layer2") ## Variable + self.front_angle = 40 # degree + self.danger_dist = 1.0 # meter self.waypoint_num = 0 self.front_range = None - self.danger_dist = 1.0 self.in_tandem_area = False self.stop = False return @@ -70,15 +71,17 @@ try: if self.front_range is None: front = round(-msg.angle_min / msg.angle_increment) - ran = int(round(np.deg2rad(20) / msg.angle_increment)) + ran = int(round(np.deg2rad(self.front_angle/2) / msg.angle_increment)) self.front_range = [front-ran, front+ran] return if not self.in_tandem_area: return ranges = np.array(msg.ranges[self.front_range[0]:self.front_range[1]]) + ## Use simply minimum ranges[ranges <= msg.range_min] = msg.range_max min_range = min(ranges) - #sort_ranges = np.sort(msg.ranges[self.front_range[0]:self.front_range[1]]) + ## or use sort + #sort_ranges = np.sort(ranges) #min_range = np.mean(sort_ranges[:5]) if (not self.stop) and (min_range < self.danger_dist): self.stop_nav()