@@ -16,7 +16,7 @@ from std_msgs.msg import Header
16
16
from geographic_msgs.msg import (
17
17
GeoPoint,
18
18
)
19
- from uavf .msg import (
19
+ from uavfros .msg import (
20
20
Mission,
21
21
FlyZone,
22
22
FlyZoneArray,
@@ -26,7 +26,7 @@ from uavf.msg import (
26
26
StationaryObstacles,
27
27
)
28
28
29
- from uavf .srv import MissionService
29
+ from uavfros .srv import MissionService
30
30
31
31
# AUVSI SERVER INFO
32
32
SERVER_IP = "127.0.0.1"
@@ -61,108 +61,111 @@ class UAVFInteropClient(client.AsyncClient):
61
61
mission_dict = json_format.MessageToDict(mission.result())
62
62
return mission_dict
63
63
64
+
64
65
def pack_mission_to_msg(mission: dict):
65
66
msg = Mission()
66
67
67
68
# header
68
69
header = Header()
69
70
header.stamp = rospy.get_rostime()
70
- # header.frame_id =
71
+ # header.frame_id =
71
72
msg.header = header
72
73
73
74
# id
74
- msg.id = mission['id' ]
75
+ msg.id = mission["id" ]
75
76
76
77
# lostCommsPos
77
78
lostCommsPos = GeoPoint()
78
- lostCommsPos.latitude = mission[' lostCommsPos'][' latitude' ]
79
- lostCommsPos.longitude = mission[' lostCommsPos'][' longitude' ]
79
+ lostCommsPos.latitude = mission[" lostCommsPos"][" latitude" ]
80
+ lostCommsPos.longitude = mission[" lostCommsPos"][" longitude" ]
80
81
msg.lostCommsPos = lostCommsPos
81
82
82
83
# flyzones
83
84
flyzones = FlyZoneArray()
84
- for zone in mission[' flyZones' ]:
85
+ for zone in mission[" flyZones" ]:
85
86
flyzone = FlyZone()
86
- flyzone.max_alt = zone[' altitudeMax' ]
87
- flyzone.min_alt = zone[' altitudeMin' ]
87
+ flyzone.max_alt = zone[" altitudeMax" ]
88
+ flyzone.min_alt = zone[" altitudeMin" ]
88
89
boundaryPoints = []
89
- for points in zone[' boundaryPoints' ]:
90
+ for points in zone[" boundaryPoints" ]:
90
91
boundaryPoint = GeoPoint()
91
- boundaryPoint.latitude = points[' latitude' ]
92
- boundaryPoint.longitude = points[' longitude' ]
92
+ boundaryPoint.latitude = points[" latitude" ]
93
+ boundaryPoint.longitude = points[" longitude" ]
93
94
boundaryPoints.append(boundaryPoint)
94
95
flyzone.zone = boundaryPoints
95
96
flyzones.flyzones.append(flyzone)
96
97
msg.flyzones = flyzones
97
-
98
+
98
99
# waypoints
99
100
waypoints = []
100
- for waypoint in mission[' waypoints' ]:
101
+ for waypoint in mission[" waypoints" ]:
101
102
temp = GeoPoint()
102
- temp.latitude = waypoint[' latitude' ]
103
- temp.longitude = waypoint[' longitude' ]
104
- temp.altitude = waypoint[' altitude' ]
103
+ temp.latitude = waypoint[" latitude" ]
104
+ temp.longitude = waypoint[" longitude" ]
105
+ temp.altitude = waypoint[" altitude" ]
105
106
waypoints.waypoints.append(temp)
106
107
msg.waypoints = waypoints
107
-
108
+
108
109
# searchGridPoints
109
110
searchGridPoints = []
110
- for point in mission[' searchGridPoints' ]:
111
+ for point in mission[" searchGridPoints" ]:
111
112
temp = GeoPoint()
112
- temp.latitude = point[' latitude' ]
113
- temp.longitude = point[' longitude' ]
113
+ temp.latitude = point[" latitude" ]
114
+ temp.longitude = point[" longitude" ]
114
115
searchGridPoints.append(temp)
115
116
msg.searchGridPoints = searchGridPoints
116
117
117
118
# offAxisOdlcPos
118
- msg.offAxisOdlcPos.latitude = mission[' offAxisOdlcPos'][' latitude' ]
119
- msg.offAxisOdlcPos.longitude = mission[' offAxisOdlcPos'][' longitude' ]
119
+ msg.offAxisOdlcPos.latitude = mission[" offAxisOdlcPos"][" latitude" ]
120
+ msg.offAxisOdlcPos.longitude = mission[" offAxisOdlcPos"][" longitude" ]
120
121
121
122
# emergentLastKnownPos
122
- msg.emergentLastKnownPos.latitude = mission[' emergentLastKnownPos'][' latitude' ]
123
- msg.emergentLastKnownPos.longitude = mission[' emergentLastKnownPos'][' longitude' ]
123
+ msg.emergentLastKnownPos.latitude = mission[" emergentLastKnownPos"][" latitude" ]
124
+ msg.emergentLastKnownPos.longitude = mission[" emergentLastKnownPos"][" longitude" ]
124
125
125
126
# airDropBoundaryPoints
126
127
boundaryPoints = []
127
- for point in mission[' airDropBoundaryPoints' ]:
128
+ for point in mission[" airDropBoundaryPoints" ]:
128
129
temp = GeoPoint()
129
- temp.latitude = point[' latitude' ]
130
- temp.longitude = point[' longitude' ]
130
+ temp.latitude = point[" latitude" ]
131
+ temp.longitude = point[" longitude" ]
131
132
boundaryPoints.append(temp)
132
133
msg.airDropBoundaryPoints = boundaryPoints
133
134
134
135
# airDropPos
135
- msg.airDropPos.latitude = mission[' airDropPos'][' latitude' ]
136
- msg.airDropPos.longitude = mission[' airDropPos'][' longitude' ]
136
+ msg.airDropPos.latitude = mission[" airDropPos"][" latitude" ]
137
+ msg.airDropPos.longitude = mission[" airDropPos"][" longitude" ]
137
138
138
139
# ugvDropPos
139
- msg.ugvDropPos.latitude = mission[' ugvDropPos'][' latitude' ]
140
- msg.ugvDropPos.longitude = mission[' ugvDropPos'][' longitude' ]
140
+ msg.ugvDropPos.latitude = mission[" ugvDropPos"][" latitude" ]
141
+ msg.ugvDropPos.longitude = mission[" ugvDropPos"][" longitude" ]
141
142
142
143
# stationaryObstacles
143
144
stationaryObstacles = []
144
- for obstacle in mission[' stationaryObstacles' ]:
145
+ for obstacle in mission[" stationaryObstacles" ]:
145
146
temp = GeoCylinder()
146
- temp.center.latitude = obstacle[' latitude' ]
147
- temp.center.longitude = obstacle[' longitude' ]
148
- temp.height = obstacle[' height' ]
149
- temp.radius = obstacle[' radius' ]
147
+ temp.center.latitude = obstacle[" latitude" ]
148
+ temp.center.longitude = obstacle[" longitude" ]
149
+ temp.height = obstacle[" height" ]
150
+ temp.radius = obstacle[" radius" ]
150
151
stationaryObstacles.append(temp)
151
152
msg.stationaryObstacles = stationaryObstacles
152
153
153
154
# mapCenterPos
154
- msg.mapCenterPos.latitude = mission[' mapCenterPos'][' latitude' ]
155
- msg.mapCenterPos.longitude = mission[' mapCenterPos'][' longitude' ]
155
+ msg.mapCenterPos.latitude = mission[" mapCenterPos"][" latitude" ]
156
+ msg.mapCenterPos.longitude = mission[" mapCenterPos"][" longitude" ]
156
157
157
158
# mapHeight
158
- msg.mapHeight = mission[' mapHeight' ]
159
+ msg.mapHeight = mission[" mapHeight" ]
159
160
160
161
return msg
161
162
163
+
162
164
def handle_mission(req):
163
165
mission_data = interopclient.get_mission(MISSION_ID)
164
166
return pack_mission_to_msg(mission_data)
165
167
168
+
166
169
def mission_server():
167
170
mission_serv = rospy.Service(MISSION_SERVER_TOPIC, MissionService, handle_mission)
168
171
rospy.spin()
0 commit comments