Skip to content

Commit

Permalink
Create planner service #42
Browse files Browse the repository at this point in the history 
Created a planner service and a roslaunch for it. When it is
initialized, the planner will read the contents of mission.json,
and create a new mission.Mission object. It will then solve
the optimization problem to produce an optimal surface on which
plans between waypoints can be generated.

It opens a plot in a separate process of the resulting plan.

Finally, a roslaunch file was made. This is to allow configuration
of the planner through a ros configuration file. This file is in
config/planner_settings.yaml.
  • Loading branch information
@rland93
rland93 committed Apr 29, 2022
1 parent 2f27b01 commit 6d66c4f
Show file tree
Hide file tree
Showing 6 changed files with 379 additions and 1 deletion.
2 changes: 1 addition & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -126,7 +126,7 @@ generate_messages(
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
CATKIN_DEPENDS rospy std_msgs message_runtime
CATKIN_DEPENDS rospy std_msgs message_runtime nav_msgs
)

###########
Expand Down
9 changes: 9 additions & 0 deletions config/planner_settings.yaml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,9 @@
surface_dx: 1 # gain in altitude (h) per gain in x-y direction (ft/ft)
surface_d2x: 0.05 # maximum 2nd derivative of h with respect to x-y direction (ft^2 / ft^2)
surface_gap: 20 # minimum gap between static obstacle and vehicle (ft)
grid_resolution: 25 # size of a square "occupancy grid" cell (ft)
grid_buffer: 10 # size of "buffer" around the work area (ft)

# when we solve the optimization problem, we scale the grid down to this shape (x,y)
solve_shape_x: 120
solve_shape_y: 120
5 changes: 5 additions & 0 deletions launch/planner.launch
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
<launch>
<node name="planner" pkg="uavfros" type="planner" output="screen">
<rosparam file="$(find uavfros)/config/planner_settings.yaml"/>
</node>
</launch>
250 changes: 250 additions & 0 deletions mission.json
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,250 @@
{
"id": 1,
"lostCommsPos": {
"latitude": 38.144778,
"longitude": -76.429417
},
"flyZones": [
{
"altitudeMin": 100.0,
"altitudeMax": 750.0,
"boundaryPoints": [
{
"latitude": 38.1462694444444,
"longitude": -76.4281638888889
},
{
"latitude": 38.151625,
"longitude": -76.4286833333333
},
{
"latitude": 38.1518888888889,
"longitude": -76.4314666666667
},
{
"latitude": 38.1505944444444,
"longitude": -76.4353611111111
},
{
"latitude": 38.1475666666667,
"longitude": -76.4323416666667
},
{
"latitude": 38.1446666666667,
"longitude": -76.4329472222222
},
{
"latitude": 38.1432555555556,
"longitude": -76.4347666666667
},
{
"latitude": 38.1404638888889,
"longitude": -76.4326361111111
},
{
"latitude": 38.1407194444444,
"longitude": -76.4260138888889
},
{
"latitude": 38.1437611111111,
"longitude": -76.4212055555556
},
{
"latitude": 38.1473472222222,
"longitude": -76.4232111111111
},
{
"latitude": 38.1461305555556,
"longitude": -76.4266527777778
}
]
}
],
"waypoints": [
{
"latitude": 38.1446916666667,
"longitude": -76.4279944444445,
"altitude": 200.0
},
{
"latitude": 38.1461944444444,
"longitude": -76.4237138888889,
"altitude": 300.0
},
{
"latitude": 38.1438972222222,
"longitude": -76.42255,
"altitude": 400.0
},
{
"latitude": 38.1417722222222,
"longitude": -76.4251083333333,
"altitude": 400.0
},
{
"latitude": 38.14535,
"longitude": -76.428675,
"altitude": 300.0
},
{
"latitude": 38.1508972222222,
"longitude": -76.4292972222222,
"altitude": 300.0
},
{
"latitude": 38.1514944444444,
"longitude": -76.4313833333333,
"altitude": 300.0
},
{
"latitude": 38.1505333333333,
"longitude": -76.434175,
"altitude": 300.0
},
{
"latitude": 38.1479472222222,
"longitude": -76.4316055555556,
"altitude": 200.0
},
{
"latitude": 38.1443333333333,
"longitude": -76.4322888888889,
"altitude": 200.0
},
{
"latitude": 38.1433166666667,
"longitude": -76.4337111111111,
"altitude": 300.0
},
{
"latitude": 38.1410944444444,
"longitude": -76.4321555555556,
"altitude": 400.0
},
{
"latitude": 38.1415777777778,
"longitude": -76.4252472222222,
"altitude": 400.0
},
{
"latitude": 38.1446083333333,
"longitude": -76.4282527777778,
"altitude": 200.0
}
],
"searchGridPoints": [
{
"latitude": 38.1444444444444,
"longitude": -76.4280916666667
},
{
"latitude": 38.1459444444444,
"longitude": -76.4237944444445
},
{
"latitude": 38.1439305555556,
"longitude": -76.4227444444444
},
{
"latitude": 38.1417138888889,
"longitude": -76.4253805555556
},
{
"latitude": 38.1412111111111,
"longitude": -76.4322361111111
},
{
"latitude": 38.1431055555556,
"longitude": -76.4335972222222
},
{
"latitude": 38.1441805555556,
"longitude": -76.4320111111111
},
{
"latitude": 38.1452611111111,
"longitude": -76.4289194444444
},
{
"latitude": 38.1444444444444,
"longitude": -76.4280916666667
}
],
"offAxisOdlcPos": {
"latitude": 38.145111,
"longitude": -76.427861
},
"emergentLastKnownPos": {
"latitude": 38.145111,
"longitude": -76.427861
},
"airDropBoundaryPoints": [
{
"latitude": 38.14616666666666,
"longitude": -76.42666666666668
},
{
"latitude": 38.14636111111111,
"longitude": -76.42616666666667
},
{
"latitude": 38.14558333333334,
"longitude": -76.42608333333334
},
{
"latitude": 38.14541666666667,
"longitude": -76.42661111111111
}
],
"airDropPos": {
"latitude": 38.145848,
"longitude": -76.426374
},
"ugvDrivePos": {
"latitude": 38.146152,
"longitude": -76.426396
},
"stationaryObstacles": [
{
"latitude": 38.146689,
"longitude": -76.426475,
"radius": 150.0,
"height": 750.0
},
{
"latitude": 38.142914,
"longitude": -76.430297,
"radius": 300.0,
"height": 300.0
},
{
"latitude": 38.149504,
"longitude": -76.43311,
"radius": 100.0,
"height": 750.0
},
{
"latitude": 38.148711,
"longitude": -76.429061,
"radius": 300.0,
"height": 750.0
},
{
"latitude": 38.144203,
"longitude": -76.426155,
"radius": 50.0,
"height": 400.0
},
{
"latitude": 38.146003,
"longitude": -76.430733,
"radius": 225.0,
"height": 500.0
}
],
"mapCenterPos": {
"latitude": 38.145103,
"longitude": -76.427856
},
"mapHeight": 1200.0
}
110 changes: 110 additions & 0 deletions planner
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,110 @@
#!/usr/bin/python

