def __init__(self):
# Define Adjustable Parameters
self.file_location = rospy.get_param("~file_location")
self.speed_limit = float(rospy.get_param("~speed_limit"))
# Internal USE Variables - Do not modify without consultation
self.mode = 1
self.front_increment = 0.0 # [m]
self.new_timestamp = 0
self.last_timestamp = 1
self.region_file = self.file_location +"/detection_region.yaml"
self.outer_zone = ShapelyPolygon()
self.inner_zone = ShapelyPolygon()
self.checker_rate = rospy.Rate(5)
self.cluster_tolerance = 0.05 # [m]
self.counter = 0
self.speed_type = 0
# Publishers
self.drive_pub = rospy.Publisher(rospy.get_param("~drive_topic"), Twist, queue_size=1)
self.viz_inner_region_pub = rospy.Publisher(rospy.get_param("~viz_inner_region_topic"), PolygonStamped, queue_size=1)
self.viz_outer_region_pub = rospy.Publisher(rospy.get_param("~viz_outer_region_topic"), PolygonStamped, queue_size=1)
self.debug_pub = rospy.Publisher("obstacle_stop/debug", String, queue_size=1)
# Subscribers
self.obstacle_mode_sub = rospy.Subscriber(rospy.get_param("~obstacle_mode_topic"), String, self.obstacle_modeCB, queue_size=1)
self.speed_sub = rospy.Subscriber(rospy.get_param("~speed_topic"), Twist, self.speedCB, queue_size=1)
self.scan_sub = rospy.Subscriber(rospy.get_param("~scan_topic"), LaserScan, self.scanCB, queue_size=1)
# Lidar Health Checker Loop
self.lidar_health_checker()
def getPointsGroupedFromCloud(self, cloud, outer_region, inner_region):
outer_pts = []
inner_pts = []
outer_polygon = ShapelyPolygon(outer_region)
inner_polygon = ShapelyPolygon(inner_region)
for pt in cloud:
point = ShapelyPoint(pt[0], pt[1])
if (outer_polygon.contains(point)):
outer_pts.append(Point32(pt[0], pt[1], 0))
if (inner_polygon.contains(point)):
inner_pts.append(Point32(pt[0], pt[1], 0))
return outer_pts, inner_pts
def __init__(self):
# Define Adjustable Parameters
self.file_location = rospy.get_param("~file_location")
self.speed_limit = float(rospy.get_param("~speed_limit"))
# Internal USE Variables - Do not modify without consultation
self.ch_params = 1
#set mode to see the changes or set send mode by publisher
self.mode="manual"
self.data="manual"
self.err_state=""
self.err=1
self.front_increment = 0.0 # [m]
self.new_timestamp = 0
self.last_timestamp = 1
self.region_file = "/home/atlas80evo/catkin_ws/src/obstacle_stop/config/detection_region.yaml"
self.outer_zone = ShapelyPolygon()
self.inner_zone = ShapelyPolygon()
self.front_region_zone=ShapelyPolygon()
self.inner_region_slow_speed_zone=ShapelyPolygon()
self.inner_region_top_speed_zone=ShapelyPolygon()
self.checker_rate = rospy.Rate(5)
self.cluster_tolerance = 0.05 # [m]
self.counter = 0
self.speed_type = 0
self.lifter_status = 0
self.prestate=""
self.pre_timer=rospy.Time.now().to_sec()
self.pre_timer_error=rospy.Time.now().to_sec()
self.health_param=0
self.pre_state=""
print("started")
# Publishers
self.drive_pub = rospy.Publisher(rospy.get_param("~drive_topic"), Twist, queue_size=1)
self.viz_inner_region_pub = rospy.Publisher(rospy.get_param("~viz_inner_region_topic"), PolygonStamped, queue_size=1)
self.viz_outer_region_pub = rospy.Publisher(rospy.get_param("~viz_outer_region_topic"), PolygonStamped, queue_size=1)
self.debug_pub = rospy.Publisher("obstacle_stop/debug", String, queue_size=1)
# Subscribers
self.lifter_sub = rospy.Subscriber(rospy.get_param("~lifter_call","lifter/status"), String, self.lifterCB, queue_size=1)
self.obstacle_mode_sub = rospy.Subscriber(rospy.get_param("~fsm_node","/fsm_node/state"), FSMState, self.obstacle_modeCB, queue_size=1)
self.speed_sub = rospy.Subscriber(rospy.get_param("~speed_topic"), Twist, self.speedCB, queue_size=1)
self.scan_sub = rospy.Subscriber(rospy.get_param("~scan_topic"), LaserScan, self.scanCB, queue_size=1)
#service server
self.update_parameters=rospy.Service("update_params", Empty, self.update_params)
self.health_obs= rospy.ServiceProxy("/health/obstacle",Empty)
self.img_send=rospy.ServiceProxy("/screenshot",Empty)
#load the yml file
self.send_error()
self.update_params()
def getPointsInRegionFromPoints(self, pts, front_region):
front_pts = []
front_polygon = ShapelyPolygon(front_region)
for p in pts:
if (front_polygon.contains(p)):
front_pts.append(p)
return front_pts
def __init__(self):
# Define Adjustable Parameters
self.pt_1 = Point(rospy.get_param("~pt_1_x"), rospy.get_param("~pt_1_y"), 0.0)
self.pt_1_heading = rospy.get_param("~pt_1_heading")
self.pt_2 = Point(rospy.get_param("~pt_2_x"), rospy.get_param("~pt_2_y"), 0.0)
self.pt_2_heading = rospy.get_param("~pt_2_heading")
self.pt_3 = Point(rospy.get_param("~pt_3_x"), rospy.get_param("~pt_3_y"), 0.0)
self.pt_3_heading = rospy.get_param("~pt_3_heading")
self.pt_4 = Point(rospy.get_param("~pt_4_x"), rospy.get_param("~pt_4_y"), 0.0)
self.pt_4_heading = rospy.get_param("~pt_4_heading")
self.pt_5 = Point(rospy.get_param("~pt_5_x"), rospy.get_param("~pt_5_y"), 0.0)
self.pt_5_heading = rospy.get_param("~pt_5_heading")
self.out_pt = Point(rospy.get_param("~out_pt_x"), rospy.get_param("~out_pt_y"), 0.0)
self.out_pt_heading = rospy.get_param("~out_pt_heading")
self.exit_pt = Point(rospy.get_param("~exit_pt_x"), rospy.get_param("~exit_pt_y"), 0.0)
self.exit_pt_heading = rospy.get_param("~exit_pt_heading")
self.bay_polygon = ShapelyPolygon([(rospy.get_param("~bay_1_x"), rospy.get_param("~bay_1_y")), (rospy.get_param("~bay_2_x"), rospy.get_param("~bay_2_y")), (rospy.get_param("~bay_3_x"), rospy.get_param("~bay_3_y")), (rospy.get_param("~bay_4_x"), rospy.get_param("~bay_4_y"))]) # Only available in 2D
# Internal Use Variables - Do not modify without consultation
self.refresh_rate = rospy.Rate(30) # 30 [hz] <--> 0.033 [sec]
self.mag_data = None
self.k = 0
self.command = []
self.tmp_command = []
self.mag_on = False # use only between starting and ending
self.end = False # use to identify whether currently is starting / ending
self.once = False # use to make sure the crossroad detected repeatedly won't be counted
self.last_msg = ""
self.goal_pose = Point()
self.tmp_pose = Point()
self.turn_counter = 0
self.turn = ""
self.step_counter = 0 # use for check whether currently should be "1 - exit mode" or "2 - bay mode"
self.position = Point()
self.yaw = 0
self.direction = ""
self.iters = 0
self.abort_iters = 0
# Publishers
self.notify_pub = rospy.Publisher(rospy.get_param("~notify_topic"), String, queue_size=1)
self.drive_pub = rospy.Publisher(rospy.get_param("~drive_topic"), Twist, queue_size=1)
self.init_90_turn_pub = rospy.Publisher(rospy.get_param("~init_90_turn_topic"), String, queue_size=1)
self.relocalize_pub = rospy.Publisher(rospy.get_param("~relocalize_topic"), PoseWithCovarianceStamped, queue_size=1)
self.finish_mag_pub = rospy.Publisher(rospy.get_param("~finish_mag_topic"), String, queue_size=1)
self.check_drive_pub = rospy.Publisher(rospy.get_param("~check_drive_topic"), Bool, queue_size=1)
self.debug_pub = rospy.Publisher("/mag_navi/debug", String, queue_size=1)
# Subscribers
self.init_mag_sub = rospy.Subscriber(rospy.get_param("~init_mag_topic"), String, self.init_magCB, queue_size=1)
self.pose_sub = rospy.Subscriber(rospy.get_param("~pose_topic"), Odometry, self.poseCB, queue_size=1)
self.mag_sub = rospy.Subscriber(rospy.get_param("~mag_topic"), String, self.magCB, queue_size=1)
self.mag_loop()
def getPointsInRegionFromClusters(self, clusters, cloud, detection_region):
detected_pts = []
detection_polygon = ShapelyPolygon(detection_region)
if (len(clusters) != 0):
for cluster in clusters:
for pt_id, pt in enumerate(cluster):
point = ShapelyPoint(cloud[pt][0], cloud[pt][1])
if (detection_polygon.contains(point)):
detected_pts.append(point)
if (len(detected_pts) > 10):
return detected_pts
return detected_pts
def load_params_from_yaml(self, filepath):
inner_region = []
fast_outer_region = []
slow_outer_region = []
outer_region = []
with open(filepath, 'r') as infile:
data = yaml.load(infile)
for p in data["outer_region"]["fast"]["points"]:
fast_outer_region.append((p["x"], p["y"]))
for p in data["outer_region"]["slow"]["points"]:
slow_outer_region.append((p["x"], p["y"]))
if(self.speed_type == 1):
self.outer_zone = ShapelyPolygon(fast_outer_region)
outer_region = fast_outer_region
else:
self.outer_zone = ShapelyPolygon(slow_outer_region)
outer_region = slow_outer_region
for p in data["inner_region"]["points"]:
inner_region.append((p["x"], p["y"]))
self.inner_zone = ShapelyPolygon(inner_region)
self.visualize_region(outer_region, inner_region)
def load_points():
with open("points_of_interest.json") as file:
points_of_interest = json.load(file)
bounding_box = Polygon(*points_of_interest["bounding_box"])
# push front points forward
for index in range(4, 10):
bounding_box.points[index][0] += 0.05
bounding_box.order_points()
bounding_box_max = bounding_box.with_polygon(bounding_box)
bounding_box_min = bounding_box.with_polygon(bounding_box)
# bounding_box_max.scale(1.2, 1.2)
bounding_box_max.offset(0.02, 0)
bounding_box_max_points = np.array(bounding_box_max.points)
bounding_box_min_points = np.array(bounding_box_min.points)
# theta = np.radians(180)
# c, s = np.cos(theta), np.sin(theta)
# rotation_mat = np.array([[c, -s], [s, c]])
#
# bounding_box_max_points = bounding_box_max_points.dot(rotation_mat)
# bounding_box_min_points = bounding_box_min_points.dot(rotation_mat)
shapely_bounding_box_max = ShapelyPolygon(bounding_box_max_points)
# shapely_bounding_box_min = ShapelyPolygon(bounding_box_max_points)
plt.plot(0, 0, 'ko')
plt.plot(bounding_box_max_points[:, 0], bounding_box_max_points[:, 1],
'g.')
plt.plot(bounding_box_min_points[:, 0], bounding_box_min_points[:, 1], '.')
plt.gca().add_patch(
patches.Polygon(bounding_box_max_points, closed=True, fill=False))
plt.gca().add_patch(
patches.Polygon(bounding_box_min_points, closed=True, fill=False))
# test_points = np.random.uniform(-0.5, 0.5, (100, 2))
test_points = read_test_points("test_points.txt")
test_algorithm(shapely_bounding_box_max, bounding_box_max_points,
test_points)
# test_algorithm(shapely_bounding_box_min, bounding_box_min_points)
write_to_file("bounding_box_max.txt", bounding_box_max_points)
write_to_file("bounding_box_min.txt", bounding_box_min_points)
plt.show()
def load_params_from_yaml(self, filepath):
if self.ch_params == 1:
self.inner_region = []
self.fast_outer_region = []
self.slow_outer_region = []
self.delivery_outer_region = []
self.table_dropping_outer_region = []
self.table_picking_outer_region = []
self.manual_outer_region = []
self.error_outer_region = []
self.suspend_outer_region = []
self.none_outer_region = []
self.charging_outer_region = []
self.outer_region = []
self.front_region = []
self.inner_region_slow_speed = []
self.inner_region_top_speed = []
self.newdict = {
"none": self.none_outer_region,
"suspend": self.suspend_outer_region,
"table_dropping": self.table_dropping_outer_region,
"table_picking": self.table_picking_outer_region,
"manual": self.manual_outer_region,
"error": self.error_outer_region,
"charging": self.charging_outer_region
}
with open(filepath, 'r') as infile:
data = yaml.load(infile)
for p in data["outer_region"]["fast"]["points"]:
self.fast_outer_region.append((p["x"], p["y"]))
for p in data["outer_region"]["slow"]["points"]:
self.slow_outer_region.append((p["x"], p["y"]))
for p in data["outer_region"]["delivery"]["points"]:
self.delivery_outer_region.append((p["x"], p["y"]))
for p in data["outer_region"]["table_dropping"]["points"]:
self.table_dropping_outer_region.append((p["x"], p["y"]))
for p in data["outer_region"]["table_picking"]["points"]:
self.table_picking_outer_region.append((p["x"], p["y"]))
for p in data["outer_region"]["manual"]["points"]:
self.manual_outer_region.append((p["x"], p["y"]))
for p in data["outer_region"]["error"]["points"]:
self.error_outer_region.append((p["x"], p["y"]))
for p in data["outer_region"]["suspend"]["points"]:
self.suspend_outer_region.append((p["x"], p["y"]))
for p in data["outer_region"]["none"]["points"]:
self.none_outer_region.append((p["x"], p["y"]))
for p in data["outer_region"]["charging"]["points"]:
self.charging_outer_region.append((p["x"], p["y"]))
for p in data["inner_region"]["points"]:
self.inner_region.append((p["x"], p["y"]))
for p in data["front_region"]["points"]:
self.front_region.append((p["x"], p["y"]))
for p in data["inner_region_slow_speed"]["points"]:
self.inner_region_slow_speed.append((p["x"], p["y"]))
for p in data["inner_region_top_speed"]["points"]:
self.inner_region_top_speed.append((p["x"], p["y"]))
self.ch_params = 0
if self.mode != "error":
self.pre_state = self.mode
if self.mode == "error":
self.mode = self.pre_state
if self.mode == "delivery":
if (self.speed_type == 1):
self.outer_zone = ShapelyPolygon(self.fast_outer_region)
self.outer_region = self.fast_outer_region
else:
self.outer_zone = ShapelyPolygon(self.slow_outer_region)
self.outer_region = self.slow_outer_region
else:
self.outer_zone = ShapelyPolygon(self.newdict["{}".format(
self.mode)])
self.outer_region = self.newdict["{}".format(self.mode)]
self.inner_region_slow_speed_zone = ShapelyPolygon(
self.inner_region_slow_speed)
self.inner_region_top_speed_zone = ShapelyPolygon(
self.inner_region_top_speed)
self.front_region_zone = ShapelyPolygon(self.front_region)
self.inner_zone = ShapelyPolygon(self.inner_region)
self.visualize_region(self.outer_region, self.inner_region)
"""
map_vertex_list = np.array([(0, 0), (250, 1000), (500, 750), (1000, 1000),
(750, 0)])
xmin = get_x(min(map_vertex_list, key=get_x))
xmax = get_x(max(map_vertex_list, key=get_x))
ymin = get_y(min(map_vertex_list, key=get_y))
ymax = get_y(max(map_vertex_list, key=get_y))
"""
Mutation rate for GA
Format: decimal (1 = 100% (complete randomness))
"""
mutation_rate = 0.01
#Creates two polygon objects used for later calculations
map_poly = Polygon(map_vertex_list, True)
shapely_poly = ShapelyPolygon(map_vertex_list)
"""
The section below calculates the height and coverage radius of the network module on the drone
"""
#Parameters for the caclulation:
wavelength = 0.125
directivity_transmitter_dBi = 14
directivity_reciever_dBi = 5
power_transmitter_dBm = -10
power_reciever_dBm = -70
aperature_angle = 60
#convert angle in degrees to radians
theta = aperature_angle * (np.pi / 180)
def extract(access, geocode, layer='twitter', **kwargs):
if 'consumer_key' in access.keys() and 'consumer_secret' in access.keys(
) and 'access_token' in access.keys() and 'access_secret' in access.keys():
auth = OAuthHandler(access['consumer_key'], access['consumer_secret'])
auth.set_access_token(access['access_token'], access['access_secret'])
if 'items_per_request' not in kwargs.keys():
kwargs['items_per_request'] = 100
if 'wait' not in kwargs.keys():
kwargs['wait'] = 60
if 'max_area' not in kwargs.keys():
kwargs['max_area'] = 200
api = tweepy.API(auth)
count = 0
total = 0
data = []
while True:
try:
collection = tweepy.Cursor(api.search, geocode=geocode).items(
kwargs['items_per_request'])
for status in collection:
if status.coordinates:
data.append(
(status.user.screen_name + ':' + str(status.user.id),
int(time.mktime(status.created_at.timetuple())),
status.coordinates['coordinates'][1],
status.coordinates['coordinates'][0], layer))
total = total + 1
elif status.place:
polygon = ShapelyPolygon(
status.place.bounding_box.coordinates[0])
geom_area = ops.transform(
partial(
pyproj.transform, pyproj.Proj(init='EPSG:4326'),
pyproj.Proj(proj='aea',
lat1=polygon.bounds[1],
lat2=polygon.bounds[3])), polygon)
if geom_area.area < kwargs['max_area']:
point = polygon.representative_point().xy
data.append(
(status.user.screen_name + ':' +
str(status.user.id),
int(time.mktime(status.created_at.timetuple())),
point[1][0], point[0][0], layer))
# except tweepy.TweepError as e:
except:
logpath = 'data/logs/'
if not os.path.exists(logpath):
os.makedirs(logpath)
filename = datetime.datetime.fromtimestamp(
time.time()).strftime(logpath + 'error_%Y%m%d.log')
logging.basicConfig(filename=filename, filemode='a+')
logging.exception("message")
if 'force' in kwargs.keys() and kwargs['force']:
time.sleep(5 * kwargs['wait'])
continue
if len(data) > 0:
frame = pd.DataFrame(data)
frame.columns = ['uid', 'timestamp', 'lat', 'lon', 'layer']
data = []
if 'filename' not in kwargs.keys():
kwargs['filename'] = 'data/entries/twitter.csv'
if not os.path.exists('data/entries/'):
os.makedirs('data/entries/')
header = not os.path.exists(kwargs['filename'])
with open(kwargs['filename'], 'a+') as file:
frame.to_csv(file, header=header, index=False)
count = count + 1
if 'limit' in kwargs.keys() and count >= kwargs['limit']:
break
time.sleep(kwargs['wait'])
def to_shapely(self) -> ShapelyPolygon:
return ShapelyPolygon(self.to_list(demarcation=True))
def to_shapely(self) -> ShapelyPolygon:
p0 = [self.xmin, self.ymin]
p1 = [self.xmax, self.ymin]
p2 = [self.xmax, self.ymax]
p3 = [self.xmin, self.ymax]
return ShapelyPolygon([p0, p1, p2, p3])