Skip to content

Commit a074d24

Browse files
jeffbyjujeffbyju
committed
code for extracting occupancy grid from the cropped map .pgm file
1 parent ccfd1e6 commit a074d24

6 files changed

Lines changed: 271 additions & 17 deletions

File tree

‎GEMstack/knowledge/routes/forward_15m_extra.csv‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
15.0,0,0
2+
16.0,0,0
3+
17.0,0,0
4+
18.0,0,0
5+
19.0,0,0
6+
20.0,0,0
7+
21.0,0,0
8+
22.0,0,0
9+
23.0,0,0
10+
24.0,0,0
11+
25.0,0,0
12+
26.0,0,0
13+
27.0,0,0
14+
28.0,0,0
15+
29.0,0,0
16+
30.0,0,0
17+
31.0,0,0

‎GEMstack/mathutils/transforms.py‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -117,7 +117,8 @@ def lat_lon_to_xy(lat : float, lon : float, lat_reference : float, lon_reference
117117
118118
Returns (x,y), where x is east in m, y is north in m.
119119
"""
120-
import alvinxy.alvinxy as axy
120+
121+
121122
# convert GNSS waypoints into local fixed frame reprented in x and y
122123
east_x, north_y = axy.ll2xy(lat, lon, lat_reference, lon_reference)
123124
return east_x, north_y

‎GEMstack/onboard/planning/bag_2_waypts.py‎

Lines changed: 99 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,99 @@
1+
#!/usr/bin/env python3
2+
3+
import atexit
4+
import rospy
5+
from sensor_msgs.msg import NavSatFix
6+
from septentrio_gnss_driver.msg import INSNavGeod
7+
import pandas as pd
8+
from mathutils import transforms
9+
10+
bag_file_path = "/media/yourmom/Seagate/message_2025-04-01-17-06-29.bag"
11+
xy_csv_path = "/media/yourmom/Seagate/waypoints_xy.csv"
12+
heading_csv_path = "/media/yourmom/Seagate/waypoints_heading.csv"
13+
final_csv_path = "/media/yourmom/Seagate/waypoints.csv"
14+
olat = 40.0928563
15+
olon = -88.2359994
16+
file1 = open(xy_csv_path, mode="w", newline="")
17+
file2 = open(heading_csv_path, mode="w", newline="")
18+
file3 = open("/media/yourmom/Seagate/IRL_sensor_waypoints_lat_lon.csv", mode="w", newline="")
19+
20+
def save_waypoints_xy(data):
21+
lat_lon_to_x, lat_lon_to_y = wps_to_local_xy(data.longitude, data.latitude)
22+
file1.write('%f, %f\n' % (lat_lon_to_x, lat_lon_to_y))
23+
file3.write('%f, %f\n' % (data.longitude, data.latitude))
24+
25+
def save_waypoints_heading(data):
26+
file2.write('%f\n' % (data.heading))
27+
28+
29+
def wps_to_local_xy(lon_wp, lat_wp):
30+
# convert GNSS waypoints into local fixed frame reprented in x and y
31+
lon_wp_x, lat_wp_y = transforms.lat_lon_to_xy(lat_wp,lon_wp,olat,olon) #la.ll2xy(lat_wp, lon_wp, olat, olon)
32+
return lon_wp_x, lat_wp_y
33+
34+
def listener():
35+
rospy.init_node('rosbag_to_waypoints', anonymous=True)
36+
xy_sub = rospy.Subscriber('/septentrio_gnss/navsatfix', NavSatFix, save_waypoints_xy)
37+
heading_sub = rospy.Subscriber('/septentrio_gnss/insnavgeod', INSNavGeod, save_waypoints_heading)
38+
rospy.spin()
39+
40+
def shutdown():
41+
print("shutting down!")
42+
file1.close()
43+
file2.close()
44+
file3.close()
45+
46+
df1 = pd.read_csv(xy_csv_path)
47+
df2 = pd.read_csv(heading_csv_path)
48+
49+
merged_df = pd.concat([df1, df2], axis=1)
50+
merged_df.to_csv(final_csv_path, index=False)
51+
52+
if __name__ == '__main__':
53+
atexit.register(shutdown)
54+
print('Generating Waypoints')
55+
try:
56+
listener()
57+
except rospy.ROSInterruptException:
58+
pass
59+
60+
61+
62+
63+
# def wps_to_local_xy(lon_wp, lat_wp):
64+
# # convert GNSS waypoints into local fixed frame reprented in x and y
65+
# lon_wp_x, lat_wp_y = axy.ll2xy(lat_wp, lon_wp, olat, olon)
66+
# return lon_wp_x, lat_wp_y
67+
# # Path to the rosbag file
68+
# bag_file_path = "/home/sanjay/Downloads/2024-10-26-11-03-12.bag"
69+
# olat = 40.0928563
70+
# olon = -88.2359994
71+
# sensor_waypoints = []
72+
# # Open the bag file
73+
# with rosbag.Bag(bag_file_path, 'r') as bag:
74+
75+
# # "/septentrio_gnss/insnavgeod" -> To get heading
76+
# # "/septentrio_gnss/navsatfix" -> To get lat lon
77+
# topic_to_read_from = ["/septentrio_gnss/insnavgeod", "/septentrio_gnss/navsatfix"]
78+
# xy_msgs = bag.read_messages(topics=[topic_to_read_from[1]])
79+
# heading_msgs = bag.read_messages(topics=[topic_to_read_from[0]])
80+
81+
# xy_iter = iter(xy_msgs)
82+
# heading_iter = iter(heading_msgs)
83+
84+
# xy_msg = next(xy_iter)
85+
# heading_msg = next(heading_iter)
86+
87+
# while xy_msg is not None or heading_msg is not None:
88+
# # print(xy_msg[1].latitude)
89+
# # print(xy_msg[1].longitude)
90+
# # print("-----")
91+
# # print(heading_msg[1].heading)
92+
# lat_lon_to_x, lat_lon_to_y = wps_to_local_xy(xy_msg[1].latitude, xy_msg[1].latitude)
93+
# sensor_waypoints.append([lat_lon_to_x, lat_lon_to_y, heading_msg[1].heading])
94+
95+
# with open("IRL_sensor_waypoints.csv", mode="w", newline="") as file:
96+
# writer = csv.writer(file)
97+
# writer.writerows(sensor_waypoints)
98+
99+

‎GEMstack/onboard/planning/map.png‎

21.6 KB
Loading

‎GEMstack/onboard/planning/out.pgm‎

954 KB
Binary file not shown.

‎GEMstack/onboard/planning/route_planning_component.py‎

Lines changed: 153 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -1,18 +1,31 @@
11
import os
2-
from typing import List
2+
from typing import Dict, List
33

44
import numpy as np
55
from GEMstack.onboard.component import Component
6+
from GEMstack.state.agent import AgentState
67
from GEMstack.state.all import AllState
78
from GEMstack.state.physical_object import ObjectFrameEnum
89
from GEMstack.state.route import PlannerEnum, Route
10+
from .rrt_star import RRTStar
11+
import rospy
12+
from sensor_msgs.msg import Image
13+
from cv_bridge import CvBridge
914

1015

16+
import cv2
17+
18+
ORIGIN_PX = (190, 80)
19+
SCALE_PX_PER_M = 6.5 # px per meter
1120

1221
class RoutePlanningComponent(Component):
1322
"""Reads a route from disk and returns it as the desired route."""
1423
def __init__(self):
1524
print("Route Planning Component init")
25+
self.planner = None
26+
self.route = None
27+
self.bridge = CvBridge()
28+
self.img_pub = rospy.Publisher("/image_with_car_xy", Image, queue_size=1)
1629

1730
def state_inputs(self):
1831
return ["all"]
@@ -22,27 +35,151 @@ def state_outputs(self) -> List[str]:
2235

2336
def rate(self):
2437
return 10.0
38+
39+
def _car_to_pixel(self, x, y, img_w, img_h):
40+
# (x,y)[m] → (u,v)[px]
41+
u0, v0 = ORIGIN_PX
42+
u = int(round(u0 + SCALE_PX_PER_M * x))
43+
v = int(round(v0 - SCALE_PX_PER_M * y))
44+
45+
# clamp to image bounds
46+
u = max(0, min(u, img_w - 1))
47+
v = max(0, min(v, img_h - 1))
48+
return u, v
49+
50+
def _pixel_to_car(self, u, v, img_w, img_h):
51+
# clamp to image bounds
52+
u = max(0, min(u, img_w - 1))
53+
v = max(0, min(v, img_h - 1))
54+
55+
# origin and scale (same as in _car_to_pixel)
56+
u0, v0 = ORIGIN_PX
57+
scale = SCALE_PX_PER_M
58+
59+
# invert:
60+
# u = u0 + scale * x → x = (u - u0) / scale
61+
# v = v0 - scale * y → y = (v0 - v) / scale
62+
x = (u - u0) / scale
63+
y = (v0 - v) / scale
64+
65+
return x, y
66+
67+
def visualize_route_pixels(self, route_pts, start_pt, goal_pt):
68+
"""
69+
route_pts: list of (u, v) in pixels
70+
start_pt: (u, v) in pixels
71+
goal_pt: (u, v) in pixels
72+
"""
73+
# 1. Copy the base image
74+
script_dir = os.path.dirname(os.path.abspath(__file__))
75+
frame_path = os.path.join(script_dir, "out.pgm")
76+
frame = cv2.imread(frame_path, cv2.IMREAD_COLOR)
77+
img_h, img_w = frame.shape[:2]
78+
print("frame shape", frame.shape)
79+
print("frame dtype", frame.dtype)
80+
81+
# 2. Optionally clamp all points to image bounds
82+
def clamp(pt):
83+
u, v = pt
84+
u = max(0, min(int(round(u)), img_w - 1))
85+
v = max(0, min(int(round(v)), img_h - 1))
86+
return (u, v)
87+
88+
pts = np.array([clamp(p) for p in route_pts], dtype=np.int32).reshape(-1, 1, 2)
89+
90+
# 3. Draw the route polyline in green
91+
cv2.polylines(
92+
frame,
93+
[pts],
94+
isClosed=False,
95+
color=(0, 255, 0),
96+
thickness=2
97+
)
98+
99+
# 4. Draw start marker in blue (star)
100+
u_s, v_s = clamp(start_pt)
101+
cv2.drawMarker(
102+
frame,
103+
(u_s, v_s),
104+
color=(255, 0, 0),
105+
markerType=cv2.MARKER_STAR,
106+
markerSize=20,
107+
thickness=3
108+
)
109+
110+
# 5. Draw goal marker in red (tilted cross)
111+
u_g, v_g = clamp(goal_pt)
112+
cv2.drawMarker(
113+
frame,
114+
(u_g, v_g),
115+
color=(0, 0, 255),
116+
markerType=cv2.MARKER_TILTED_CROSS,
117+
markerSize=20,
118+
thickness=3
119+
)
120+
121+
# 6. Publish via ROS
122+
out = self.bridge.cv2_to_imgmsg(frame, "bgr8")
123+
print("Publishing out: ", out)
124+
out.header.stamp = rospy.Time.now()
125+
self.img_pub.publish(out)
126+
127+
128+
25129

26130
def update(self, state: AllState):
27-
print("Route Planner's mission:", state.mission_plan)
131+
print("Route Planner's mission:", state.mission_plan.planner_type.value)
132+
print("type of mission plan:", type(PlannerEnum.RRT_STAR))
133+
print("Route Planner's mission:", state.mission_plan.planner_type.value == PlannerEnum.RRT_STAR.value)
134+
print("Route Planner's mission:", state.mission_plan.planner_type.value == PlannerEnum.PARKING.value)
135+
print("Mission plan:", state.mission_plan)
28136
print("Vehicle x:", state.vehicle.pose.x)
29137
print("Vehicle y:", state.vehicle.pose.y)
30138
print("Vehicle yaw:", state.vehicle.pose.yaw)
31-
if state.mission_plan.planner_type == PlannerEnum.PARKING:
32-
print("Parking mode")
33-
elif state.mission_plan.planner_type == PlannerEnum.RRT_STAR:
139+
if state.mission_plan.planner_type.value == PlannerEnum.PARKING.value:
140+
print("I am in PARKING mode")
141+
# Return a route after doing some processing based on mission plan REMOVE ONCE OTHER PLANNERS ARE IMPLEMENTED
142+
base_path = os.path.dirname(__file__)
143+
file_path = os.path.join(base_path, "../../knowledge/routes/forward_15m_extra.csv")
144+
145+
waypoints = np.loadtxt(file_path, delimiter=',', dtype=float)
146+
if waypoints.shape[1] == 3:
147+
waypoints = waypoints[:,:2]
148+
print("waypoints", waypoints)
149+
self.route = Route(frame=ObjectFrameEnum.START,points=waypoints.tolist())
150+
elif state.mission_plan.planner_type.value == PlannerEnum.RRT_STAR.value:
151+
print("I am in RRT mode")
152+
# start = (state.vehicle.pose.x, state.vehicle.pose.y)
153+
script_dir = os.path.dirname(os.path.abspath(__file__))
154+
map_path = os.path.join(script_dir, "out.pgm")
155+
156+
print("map_path", map_path)
157+
158+
map_img = cv2.imread(map_path, cv2.IMREAD_UNCHANGED)
159+
occupancy_grid = (map_img > 0).astype(np.uint8) # (590, 1656)
160+
x_bounds = (0,occupancy_grid.shape[1])
161+
y_bounds = (0,occupancy_grid.shape[0])
162+
start_u_x, start_u_y = self._car_to_pixel(state.vehicle.pose.x, state.vehicle.pose.y, occupancy_grid.shape[1], occupancy_grid.shape[0])
163+
start = (start_u_x, start_u_y)
164+
goal_u_x, goal_u_y = self._car_to_pixel(state.mission_plan.goal_x, state.mission_plan.goal_y, occupancy_grid.shape[1], occupancy_grid.shape[0])
165+
goal = (goal_u_x, goal_u_y) # When we implement kinodynamic, we need to include target_yaw also
166+
step_size = 1.0
167+
max_iter = 2000
168+
169+
self.planner = RRTStar(start, goal, x_bounds, y_bounds, max_iter=max_iter, step_size=step_size, vehicle_width=1, occupancy_grid=occupancy_grid)
34170
print("RRT mode")
171+
rrt_resp = self.planner.plan()
172+
self.visualize_route_pixels(rrt_resp, start, goal)
173+
for i in range(len(rrt_resp)):
174+
x, y = rrt_resp[i]
175+
# Convert to car coordinates
176+
car_x, car_y = self._pixel_to_car(x, y, occupancy_grid.shape[1], occupancy_grid.shape[0])
177+
rrt_resp[i] = (car_x, car_y)
178+
179+
self.route = Route(frame=ObjectFrameEnum.START, points=rrt_resp)
180+
# print("Route planning complete")
181+
35182
else:
36183
print("Unknown mode")
37184

38-
## Return a route after doing some processing based on mission plan REMOVE ONCE OTHER PLANNERS ARE IMPLEMENTED
39-
base_path = os.path.dirname(__file__)
40-
file_path = os.path.join(base_path, "../../knowledge/routes/forward_15m.csv")
41-
42-
waypoints = np.loadtxt(file_path, delimiter=',', dtype=float)
43-
if waypoints.shape[1] == 3:
44-
waypoints = waypoints[:,:2]
45-
print("waypoints", waypoints)
46-
route = Route(frame=ObjectFrameEnum.START,points=waypoints.tolist())
47-
print("route", route)
48-
return route
185+
return self.route

0 commit comments

Comments
 (0)