| |
|---|
| | ## 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 |
|---|
| | ## for debug |
|---|
| | # self.scan_pub = rospy.Publisher("/debug_scan", LaserScan, queue_size=10) |
|---|
| | return |
|---|
| | |
|---|
| | |
|---|
| | ## Subscribe current waypoint number |
|---|
| |
|---|
| | 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(10) / 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 |
|---|
| | |
|---|
| | sort_ranges = np.sort(msg.ranges[self.front_range[0]:self.front_range[1]]) |
|---|
| | min_range = np.mean(sort_ranges[:5]) |
|---|
| | 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) |
|---|
| | ## 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 |
|---|
| | rospy.loginfo("Stop because of obstacle within {}m ahead.".format(self.danger_dist)) |
|---|
| |
|---|
| | elif (self.stop) and (min_range >= self.danger_dist+0.1): |
|---|
| | self.resume_nav() |
|---|
| | self.stop = False |
|---|
| | rospy.loginfo("Resumed navigation because the obstacle ahead was more than {}m away.".format(self.danger_dist+0.1)) |
|---|
| | |
|---|
| | ## for debug |
|---|
| | # debug_scan = msg |
|---|
| | # debug_scan.ranges = np.array(msg.ranges) |
|---|
| | # debug_scan.ranges[:self.front_range[0]] = 0 |
|---|
| | # debug_scan.ranges[self.front_range[1]:] = 0 |
|---|
| | # debug_scan.ranges = tuple(debug_scan.ranges) |
|---|
| | # self.scan_pub.publish(debug_scan) |
|---|
| | |
|---|
| | except AttributeError: |
|---|
| | pass |
|---|
| | return |
|---|
| | |
|---|
| |
|---|
| | |
