Skip to content

Commit

Permalink
pylint pt. 1
Browse files Browse the repository at this point in the history
  • Loading branch information
mehulgoel873 committed Jan 5, 2025
1 parent 19a1815 commit cac2ec7
Show file tree
Hide file tree
Showing 5 changed files with 12 additions and 14 deletions.
4 changes: 3 additions & 1 deletion .gitignore
Original file line number Diff line number Diff line change
Expand Up @@ -5,4 +5,6 @@ rb_ws/bags
docker-compose.yml~
*TEMP_DO_NOT_EDIT.txt
rb_ws/src/buggy/bags/*
*.bag
*.bag
.vision*/*
vision/data*/*
1 change: 0 additions & 1 deletion rb_ws/src/buggy/buggy/controller/controller_superclass.py
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
from abc import ABC, abstractmethod
import sys

from sensor_msgs.msg import NavSatFix
from nav_msgs.msg import Odometry


Expand Down
14 changes: 7 additions & 7 deletions rb_ws/src/buggy/buggy/controller/stanley_controller.py
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@ def __init__(self, start_index, namespace, node):
self.debug_error_publisher = self.node.create_publisher(
ROSPose, "auton/debug/error", 1
)

def compute_control(self, state_msg : Odometry, trajectory : Trajectory):
"""Computes the steering angle determined by Stanley controller.
Does this by looking at the crosstrack error + heading error
Expand All @@ -50,15 +50,15 @@ def compute_control(self, state_msg : Odometry, trajectory : Trajectory):
if self.current_traj_index >= trajectory.get_num_points() - 1:
self.node.get_logger.error("[Stanley]: Ran out of path to follow!")
raise Exception("[Stanley]: Ran out of path to follow!")

current_rospose = state_msg.pose.pose
current_speed = np.sqrt(
state_msg.twist.twist.linear.x**2 + state_msg.twist.twist.linear.y**2
)
yaw_rate = state_msg.twist.twist.angular.z
heading = current_rospose.orientation.z
x, y = current_rospose.position.x, current_rospose.position.y #(Easting, Northing)

front_x = x + StanleyController.WHEELBASE * np.cos(heading)
front_y = y + StanleyController.WHEELBASE * np.sin(heading)

Expand All @@ -76,12 +76,12 @@ def compute_control(self, state_msg : Odometry, trajectory : Trajectory):
self.LOOK_AHEAD_DIST_CONST * current_speed,
self.MIN_LOOK_AHEAD_DIST,
self.MAX_LOOK_AHEAD_DIST)

traj_dist = trajectory.get_distance_from_index(self.current_traj_index) + lookahead_dist

ref_heading = trajectory.get_heading_by_index(
trajectory.get_index_from_distance(traj_dist))

error_heading = ref_heading - heading
error_heading = np.arctan2(np.sin(error_heading), np.cos(error_heading)) #Bounds error_heading

Expand All @@ -103,7 +103,7 @@ def compute_control(self, state_msg : Odometry, trajectory : Trajectory):
cross_track_component = -np.arctan2(
StanleyController.CROSS_TRACK_GAIN * error_dist, current_speed + StanleyController.K_SOFT
)

# Calculate yaw rate error
r_meas = yaw_rate
r_traj = current_speed * (trajectory.get_heading_by_index(trajectory.get_index_from_distance(traj_dist) + 0.05)
Expand All @@ -118,7 +118,7 @@ def compute_control(self, state_msg : Odometry, trajectory : Trajectory):
current_pose = Pose(current_rospose.position.x, current_rospose.position.y, heading)
reference_error = current_pose.convert_point_from_global_to_local_frame(closest_position)
reference_error -= np.array([StanleyController.WHEELBASE, 0]) #Translae back to back wheel to get accurate error

error_pose = ROSPose()
error_pose.position.x = reference_error[0]
error_pose.position.y = reference_error[1]
Expand Down
3 changes: 0 additions & 3 deletions rb_ws/src/buggy/buggy/util/trajectory.py
Original file line number Diff line number Diff line change
@@ -1,7 +1,4 @@
import json
import time
import uuid
import matplotlib.pyplot as plt

# from buggy.msg import TrajectoryMsg

Expand Down
4 changes: 2 additions & 2 deletions rb_ws/src/buggy/launch/sim_2d_single.xml
Original file line number Diff line number Diff line change
Expand Up @@ -13,11 +13,11 @@
<param name="controller" value="stanley"/>
</node>

<node pkg="buggy" exec="buggy_state_converter" name="NAND_state_converter" namespace="NAND"/>
<!-- <node pkg="buggy" exec="buggy_state_converter" name="NAND_state_converter" namespace="NAND"/>
<node pkg="buggy" exec="sim_single" name="NAND_sim_single" namespace="NAND">
<param name="velocity" value="12"/>
<param name="pose" value="Hill1_NAND"/>
</node>
</node> -->

<!-- <node name="foxglove" pkg="foxglove_bridge" type="foxglove_bridge" />
<node name="hello_world" pkg="buggy" type="hello_world" /> -->
Expand Down

0 comments on commit cac2ec7

Please sign in to comment.