Final package changes

Project.toml

@@ -3,6 +3,9 @@ uuid = "123b5ccf-89cf-4973-9536-c0f20d353bb2"
 authors = ["Himanshu Gupta"]
 version = "1.0.0-DEV"
 
+[deps]
+DataStructures = "864edb3b-99cc-5e75-8d2d-829cb0a9cfe8"
+
 [compat]
 julia = "1"
 

examples/Manifest.toml

@@ -0,0 +1,88 @@
+# This file is machine-generated - editing it directly is not advised
+
+julia_version = "1.8.1"
+manifest_format = "2.0"
+project_hash = "289f9bb1a50577dc9e4ada4f19ad232248d43d52"
+
+[[deps.Artifacts]]
+uuid = "56f22d72-fd6d-98f1-02f0-08ddc0907c33"
+
+[[deps.CompilerSupportLibraries_jll]]
+deps = ["Artifacts", "Libdl"]
+uuid = "e66e0078-7015-5450-92f7-15fbd957f2ae"
+version = "0.5.2+0"
+
+[[deps.CompositeTypes]]
+git-tree-sha1 = "02d2316b7ffceff992f3096ae48c7829a8aa0638"
+uuid = "b152e2b5-7a66-4b01-a709-34e65c35f657"
+version = "0.1.3"
+
+[[deps.Dates]]
+deps = ["Printf"]
+uuid = "ade2ca70-3891-5945-98fb-dc099432e06a"
+
+[[deps.DomainSets]]
+deps = ["CompositeTypes", "IntervalSets", "LinearAlgebra", "Random", "StaticArrays", "Statistics"]
+git-tree-sha1 = "51b4b84d33ec5e0955b55ff4b748b99ce2c3faa9"
+uuid = "5b8099bc-c8ec-5219-889f-1d9e522a28bf"
+version = "0.6.7"
+
+[[deps.IntervalSets]]
+deps = ["Dates", "Random", "Statistics"]
+git-tree-sha1 = "8e59ea773deee525c99a8018409f64f19fb719e6"
+uuid = "8197267c-284f-5f27-9208-e0e47529a953"
+version = "0.7.7"
+
+[[deps.Libdl]]
+uuid = "8f399da3-3557-5675-b5ff-fb832c97cbdb"
+
+[[deps.LinearAlgebra]]
+deps = ["Libdl", "libblastrampoline_jll"]
+uuid = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
+
+[[deps.OpenBLAS_jll]]
+deps = ["Artifacts", "CompilerSupportLibraries_jll", "Libdl"]
+uuid = "4536629a-c528-5b80-bd46-f80d51c5b363"
+version = "0.3.20+0"
+
+[[deps.Printf]]
+deps = ["Unicode"]
+uuid = "de0858da-6303-5e67-8744-51eddeeeb8d7"
+
+[[deps.Random]]
+deps = ["SHA", "Serialization"]
+uuid = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c"
+
+[[deps.SHA]]
+uuid = "ea8e919c-243c-51af-8825-aaa63cd721ce"
+version = "0.7.0"
+
+[[deps.Serialization]]
+uuid = "9e88b42a-f829-5b0c-bbe9-9e923198166b"
+
+[[deps.SparseArrays]]
+deps = ["LinearAlgebra", "Random"]
+uuid = "2f01184e-e22b-5df5-ae63-d93ebab69eaf"
+
+[[deps.StaticArrays]]
+deps = ["LinearAlgebra", "Random", "StaticArraysCore", "Statistics"]
+git-tree-sha1 = "d5fb407ec3179063214bc6277712928ba78459e2"
+uuid = "90137ffa-7385-5640-81b9-e52037218182"
+version = "1.6.4"
+
+[[deps.StaticArraysCore]]
+git-tree-sha1 = "36b3d696ce6366023a0ea192b4cd442268995a0d"
+uuid = "1e83bf80-4336-4d27-bf5d-d5a4f845583c"
+version = "1.4.2"
+
+[[deps.Statistics]]
+deps = ["LinearAlgebra", "SparseArrays"]
+uuid = "10745b16-79ce-11e8-11f9-7d13ad32a3b2"
+
+[[deps.Unicode]]
+uuid = "4ec0a83e-493e-50e2-b9ac-8f72acf5a8f5"
+
+[[deps.libblastrampoline_jll]]
+deps = ["Artifacts", "Libdl", "OpenBLAS_jll"]
+uuid = "8e850b90-86db-534c-a0d3-1478176c7d93"
+version = "5.1.1+0"

examples/Project.toml

@@ -0,0 +1,3 @@
+[deps]
+DomainSets = "5b8099bc-c8ec-5219-889f-1d9e522a28bf"
+StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"

examples/demo_dubin_car.jl

