diff --git a/Motion_Planning/0Maps/forest_environment.txt b/Motion_Planning/0Maps/forest_environment.txt new file mode 100644 index 0000000..d9f3b76 --- /dev/null +++ b/Motion_Planning/0Maps/forest_environment.txt @@ -0,0 +1,13 @@ +# map forest environment +boundary 0.0 0.0 0.0 5 5 3.0 + +block 1.0 0.5 0.1 1.3 0.8 3.0 0.000000 255.000000 0.000000 +block 0 1.5 0 0.3 1.8 3.0 0.000000 255.000000 0.000000 +block 2.0 1.5 0.1 2.3 1.8 3.0 0.000000 255.000000 0.000000 +block 1.0 3 0.1 1.3 3.3 3.0 0.000000 255.000000 0.000000 +block 3 3 0.1 3.3 3.3 3.0 0.000000 255.000000 0.000000 +block 0 4.5 0.1 0.3 4.8 3.0 0.000000 255.000000 0.000000 +block 2 4.5 0.1 2.3 4.8 3.0 0.000000 255.000000 0.000000 + + + diff --git a/Motion_Planning/0Maps/urban_environment.txt b/Motion_Planning/0Maps/urban_environment.txt new file mode 100644 index 0000000..0ab75b1 --- /dev/null +++ b/Motion_Planning/0Maps/urban_environment.txt @@ -0,0 +1,15 @@ +# map urban environment +boundary 0.0 0.0 0.0 40 40 3.0 + +block 32 5 0 35 8 3.0 0 0 255 +block 28 8 0 35 9.5 3.0 0 0 255 +block 25 5 0 28 9.5 3.0 0 0 255 +block 5 13.5 0 15 29.5 3.0 255 0 0 +block 5 2 0 12.5 9.5 3.0 0 0 255 +block 25 13.5 0 35 25.5 3.0 0 0 255 + + + + + + diff --git a/Motion_Planning/3Safe_Flight_Corridors/LineSegment.m b/Motion_Planning/3Safe_Flight_Corridors/LineSegment.m index 664688d..7f07c66 100644 --- a/Motion_Planning/3Safe_Flight_Corridors/LineSegment.m +++ b/Motion_Planning/3Safe_Flight_Corridors/LineSegment.m @@ -27,7 +27,7 @@ function set_local_bbox(obj, local_bbox) end function set_obs(obj, obs) - Vs = Polyhedron(); + Vs = Polyhedron_(); obj.add_local_bbox(Vs); obj.obs_ = Vs.points_inside(obs); end @@ -144,7 +144,7 @@ function find_ellipsoid(obj, offset_x) end function find_polyhedron(obj) - Vs = Polyhedron(); + Vs = Polyhedron_(); obs_remain = obj.obs_; while(length(obs_remain)) plane = obj.ellipsoid_.closest_hyperplane(obs_remain); diff --git a/Motion_Planning/3Safe_Flight_Corridors/Polyhedron.m b/Motion_Planning/3Safe_Flight_Corridors/Polyhedron_.m similarity index 78% rename from Motion_Planning/3Safe_Flight_Corridors/Polyhedron.m rename to Motion_Planning/3Safe_Flight_Corridors/Polyhedron_.m index 8533202..3f642e0 100644 --- a/Motion_Planning/3Safe_Flight_Corridors/Polyhedron.m +++ b/Motion_Planning/3Safe_Flight_Corridors/Polyhedron_.m @@ -1,24 +1,25 @@ -classdef Polyhedron < handle +classdef Polyhedron_ < handle properties polys_; epsilon_; end methods - function obj = Polyhedron() + function obj = Polyhedron_() obj.polys_ = cell(0); obj.epsilon_ = 1e-10; end % Append Hyperplane function add(obj, plane) + % Does normal vector of the plane point inside or outside? obj.polys_{end+1} = plane; end % Check if the point is inside polyhedron, function isinside = inside(obj, pt) isinside = false; - [len, ~]=size(obj.polys_); + [~, len]=size(obj.polys_); for i = 1 : len - if(obj.polys_{i}.signed_dist(pt) > obj.epsilon_) + if(obj.polys_{i}.signed_dist(pt) < -obj.epsilon_) return; end end diff --git a/Motion_Planning/6Star_Convex/LargeConvexPolytopes.m b/Motion_Planning/6Star_Convex/LargeConvexPolytopes.m new file mode 100644 index 0000000..3a3c3e4 --- /dev/null +++ b/Motion_Planning/6Star_Convex/LargeConvexPolytopes.m @@ -0,0 +1,70 @@ +function [A, b] = LargeConvexPolytopes(pointclouds_xyz, pos, R) + % Given a set of pointclouds coordinates n*3 in 3d, and the current position. + % With the user defined radius R as well. + % Return a convex region denoted by Ax <= b + dim = length(pos); + assert(dim == 2 || dim == 3, "Wrong dimension! Check input data!"); + + points_inside = FindInsidePoints(pointclouds_xyz, pos, R); + +% pointclouds_xyz = AddSurroundingPoints(pointclouds_xyz, pos, R); + flipping_xyz = zeros(size(points_inside)); + for i = 1: size(points_inside, 1) + flipping_xyz(i, :) = SphereFlipping(pos, points_inside(i, :), R); + end + k = convhull(flipping_xyz); + if dim == 2 + k = k(1:end-1); + sc = StarConvex(points_inside(k, 1),... + points_inside(k, 2), pos); + else + k = unique(k); + sc = StarConvex(points_inside(k, 1), points_inside(k, 2),... + pos, points_inside(k, 3)); + end + sc.ConstructConvexFromStar(); + sc.ShrinkToConvex(); + A = sc.A; + b = sc.b; +% sc.VisualizeResult(); +end + +function points_inside = FindInsidePoints(pointclouds_xyz, pos, R) + % Find all the points inside the circle with R as radius and pos as the + % center. + distance = vecnorm(pointclouds_xyz - pos, 2, 2); + points_inside = pointclouds_xyz(distance <= R, :); + points_inside = AddSurroundingPoints(points_inside, pos, R); +end + +function p_xyz = AddSurroundingPoints(pointclouds_xyz, pos, R) + % If there are not enough surrounding points, add surrounding points + % manually. For 2d, add them as a square, for 3d, add them as a cube + dim = length(pos); + if dim == 2 + % Add four points as a square. + rc = R / sqrt(2); + pointclouds_xyz(end+1, :) = [pos(1)+rc, pos(2)+rc]; + pointclouds_xyz(end+1, :) = [pos(1)-rc, pos(2)+rc]; + pointclouds_xyz(end+1, :) = [pos(1)-rc, pos(2)-rc]; + pointclouds_xyz(end+1, :) = [pos(1)+rc, pos(2)-rc]; + else + % Add eight points as a cube. + rc = R / sqrt(3); + pointclouds_xyz(end+1, :) = [pos(1)+rc, pos(2)+rc, pos(3)-rc]; + pointclouds_xyz(end+1, :) = [pos(1)+rc, pos(2)-rc, pos(3)+rc]; + pointclouds_xyz(end+1, :) = [pos(1)-rc, pos(2)+rc, pos(3)+rc]; + pointclouds_xyz(end+1, :) = [pos(1)+rc, pos(2)-rc, pos(3)-rc]; + pointclouds_xyz(end+1, :) = [pos(1)-rc, pos(2)+rc, pos(3)-rc]; + pointclouds_xyz(end+1, :) = [pos(1)-rc, pos(2)-rc, pos(3)+rc]; + pointclouds_xyz(end+1, :) = [pos(1)-rc, pos(2)-rc, pos(3)-rc]; + pointclouds_xyz(end+1, :) = [pos(1)+rc, pos(2)+rc, pos(3)+rc]; +% disp(pointclouds_xyz(end-7:end, :)); + end + p_xyz = pointclouds_xyz; +end + +function p2 = SphereFlipping(pos, p1, R) + % Sphere flipping p1 from pos by a radius R to p2 + p2 = pos + (p1 - pos) * (2 * R - norm(p1 - pos)) / norm(p1 - pos); +end diff --git a/Motion_Planning/6Star_Convex/SCMFromPath.m b/Motion_Planning/6Star_Convex/SCMFromPath.m new file mode 100644 index 0000000..490c785 --- /dev/null +++ b/Motion_Planning/6Star_Convex/SCMFromPath.m @@ -0,0 +1,56 @@ +function [A, b] = SCMFromPath(path, point_cloud_obs, map_boundary) +% LargeConvexPolytopes generates A and b from a single point, SCMFrom path +% generate A and b from a path. + map_size = min(abs(map_boundary.ld - map_boundary.ru)); +% fprintf("Map size is: %f \n", map_size); + R = map_size/2; + tolerance = 0.01; + A = {}; + b = {}; + ieq = 1; + n_node = size(path, 1) - 1; + pos = path(1, :); + [cur_A, cur_b] = LargeConvexPolytopes(point_cloud_obs, pos, R); + A{1} = cur_A; +% cur_b = cur_A * pos' + cur_b; + b{1} = cur_b; + for i = 1:n_node +% fprintf("Node number %d \n", i); +% if i == 7 +% ; +% end + cur_pt = path(i, :); + next_pt = path(i+1, :); +% disp(cur_pt); +% disp(next_pt); + while any(cur_A * next_pt' >= cur_b) + if any(cur_A * cur_pt' >= cur_b) + disp("Wrong! Current point is not in the polytope!"); + end +% fprintf("Checking node number %d \n", i); + start = 0; + goal = 1; + distance = norm(next_pt - cur_pt); +% fprintf("Distance between start and goal is %f .\n", distance); + while distance*(goal - start) > tolerance + mid = start + (goal - start) / 2; + mid_pt = cur_pt + (next_pt - cur_pt) * mid; +% fprintf("Start is %f, middle is %f, goal is %f.\n", start, mid, goal); + if any(cur_A * mid_pt' >= cur_b) + goal = mid; + else + start = mid; + end + end + cur_pt = cur_pt + (next_pt - cur_pt) * goal; + [cur_A, cur_b] = LargeConvexPolytopes(point_cloud_obs, cur_pt, R); + if any(cur_A * cur_pt' >= cur_b) + disp("Wrong! Current point is not in the polytope!"); + end + A{ieq+1} = cur_A; +% cur_b = cur_A * cur_pt' + cur_b; + b{ieq+1} = cur_b; + ieq = ieq + 1; + end + end +end \ No newline at end of file diff --git a/Motion_Planning/6Star_Convex/StarConvex.m b/Motion_Planning/6Star_Convex/StarConvex.m new file mode 100644 index 0000000..8c5fdbf --- /dev/null +++ b/Motion_Planning/6Star_Convex/StarConvex.m @@ -0,0 +1,285 @@ +classdef StarConvex < handle + properties + % All verticals are in CCW + xs_; + ys_; + zs_; + vertices_; + Po_; + epsilon_ = 1e-5; + % Output convex region in original space as Ax <= b + A; + b; + end + + properties %(Access = private) + dim; + + % Constructed convex hull from star convex polytope, 2/3*n + convex_hull; + + % Hyperedges + Edges; + + % Points inside the constructed convex hull, 2/3*k + P_in_; + end + + methods + function obj = StarConvex(xs, ys, Po, zs) + assert(all(size(xs) == size(ys))); + obj.xs_ = xs; + obj.ys_ = ys; + obj.Po_ = Po; + if nargin > 3 + obj.zs_ = zs; + obj.dim = 3; + obj.vertices_ = [obj.xs_, obj.ys_, obj.zs_]; + else + obj.dim = 2; + obj.vertices_ = [obj.xs_, obj.ys_]; + end + end + + function ConstructConvexFromStar(obj) + if obj.dim == 2 +% % Graham Scan, reference +% % http://www.cosy.sbg.ac.at/~held/teaching/compgeo/cg_study.pdf +% +% % Find the bottommost point +% ymin = obj.vertices_(1, 2); +% min_ind = 1; +% for i = 2:size(obj.vertices_,1) +% y = obj.vertices_(i, 2); +% % Pick the bottom-most or choose the left most point in +% % case of tie +% if y < ymin || (y == ymin && obj.vertices_(i, 1) < ... +% obj.vertices_(min_ind, 1)) +% ymin = obj.vertices_(i, 2); +% min_ind = i; +% end +% end +% obj.vertices_ = circshift(obj.vertices_, [min_ind, 0]); +% +% obj.convex_hull = obj.vertices_; +% +% cur_ind = 1; +% obj.P_in_ = []; +% init_pt = obj.convex_hull(cur_ind, :); +% done = false; +% while done == false +% if cur_ind == 1 +% pre_ind = size(obj.convex_hull, 1); +% next_ind = cur_ind + 1; +% elseif cur_ind == size(obj.convex_hull, 1) +% pre_ind = cur_ind - 1; +% next_ind = 1; +% else +% pre_ind = cur_ind - 1; +% next_ind = cur_ind + 1; +% end +% pre_pt = obj.convex_hull(pre_ind, :); +% cur_pt = obj.convex_hull(cur_ind, :); +% next_pt = obj.convex_hull(next_ind, :); +% v1 = cur_pt - pre_pt; +% v2 = next_pt - cur_pt; +% cross_turn = v1(1)*v2(2) - v1(2)*v2(1); +% % fprintf("Pre index is %d, current index is %d, next index is %d \n", pre_ind, cur_ind, next_ind); +% % fprintf("Pre pt is [%.2f, %.2f, %.2f], cur pt is [%.2f, %.2f, %.2f]," + ... +% % " next pt is [%.2f, %.2f, %.2f]", pre_pt(1), pre_pt(2), pre_pt(3),... +% % cur_pt(1), cur_pt(2), cur_pt(3), next_pt(1), next_pt(2), next_pt(3)); +% % fprintf("crossturn is %f \n", cross_turn(3)); +% if cross_turn > obj.epsilon_ % left turn if CCW +% cur_ind = mod(cur_ind, size(obj.convex_hull, 1)) + 1; +% if norm(obj.convex_hull(cur_ind, :) - init_pt) < obj.epsilon_ +% done = true; +% end +% elseif cross_turn < -obj.epsilon_ % right turn if CCW +% obj.P_in_(end+1, :) = obj.convex_hull(cur_ind, :); +% obj.convex_hull(cur_ind, :) = []; +% % start from bottom most and left point guarantee +% % cur_ind is not 1 +% cur_ind = cur_ind - 1; +% else % collinear +% obj.convex_hull(cur_ind, :) = []; +% if norm(obj.convex_hull(cur_ind, :)-init_pt) < obj.epsilon_ +% done = true; +% end +% end +% end +% obj.convex_hull(end+1, :) = obj.convex_hull(1, :); + k = convhull(obj.vertices_, 'Simplify', true); + ind_in = setdiff(1:size(obj.vertices_, 1), k); + obj.P_in_ = obj.vertices_(ind_in, :); + obj.convex_hull = obj.vertices_(k, :); + obj.Edges = repmat(struct, 1, size(obj.convex_hull, 1)-1); + for i = 1: size(obj.convex_hull, 1)-1 + obj.Edges(i).p1 = obj.convex_hull(i, :); + obj.Edges(i).p2 = obj.convex_hull(i+1, :); + vec = obj.Edges(i).p2 - obj.Edges(i).p1; + n_vec = [-vec(2), vec(1)]; % Left is the inside part + obj.Edges(i).n = n_vec / norm(n_vec); + end + else + k = convhull(obj.vertices_, 'Simplify',true); + ind_in = setdiff(1:size(obj.vertices_, 1), unique(k)); + obj.P_in_ = obj.vertices_(ind_in, :); + obj.convex_hull = repmat(struct, 1, size(k, 1)); + for i = 1: size(k, 1) + obj.convex_hull(i).p1 = obj.vertices_(k(i, 1), :); + obj.convex_hull(i).p2 = obj.vertices_(k(i, 2), :); + obj.convex_hull(i).p3 = obj.vertices_(k(i, 3), :); + % Normal vector of this hyperedge. It points inside if + % facet vertices detemined by k are CCW + n_vec = cross(obj.convex_hull(i).p3 - obj.convex_hull(i).p1, ... + obj.convex_hull(i).p2 - obj.convex_hull(i).p1); + % Inverse if the vector point to the other side + if dot(n_vec, obj.Po_ - obj.convex_hull(i).p1) < -obj.epsilon_ + n_vec = -n_vec; + end + obj.convex_hull(i).n = n_vec / norm(n_vec); + end + obj.Edges = obj.convex_hull; + end + end + + function ShrinkToConvex(obj) + % Shrink the convex hull to pointcloud-free convex region + obj.A = zeros(size(obj.Edges, 2), obj.dim); + obj.b = zeros(size(obj.Edges, 2), 1); + for i = 1: size(obj.Edges, 2) + polyhedron = Polyhedron_(); + base = Hyperplane(obj.Edges(i).p1,... + obj.Edges(i).n); + polyhedron.add(base); + if obj.dim == 2 % 2d dimension + % Add first edge + vec1 = obj.Edges(i).p1 - obj.Po_; + n_vec1 = [-vec1(2), vec1(1)]; % Left is the inside part + n1 = n_vec1 / norm(n_vec1); + polyhedron.add(Hyperplane(obj.Po_, n1)); + % Add second edge + vec2 = obj.Po_ - obj.Edges(i).p2; + n_vec2 = [-vec2(2), vec2(1)]; + n2 = n_vec2 / norm(n_vec2); + polyhedron.add(Hyperplane(obj.Po_, n2)); + else % 3d dimension + % Add first edge + n1 = cross(obj.Edges(i).p2 - obj.Po_, ... + obj.Edges(i).p1 - obj.Po_); + n1 = n1 / norm(n1); + if dot(n1, obj.Edges(i).p3 - obj.Po_) < -obj.epsilon_ + n1 = -n1; + end + polyhedron.add(Hyperplane(obj.Po_, n1)); + % Add second edge + n2 = cross(obj.Edges(i).p3 - obj.Po_, ... + obj.Edges(i).p2 - obj.Po_); + n2 = n2 / norm(n2); + if dot(n2, obj.Edges(i).p1 - obj.Po_) < -obj.epsilon_ + n2 = -n2; + end + polyhedron.add(Hyperplane(obj.Po_, n2)); + % Add third edge + n3 = cross(obj.Edges(i).p1 - obj.Po_, ... + obj.Edges(i).p3 - obj.Po_); + n3 = n3 / norm(n3); + if dot(n3, obj.Edges(i).p2 - obj.Po_) < -obj.epsilon_ + n3 = -n3; + end + polyhedron.add(Hyperplane(obj.Po_, n3)); + end + % Iterate every point inside, find the furthest to base + furthest_dis = -1; + p_i = obj.Edges(i).p1; + for j = 1: size(obj.P_in_, 1) + p_in = obj.P_in_(j,:); + if (polyhedron.inside(p_in)) % CHECK IF IT NEEDS TO BE REVERSED + dis = base.signed_dist(p_in); + if (dis >= furthest_dis) + p_i = p_in; + furthest_dis = dis; + end + end + end + % The normal vector in the paper is pointing to outside. + obj.A(i, :) = -base.n_; + obj.b(i) = dot(-base.n_, p_i); +% if i == 12 +% ; +% end +% disp("Current point Po is: "); +% disp(obj.Po_); +% if dot(obj.A(i, :), obj.Po_) >= obj.b(i) +% disp("Shrink the edge inside the pos."); +% end + assert(dot(obj.A(i, :), obj.Po_) < obj.b(i), "Shrink the edge inside the pos."); + end + end + + function PlotConvexHull(obj) + plot(obj.convex_hull(:, 1), obj.convex_hull(:, 2), 'k*--'); + end + + function PlotVertices(obj) + plot([obj.vertices_(:, 1); obj.vertices_(1, 1)], ... + [obj.vertices_(:, 2); obj.vertices_(1, 2)], 'b*-'); + end + + function PlotInterPoint(obj) + plot(obj.P_in_(:, 1), obj.P_in_(:, 2), 'ro'); + end + + function PlotResults(obj) + upper = max(obj.vertices_); + lower = min(obj.vertices_); + range = upper - lower; + upper = upper + abs(range * 0.1); + lower = lower - abs(range * 0.1); + if obj.dim == 2 + [xx, yy] = meshgrid(linspace(lower(1), upper(1), 13),... + linspace(lower(2), upper(2), 13)); + region = ones(size(xx)); + for i=1:size(obj.b, 1) + sub_region = obj.A(i, 1).*xx + obj.A(i, 2).*yy <= obj.b(i); + region = region & sub_region; + end + surf(xx,yy,double(region)); + colorbar; + view(0,90); + else + [xx, yy, zz] = meshgrid(linspace(lower(1), upper(1), 13),... + linspace(lower(2), upper(2), 13),... + linspace(lower(3), upper(3), 13)); + region = ones(size(xx)); + for i=1:size(obj.b, 1) + sub_region = obj.A(i, 1).*xx + obj.A(i, 2).*yy +... + obj.A(i, 3).*zz <= obj.b(i); + region = region & sub_region; + end + + scatter3(xx(region), yy(region), zz(region)); +% p=patch(isosurface(region, 0)); +% set(p,'FaceColor','red','EdgeColor','none'); + daspect([1,1,1]) + view(3); axis equal + camlight + lighting gouraud + grid on + end + end + + function VisualizeResult(obj) + figure; + hold on + if obj.dim == 2 + obj.PlotVertices(); + obj.PlotConvexHull(); + obj.PlotInterPoint(); + end + obj.PlotResults(); + hold off; + end + end + +end diff --git a/Motion_Planning/demo/demo_forest_environment.m b/Motion_Planning/demo/demo_forest_environment.m new file mode 100644 index 0000000..3ad3c27 --- /dev/null +++ b/Motion_Planning/demo/demo_forest_environment.m @@ -0,0 +1,9 @@ +close all; +clear all; +clc; + +demoActivate = true; +choose_map = 1; +path_id = 2; + +runsim \ No newline at end of file diff --git a/Motion_Planning/demo/demo_urban_environment.m b/Motion_Planning/demo/demo_urban_environment.m new file mode 100644 index 0000000..3ad3c27 --- /dev/null +++ b/Motion_Planning/demo/demo_urban_environment.m @@ -0,0 +1,9 @@ +close all; +clear all; +clc; + +demoActivate = true; +choose_map = 1; +path_id = 2; + +runsim \ No newline at end of file diff --git a/Motion_Planning/init_data.m b/Motion_Planning/init_data.m index b671059..731e482 100644 --- a/Motion_Planning/init_data.m +++ b/Motion_Planning/init_data.m @@ -2,7 +2,7 @@ stop = {[]}; if (~exist('demoActivate')) || (demoActivate == false) - choose_map = 2; + choose_map = 1; end demoActivate = false; if (~exist('path_id')) @@ -17,11 +17,11 @@ switch choose_map case 1 %% 3 blocks - map.load_map('0Maps/ellipsoid.txt', 0.2, 0.2, 0.1); - start = {[1.5 0.2 0.2]}; - stop = {[1.5 2.0 1.9]}; + map.load_map('0Maps/forest_environment.txt', 0.2, 0.2, 0.1); + start = {[1.2 0 0]}; + stop = {[1.5 5 2]}; speed = 1; - acc = 2; + acc = 1; case 2 map.load_map('0Maps/3dCorner.txt', 0.1, 0.1, 0.1); start = {[2.6 1.0 1.0]}; @@ -37,7 +37,7 @@ start = {[-1 1.5 2]}; stop = {[11 1.5 1]}; acc = 2; - if path_id == 4 + if path_id == 2 speed = 1.4; %for path_id = 4, time = 12.9343, snap = 58.376, fly use 12.45s else speed = 1.10423; %for path_id = 2, time = 12.9343, snap = 300.3111, fly use 13.9s diff --git a/Motion_Planning/runsim.m b/Motion_Planning/runsim.m index 75e33be..08309a3 100644 --- a/Motion_Planning/runsim.m +++ b/Motion_Planning/runsim.m @@ -39,6 +39,12 @@ decomps{1} = SFC_3D(path{2}, obps, map.boundary); % call SFC toc +disp('JPS -> StarConvexMethod time is : '); +tic +[A, b] = SCMFromPath(path{2}, obps, map.boundary); +toc + + path{path_id} diff --git a/Motion_Planning/xyzPoints.mat b/Motion_Planning/xyzPoints.mat new file mode 100644 index 0000000..d4f8b5b Binary files /dev/null and b/Motion_Planning/xyzPoints.mat differ diff --git a/forest_environment.txt b/forest_environment.txt new file mode 100644 index 0000000..d9f3b76 --- /dev/null +++ b/forest_environment.txt @@ -0,0 +1,13 @@ +# map forest environment +boundary 0.0 0.0 0.0 5 5 3.0 + +block 1.0 0.5 0.1 1.3 0.8 3.0 0.000000 255.000000 0.000000 +block 0 1.5 0 0.3 1.8 3.0 0.000000 255.000000 0.000000 +block 2.0 1.5 0.1 2.3 1.8 3.0 0.000000 255.000000 0.000000 +block 1.0 3 0.1 1.3 3.3 3.0 0.000000 255.000000 0.000000 +block 3 3 0.1 3.3 3.3 3.0 0.000000 255.000000 0.000000 +block 0 4.5 0.1 0.3 4.8 3.0 0.000000 255.000000 0.000000 +block 2 4.5 0.1 2.3 4.8 3.0 0.000000 255.000000 0.000000 + + + diff --git a/urban_environment.txt b/urban_environment.txt new file mode 100644 index 0000000..0ab75b1 --- /dev/null +++ b/urban_environment.txt @@ -0,0 +1,15 @@ +# map urban environment +boundary 0.0 0.0 0.0 40 40 3.0 + +block 32 5 0 35 8 3.0 0 0 255 +block 28 8 0 35 9.5 3.0 0 0 255 +block 25 5 0 28 9.5 3.0 0 0 255 +block 5 13.5 0 15 29.5 3.0 255 0 0 +block 5 2 0 12.5 9.5 3.0 0 0 255 +block 25 13.5 0 35 25.5 3.0 0 0 255 + + + + + +