#!/usr/bin/env python3 import rospy import ruamel.yaml import numpy as np from std_msgs.msg import UInt16 from sensor_msgs.msg import LaserScan from std_srvs.srv import Trigger class TandemManager(): def __init__(self): yaml = ruamel.yaml.YAML() waypoints_path = rospy.get_param("tandem_run_manager/waypoints_file") self.tandem_start_list = [] self.tandem_end_list = [] self.tandem_id = 0 with open(waypoints_path) as file: waypoints_yaml = yaml.load(file) for i, data in enumerate(waypoints_yaml["waypoints"]): if ("tandem_start" in data["point"]): self.tandem_start_list.append(i+2) if ("tandem_end" in data["point"]): self.tandem_end_list.append(i+2) self.in_tandem_area = False self.stop = False ## self.waypoint_num = 0 self.wp_num_sub = rospy.Subscriber("/waypoint_num", UInt16, self.waypoint_num_callback) self.scan_sub = rospy.Subscriber("/scan", LaserScan, self.laserscan_callback) ## self.stop_nav = rospy.ServiceProxy("/stop_wp_nav", Trigger) self.resume_nav = rospy.ServiceProxy("/resume_nav", Trigger) ## self.front_range = None self.danger_dist = 1.0 ## for debug self.scan_pub = rospy.Publisher("/debug_scan", LaserScan, queue_size=10) return def waypoint_num_callback(self, msg): if (msg.data == self.waypoint_num) or (self.stop): return if (msg.data == self.tandem_start_list[self.tandem_id]): self.in_tandem_area = True rospy.loginfo("Enter tandem area.") elif (msg.data == self.tandem_end_list[self.tandem_id]): self.in_tandem_area = False self.tandem_id += 1 self.stop = (self.tandem_id >= len(self.tandem_start_list)) # if True, self.stop will never be False rospy.loginfo("Exit from tandem area.") self.waypoint_num = msg.data return def laserscan_callback(self, msg): if self.front_range is None: front = round(-msg.angle_min / msg.angle_increment) ran = int(round(np.deg2rad(5) / msg.angle_increment)) self.front_range = [front-ran, front+ran] return ## if not self.in_tandem_area: return #ave_range = np.mean(msg.ranges[self.front_range[0]:self.front_range[1]]) min_range = np.min(msg.ranges[self.front_range[0]:self.front_range[1]]) rospy.loginfo(min_range) 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) return if __name__ == '__main__': rospy.init_node("map_changer") tandem_manager = TandemManager() rospy.spin()