@@ -0,0 +1,182 @@
+using LinearAlgebra
+using HybridAStar: hybrid_astar_search, get_path
+
+struct Location
+    x::Float64
+    y::Float64
+end
+
+struct ObstacleLocation
+    x::Float64
+    y::Float64
+    r::Float64 #Radius of the obstacle which is assumed to be a circle
+end
+
+struct ExperimentEnvironment
+    length::Float64
+    breadth::Float64
+    obstacles::Array{ObstacleLocation,1}
+end
+
+#State
+struct VehicleState
+    x::Float64
+    y::Float64
+    theta::Float64
+end
+
+function Base.in(agent_state::VehicleState, goal::Location)
+    euclidean_distance = sqrt( (agent_state.x - goal.x)^2 + (agent_state.y - goal.y)^2 )
+    if( euclidean_distance < 2.0)
+         return true
+    else
+        return false
+    end
+end
+
+#Actions
+function get_vehicle_actions(max_delta_angle, min_delta_angle_difference)
+    set_of_delta_angles = Float64[0.0]
+    half_num_actions = Int( floor(max_delta_angle/min_delta_angle_difference) )
+    for i in 1:half_num_actions
+        neg_angle = -min_delta_angle_difference*i*pi/180
+        pos_angle = min_delta_angle_difference*i*pi/180
+        if(wrap_between_0_and_2Pi(neg_angle) != wrap_between_0_and_2Pi(pos_angle))
+            push!(set_of_delta_angles, neg_angle)
+            push!(set_of_delta_angles, pos_angle)
+        else
+            push!(set_of_delta_angles, pos_angle)
+        end
+    end
+    return set_of_delta_angles
+end
+
+function wrap_between_0_and_2Pi(theta)
+   return mod(theta,2*pi)
+end
+
+
+#Dynamics
+function holonomic_vehicle_dynamics(vehicle_state::VehicleState,delta_angle)
+    vehicle_speed = 1.0
+    time_duration = 0.5
+    vehicle_x = vehicle_state.x
+    vehicle_y = vehicle_state.y
+    vehicle_theta = vehicle_state.theta
+    if(vehicle_speed == 0.0)
+        return VehicleState(vehicle_x,vehicle_y,vehicle_theta)
+    else
+        new_theta = wrap_between_0_and_2Pi(vehicle_theta + delta_angle)
+        new_x = vehicle_x + vehicle_speed*cos(new_theta)*time_duration
+        new_y = vehicle_y + vehicle_speed*sin(new_theta)*time_duration
+        return VehicleState(new_x,new_y,new_theta)
+    end
+end
+
+#NodeKey
+struct NodeBin
+    discrete_x::Float64
+    discrete_y::Float64
+    discrete_θ::Float64
+end
+
+function get_node_key(vehicle_state::VehicleState)
+    world_length = 100.0
+    world_breadth = 100.0
+    bin_width = 0.25
+    max_num_bins_x = ceil(world_length/bin_width)
+    discrete_x = clamp(ceil(vehicle_state.x/bin_width),1.0,max_num_bins_x)
+    max_num_bins_y = ceil(world_breadth/bin_width)
+    discrete_y = clamp(ceil(vehicle_state.y/bin_width),1.0,max_num_bins_y)
+    discrete_theta = ceil(vehicle_state.theta*180/pi)
+    return NodeBin(discrete_x,discrete_y,discrete_theta)
+end
+
+#Node Cost
+struct node_cost <: Function
+    wheelbase::Float64
+    env::ExperimentEnvironment
+end
+
+function (obj::node_cost)(old_vehicle_state,new_vehicle_state,action,time_stamp)
+
+    total_cost = 0.0
+    vehicle_state = new_vehicle_state
+    vehicle_x = vehicle_state.x
+    vehicle_y = vehicle_state.y
+    vehicle_L = obj.wheelbase
+    world = obj.env
+
+    #Cost from going out of bounds
+    if(vehicle_x>world.length-vehicle_L || vehicle_y>world.breadth-vehicle_L || vehicle_x<0.0+vehicle_L || vehicle_y<0.0+vehicle_L)
+        return Inf
+    end
+
+    #Cost from collision with obstacles
+    for obstacle in world.obstacles
+        if(in_obstacle(vehicle_x,vehicle_y,obstacle,vehicle_L))
+            return Inf
+        else
+            continue
+        end
+    end
+
+    #Cost from no change in heading angle
+    if(action == 0.0)
+       total_cost += -1.0
+    end
+
+    #Cost from Long Paths
+    total_cost += 1
+
+    return total_cost
+end
+
+function is_within_range(p1_x,p1_y, p2_x, p2_y, threshold_distance)
+    euclidean_distance = sqrt((p1_x - p2_x)^2 + (p1_y - p2_y)^2)
+    if(euclidean_distance<=threshold_distance)
+        return true
+    else
+        return false
+    end
+end
+
+function in_obstacle(px,py,obstacle,padding=0.0)
+    return is_within_range(px,py,obstacle.x,obstacle.y,obstacle.r+padding)
+end
+
+
+#Heuristic Cost
+struct heuristic_cost <: Function
+    goal::Location
+end
+
+function (obj::heuristic_cost)(vehicle_state)
+    goal = obj.goal
+    euclidean_distance =  sqrt( (vehicle_state.x - goal.x)^2 + (vehicle_state.y - goal.y)^2 )
+    direct_line_to_goal_slope = wrap_between_0_and_2Pi(atan(goal.y-vehicle_state.y,goal.x-vehicle_state.x))
+    orientation_cost = 10* dot( (cos(direct_line_to_goal_slope), sin(direct_line_to_goal_slope)) ,
+                                (cos(vehicle_state.theta), sin(vehicle_state.theta)) )
+    return euclidean_distance - orientation_cost
+end
+
+
+e = ExperimentEnvironment(100.0,100.0,ObstacleLocation[ObstacleLocation(30.0,30.0,15.0),ObstacleLocation(70.0,60.0,15.0)])
+holonomic_vs = VehicleState(10.0,20.0,0.0)
+holonomic_va = get_vehicle_actions(45,5)
+g = Location(75.0,95.0)
+nc = node_cost(0.5,e)
+hc = heuristic_cost(g)
+cs = hybrid_astar_search(g, holonomic_vs, holonomic_va, holonomic_vehicle_dynamics, get_node_key, nc, hc)
+p = get_path(holonomic_vs, cs, holonomic_vehicle_dynamics)
+
+#=
+p = get_path(holonomic_vs, cs, holonomic_vehicle_dynamics)
+x = [s.x for s in p]; y = [s.y for s in p]
+plot(x,y)
+using JET
+using BenchmarkTools
+@report_opt hybrid_astar_search(g, holonomic_vs, holonomic_va, holonomic_vehicle_dynamics, get_node_key, nc, hc)
+@profiler hybrid_astar_search(g, holonomic_vs, holonomic_va, holonomic_vehicle_dynamics, get_node_key, nc, hc)
+@benchmark hybrid_astar_search($g, $holonomic_vs, $holonomic_va, holonomic_vehicle_dynamics, get_node_key, nc, hc)
+=#

