| |
|---|
| | ## Dynamic reconfigure clients |
|---|
| | 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 |
|---|
| | |
|---|
| |
|---|
| | def laserscan_callback(self, msg): |
|---|
| | 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() |
|---|
| | self.stop = True |
|---|
| |
|---|
| | |
