Skip to content

Commit

Permalink
pylint fixes yet again
Browse files Browse the repository at this point in the history
  • Loading branch information
Jackack committed Feb 3, 2024
1 parent e687393 commit ee77ccf
Show file tree
Hide file tree
Showing 2 changed files with 17 additions and 19 deletions.
5 changes: 1 addition & 4 deletions rb_ws/src/buggy/scripts/auton/model_predictive_controller.py
Original file line number Diff line number Diff line change
@@ -1,5 +1,4 @@
#!/usr/bin/env python3
from abc import ABC, abstractmethod

import time

Expand All @@ -11,12 +10,10 @@
from scipy import sparse

import rospy
from std_msgs.msg import Float32, Float64, Float64MultiArray, MultiArrayDimension, Bool
from nav_msgs.msg import Odometry
from std_msgs.msg import Float64, Float64MultiArray, MultiArrayDimension, Bool
from sensor_msgs.msg import NavSatFix
from geometry_msgs.msg import Pose as ROSPose
from geometry_msgs.msg import Pose2D
from rospy.numpy_msg import numpy_msg

from pose import Pose
from trajectory import Trajectory
Expand Down
31 changes: 16 additions & 15 deletions rb_ws/src/buggy/scripts/auton/occupancy_grid/grid_manager.py
Original file line number Diff line number Diff line change
@@ -1,15 +1,16 @@
#!/usr/bin/env python3

import numpy as np
import scipy
import sys
import json

import numpy as np
# import scipy
import cv2
import sys
from matplotlib import pyplot as plt
# from matplotlib import pyplot as plt
import utm

sys.path.append('rb_ws/src/buggy/scripts/auton')
# from pose import Pose
import json

"""
####################################
Expand All @@ -29,12 +30,12 @@
INIT_WITH_EMPTY_GRID = True
class OccupancyGrid:
def __init__(self):

# Grid 0, 0 is NW
# 1 pixel = 0.5m
# X = East
# Y = North

# the original grid, should never change
self.sat_img = np.array(cv2.cvtColor(cv2.imread("/rb_ws/src/buggy/assets/sat_img_resized.png"), cv2.COLOR_BGR2GRAY), np.uint8)

Expand All @@ -51,7 +52,7 @@ def __init__(self):
# this grid will vary from call to call since NAND will be plotted in here and removed by reverting it back to grid_og
self.grid = np.array(cv2.cvtColor(cv2.imread("/rb_ws/src/buggy/assets/cost_grid.png"), cv2.COLOR_BGR2GRAY), np.uint8)



correspondence_f = open("/rb_ws/src/buggy/assets/landmarks.json")
self.correspondence = json.load(correspondence_f)
Expand All @@ -70,10 +71,10 @@ def __init__(self):
sat_img_pixel = self.correspondence[i]["pixel"]
sat_img_pixel.append(1) # homogenous coordinates
pts_dst.append(sat_img_pixel)

self.pts_src = np.array(pts_src)
self.pts_dst = np.array(pts_dst)


# REF_LOC_UTM_1 = utm.from_latlon(40.438834, -79.946334)
# REF_LOC_UTM_1 = [REF_LOC_UTM_1[0], REF_LOC_UTM_1[1], 1]
Expand All @@ -88,15 +89,15 @@ def __init__(self):
# SAT_IMG_LOC_2 = [1043, 1048, 1]
# SAT_IMG_LOC_3 = [1128, 290, 1]
# SAT_IMG_LOC_4 = [406, 366, 1]

# pts_src = np.array([REF_LOC_UTM_1, REF_LOC_UTM_2, REF_LOC_UTM_3, REF_LOC_UTM_4])
# pts_dst = np.array([SAT_IMG_LOC_1, SAT_IMG_LOC_2, SAT_IMG_LOC_3, SAT_IMG_LOC_4])

self.homography, status = cv2.findHomography(self.pts_src, self.pts_dst, 0)

if not np.allclose(status, 1, atol=1e-5):
raise Exception("Cannot compute homography")

def get_pixel_from_utm(self, utm_coord: np.array):
"""Calculate the pixel coordinates from the utm coordinates
Expand Down Expand Up @@ -131,7 +132,7 @@ def get_pixel_from_latlon(self, latlon_coord: np.array):
utm_coord_homogenous = np.array([utm_coord[0], utm_coord[1], ones])
loc = self.homography @ utm_coord_homogenous
return np.array([loc[0]/loc[2], loc[1]/loc[2]]).T

def plot_points(self, utm_coords: list):
"""Mark the points on the grid completely BLACK
Expand All @@ -147,7 +148,7 @@ def plot_points(self, utm_coords: list):
self.grid[pixel_coords[:, 0], pixel_coords[:, 1]] = 255

def set_cost(self, utm_coords: list, cost: list):
"""Set the grid's cost
"""Set the grid's cost
Args:
utm_coords (list): list of utm coordinates to mark (x, y)
Expand Down Expand Up @@ -201,7 +202,7 @@ def get_utm_cost(self, coords):
num_pixels_passed_thru = len(filtered_pxl_coords)
total = np.sum(self.grid[filtered_pxl_coords[:, 0], filtered_pxl_coords[:, 1]])
return total/num_pixels_passed_thru

def get_pxl_cost(self, pxl_coords: list):
"""Get the cost of the trajectory passed in as pxl_coordinates
Expand Down

0 comments on commit ee77ccf

Please sign in to comment.