import rospy, time
from uavf.srv import WaypointPlan, WaypointPlanResponse, WaypointPlanRequest
from uavfpy.planner import mission, plots
from pathlib import Path
import multiprocessing
import matplotlib.pyplot as plt


class PlannerService(object):
def __init__(self, missionfile):
# read parameters
self.get_params()

# read JSON mission into file
with open(missionfile, "r") as f:
mission_json = f.read()
self.mission_json = mission_json

def compute_plan(self, req: WaypointPlanRequest):
rospy.loginfo("Recieved a request for waypoint plan.")
wgs84_waypoints = []
for wp in req.desired_wp:
wgs84_waypoints.append(wp)

print(wp)

def surfplot(self, mission_inst: mission.Mission):
while True:
"""draw a plot in 2D and 3D of the computed mission."""
fig = plt.figure()
ax0 = fig.add_subplot(121)
ax1 = fig.add_subplot(122, projection="3d")

X, Y = mission_inst.X, mission_inst.Y
X -= X.min()
Y -= Y.min()

plots.plot_surface_2d(
ax=ax0,
X=X,
Y=Y,
Hsheet=mission_inst.Hsurf,
cmap="coolwarm",
)

plots.plot_surface_3d(
ax=ax1,
X=X,
Y=Y,
Hground=mission_inst.Hterrain,
Hsheet=mission_inst.Hsurf,
)

plt.show()
time.sleep(0.25)

def get_params(self):
self.surface_dx = rospy.get_param("surface_dx")
self.surface_d2x = rospy.get_param("surface_d2x")
self.surface_gap = rospy.get_param("surface_gap")
self.grid_resolution = rospy.get_param("grid_resolution")
self.grid_buffer = rospy.get_param("grid_buffer")
self.solve_shape_x = rospy.get_param("solve_shape_x")
self.solve_shape_y = rospy.get_param("solve_shape_y")

def plan_srv(self):
rospy.init_node("planner")
rospy.loginfo("Started planner node.")
# for now, we can just read mission data into JSON

# define coordinate system transforms
wgs2loc = mission.get_xformer_from_CRS_str("WGS84", "Irvine")
loc2wgs = mission.get_xformer_from_CRS_str("Irvine", "WGS84")

# create mission instance
mission1 = mission.Mission(
self.mission_json,
wgs2loc,
loc2wgs,
(self.grid_buffer, self.grid_buffer),
(500, 500),
(self.grid_resolution, self.grid_resolution),
)
rospy.loginfo("Computing optimal altitude...")
mission1.solve_Hsurf(
self.surface_gap,
self.surface_dx,
self.surface_d2x,
solve_shape=(self.solve_shape_x, self.solve_shape_y),
)

# plot in another process
multiprocessing.Process(
target=self.surfplot,
args=(mission1,),
daemon=True,
).start()

rospy.loginfo("Completed!")
rospy.Service("planner", WaypointPlan, self.plan_srv)
rospy.spin()


if __name__ == "__main__":
missionpath = (Path(__file__).parent / "mission.json").resolve()

plannerserv = PlannerService(missionpath)
plannerserv.plan_srv()
4 changes: 4 additions & 0 deletions srv/WaypointPlan.srv
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,4 @@
Waypoints desired_wp
nav_msgs/OccupancyGrid occupancygrid
---
Waypoints computed_wp

0 comments on commit 6d66c4f

Please sign in to comment.