examples/demo_holonomic_vehicle.jl

@@ -0,0 +1,35 @@
+using LinearAlgebra
+using StaticArrays: SA
+using DomainSets: Disk
+using HybridAStar: hybrid_astar_search,get_path
+
+dynamics(state, action) = state + SA[cosd(action), sind(action)]
+
+node_key(state) = round.(Int, state)
+
+function node_g(old_state, new_state, action, depth)
+    if new_state in Disk(2.0, SA[6.0, 6.0])
+        return Inf
+    end
+    return 1.0
+end
+
+node_h(state) = norm(state - SA[6.0, 6.0])
+
+start_state = SA[2.0, 2.0]
+actions = [0, 30, 60, 90, 120, 150, 180] # angles that the vehicle can travel
+goal = Disk(1.0, SA[9.0,9.0])
+cs = hybrid_astar_search(goal, start_state, actions, dynamics, node_key, node_g, node_h)
+p = get_path(start_state, cs, dynamics)
+
+#=
+p = get_path(start_state, cs, dynamics)
+x = [s[1] for s in p]; y = [s[2] for s in p]
+plot(x,y)
+
+using JET
+using BenchmarkTools
+@report_opt hybrid_astar_search(g, holonomic_vs, holonomic_va, holonomic_vehicle_dynamics, get_node_key, nc, hc)
+@profiler hybrid_astar_search(g, holonomic_vs, holonomic_va, holonomic_vehicle_dynamics, get_node_key, nc, hc)
+@benchmark hybrid_astar_search($g, $holonomic_vs, $holonomic_va, holonomic_vehicle_dynamics, get_node_key, nc, hc)
+=#

src/HybridAStar.jl

@@ -1,5 +1,8 @@
 module HybridAStar
 
-# Write your package code here.
+using DataStructures
+include("definition.jl")
+include("generate_path.jl")
+export hybrid_astar_search, get_path
 
 end

src/definition.jl

@@ -0,0 +1,9 @@
+struct GraphNode{ST,AT,KT}
+    state::ST
+    key::KT
+    actual_cost::Float64
+    heuristic_cost::Float64
+    parent_key::Union{KT,Nothing}
+    parent_action::Union{AT,Nothing}
+    depth::Int64
+end