Skip to content

Commit

Permalink
removed trailing whitespace
Browse files Browse the repository at this point in the history
  • Loading branch information
mehulgoel873 committed Jan 5, 2025
1 parent fa64058 commit fcfefca
Showing 1 changed file with 10 additions and 10 deletions.
20 changes: 10 additions & 10 deletions rb_ws/src/buggy/buggy/controller/controller_node.py
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ def __init__(self):
"""
super().__init__('controller')
self.get_logger().info('INITIALIZED.')


#Parameters
self.declare_parameter("dist", 0.0) #Starting Distance along path
Expand All @@ -38,7 +38,7 @@ def __init__(self):
start_index = self.cur_traj.get_index_from_distance(start_dist)

self.declare_parameter("controller_name", "stanley")

controller_name = self.get_parameter("controller_name").value
print(controller_name.lower)
if (controller_name.lower() == "stanley"):
Expand Down Expand Up @@ -69,15 +69,15 @@ def __init__(self):

timer_period = 0.01 # seconds (100 Hz)
self.timer = self.create_timer(timer_period, self.loop)

def odom_listener(self, msg : Odometry):
'''
This is the subscriber that updates the buggies state for navigation
msg, should be a CLEAN state as defined in the wiki
'''
with self.lock:
self.odom = msg

def traj_listener(self, msg): #TYPE UNKOWN AS OF NOW?? CUSTOM TYPE WHEN #TODO: FIGURE OUT BEFORE MERGE
'''
This is the subscriber that updates the buggies trajectory for navigation
Expand All @@ -99,11 +99,11 @@ def init_check(self):
if (self.odom == None):
self.get_logger().warn("WARNING: no available position estimate")
return False

elif (self.odom.pose.covariance[0] ** 2 + self.odom.pose.covariance[7] ** 2 > 1**2):
self.get_logger().warn("checking position estimate certainty")
return False

#Originally under a lock, doesn't seem necessary?
current_heading = self.odom.pose.pose.orientation.z
closest_heading = self.cur_traj.get_heading_by_index(self.cur_traj.get_closest_index_on_path(self.odom.pose.pose.position.x, self.odom.pose.pose.position.y))
Expand All @@ -118,11 +118,11 @@ def init_check(self):
current_heading = 2 * np.pi + current_heading
if (closest_heading < 0):
closest_heading = 2 * np.pi + closest_heading

if (abs(current_heading - closest_heading) >= np.pi/2):
self.get_logger().warn("WARNING: INCORRECT HEADING! restart stack. Current heading [-180, 180]: " + str(np.rad2deg(current_heading)))
return False

return True

def loop(self):
Expand All @@ -135,13 +135,13 @@ def loop(self):
self.get_logger().info("Passed Initialization Check")
else:
return

self.heading_publisher.publish(Float32(data=np.rad2deg(self.odom.pose.pose.orientation.z)))
steering_angle = self.controller.compute_control(self.odom, self.cur_traj)
steering_angle_deg = np.rad2deg(steering_angle)
self.steer_publisher.publish(Float64(data=float(steering_angle_deg)))



def main(args=None):
rclpy.init(args=args)
Expand Down

0 comments on commit fcfefca

Please sign in to comment.