diff --git a/.gitignore b/.gitignore
index 60a6406..f3fcd37 100644
--- a/.gitignore
+++ b/.gitignore
@@ -4,5 +4,7 @@ rb_ws/bags
.python-requirements.txt.un~
docker-compose.yml~
*TEMP_DO_NOT_EDIT.txt
-rb_ws/src/buggy/bags/*
-*.bag
\ No newline at end of file
+rb_ws/src/buggy/bags/
+*.bag
+.vision/
+vision/data/
\ No newline at end of file
diff --git a/rb_ws/src/buggy/buggy/controller/controller_node.py b/rb_ws/src/buggy/buggy/controller/controller_node.py
new file mode 100644
index 0000000..9d13593
--- /dev/null
+++ b/rb_ws/src/buggy/buggy/controller/controller_node.py
@@ -0,0 +1,161 @@
+import threading
+import sys
+
+import numpy as np
+
+import rclpy
+from rclpy.node import Node
+
+from std_msgs.msg import Float32, Float64, Bool
+from nav_msgs.msg import Odometry
+
+sys.path.append("/rb_ws/src/buggy/buggy")
+from util.trajectory import Trajectory
+from controller.stanley_controller import StanleyController
+
+class Controller(Node):
+
+ def __init__(self):
+ """
+ Constructor for Controller class.
+
+ Creates a ROS node with a publisher that periodically sends a message
+ indicating whether the node is still alive.
+
+ """
+ super().__init__('controller')
+ self.get_logger().info('INITIALIZED.')
+
+
+ #Parameters
+ self.declare_parameter("dist", 0.0) #Starting Distance along path
+ start_dist = self.get_parameter("dist").value
+
+ self.declare_parameter("traj_name", "buggycourse_safe.json")
+ traj_name = self.get_parameter("traj_name").value
+ self.cur_traj = Trajectory(json_filepath="/rb_ws/src/buggy/paths/" + traj_name) #TODO: Fixed filepath, not good
+
+ 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"):
+ self.controller = StanleyController(start_index = start_index, namespace = self.get_namespace(), node=self) #IMPORT STANLEY
+ else:
+ self.get_logger().error("Invalid Controller Name: " + controller_name.lower())
+ raise Exception("Invalid Controller Argument")
+
+ # Publishers
+ self.init_check_publisher = self.create_publisher(Bool,
+ "debug/init_safety_check", 1
+ )
+ self.steer_publisher = self.create_publisher(
+ Float64, "input/steering", 1
+ )
+ self.heading_publisher = self.create_publisher(
+ Float32, "auton/debug/heading", 1
+ )
+
+ # Subscribers
+ self.odom_subscriber = self.create_subscription(Odometry, 'self/state', self.odom_listener, 1)
+ self.traj_subscriber = self.create_subscription(Odometry, 'self/cur_traj', self.traj_listener, 1)
+
+ self.lock = threading.Lock()
+
+ self.odom = None
+ self.passed_init = False
+
+ 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):
+ '''
+ This is the subscriber that updates the buggies trajectory for navigation
+ '''
+ with self.lock:
+ self.cur_traj, self.controller.current_traj_index = Trajectory.unpack(msg)
+
+ def init_check(self):
+ """
+ Checks if it's safe to switch the buggy into autonomous driving mode.
+ Specifically, it checks:
+ if we can recieve odometry messages from the buggy
+ if the covariance is acceptable (less than 1 meter)
+ if the buggy thinks it is facing in the correct direction wrt the local trajectory (not 180 degrees flipped)
+
+ Returns:
+ A boolean describing the status of the buggy (safe for auton or unsafe for auton)
+ """
+ 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))
+
+ self.get_logger().info("current heading: " + str(np.rad2deg(current_heading)))
+ msg = Float32()
+ msg.data = np.rad2deg(current_heading)
+ self.heading_publisher.publish(msg)
+
+ #Converting headings from [-pi, pi] to [0, 2pi]
+ if (current_heading < 0):
+ 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):
+ if not self.passed_init:
+ self.passed_init = self.init_check()
+ msg = Bool()
+ msg.data = self.passed_init
+ self.init_check_publisher.publish(msg)
+ if self.passed_init:
+ 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)
+
+ controller = Controller()
+
+ rclpy.spin(controller)
+
+ # Destroy the node explicitly
+ # (optional - otherwise it will be done automatically
+ # when the garbage collector destroys the node object)
+ controller.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
\ No newline at end of file
diff --git a/rb_ws/src/buggy/buggy/controller/controller_superclass.py b/rb_ws/src/buggy/buggy/controller/controller_superclass.py
new file mode 100644
index 0000000..f960de7
--- /dev/null
+++ b/rb_ws/src/buggy/buggy/controller/controller_superclass.py
@@ -0,0 +1,49 @@
+from abc import ABC, abstractmethod
+import sys
+
+from nav_msgs.msg import Odometry
+
+
+sys.path.append("/rb_ws/src/buggy/buggy")
+from util.trajectory import Trajectory
+
+class Controller(ABC):
+ """
+ Base class for all controllers.
+
+ The controller takes in the current state of the buggy and a reference
+ trajectory. It must then compute the desired control output.
+
+ The method that it does this by is up to the implementation, of course.
+ Example schemes include Pure Pursuit, Stanley, and LQR.
+ """
+
+ # TODO: move this to a constants class
+ NAND_WHEELBASE = 1.3
+ SC_WHEELBASE = 1.104
+
+ def __init__(self, start_index: int, namespace : str, node) -> None:
+ self.namespace = namespace
+ if namespace.upper() == '/NAND':
+ Controller.WHEELBASE = self.NAND_WHEELBASE
+ else:
+ Controller.WHEELBASE = self.SC_WHEELBASE
+
+ self.current_traj_index = start_index
+ self.node = node
+
+ @abstractmethod
+ def compute_control(
+ self, state_msg: Odometry, trajectory: Trajectory,
+ ) -> float:
+ """
+ Computes the desired control output given the current state and reference trajectory
+
+ Args:
+ state: (Odometry): state of the buggy, including position, attitude and associated twists
+ trajectory (Trajectory): reference trajectory
+
+ Returns:
+ float (desired steering angle)
+ """
+ raise NotImplementedError
\ No newline at end of file
diff --git a/rb_ws/src/buggy/buggy/controller/stanley_controller.py b/rb_ws/src/buggy/buggy/controller/stanley_controller.py
new file mode 100644
index 0000000..3407653
--- /dev/null
+++ b/rb_ws/src/buggy/buggy/controller/stanley_controller.py
@@ -0,0 +1,150 @@
+import sys
+import numpy as np
+
+from sensor_msgs.msg import NavSatFix
+from geometry_msgs.msg import Pose as ROSPose
+from nav_msgs.msg import Odometry
+
+sys.path.append("/rb_ws/src/buggy/buggy")
+from util.trajectory import Trajectory
+from controller.controller_superclass import Controller
+from util.pose import Pose
+
+import utm
+
+
+class StanleyController(Controller):
+ """
+ Stanley Controller (front axle used as reference point)
+ Referenced from this paper: https://ai.stanford.edu/~gabeh/papers/hoffmann_stanley_control07.pdf
+ """
+
+ LOOK_AHEAD_DIST_CONST = 0.05 # s
+ MIN_LOOK_AHEAD_DIST = 0.1 #m
+ MAX_LOOK_AHEAD_DIST = 2.0 #m
+
+ CROSS_TRACK_GAIN = 1.3
+ K_SOFT = 1.0 # m/s
+ K_D_YAW = 0.012 # rad / (rad/s)
+
+ def __init__(self, start_index, namespace, node):
+ super(StanleyController, self).__init__(start_index, namespace, node)
+ self.debug_reference_pos_publisher = self.node.create_publisher(
+ NavSatFix, "auton/debug/reference_navsat", 1
+ )
+ 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
+
+ Args:
+ state_msg: ros Odometry message
+ trajectory (Trajectory): reference trajectory
+
+ Returns:
+ float (desired steering angle)
+ """
+ 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)
+
+ # setting range of indices to search so we don't have to search the entire path
+ traj_index = trajectory.get_closest_index_on_path(
+ front_x,
+ front_y,
+ start_index=self.current_traj_index - 20,
+ end_index=self.current_traj_index + 50,
+ )
+ self.current_traj_index = max(traj_index, self.current_traj_index)
+
+
+ lookahead_dist = np.clip(
+ 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
+
+ # Calculate cross track error by finding the distance from the buggy to the tangent line of
+ # the reference trajectory
+ closest_position = trajectory.get_position_by_index(self.current_traj_index)
+ next_position = trajectory.get_position_by_index(
+ self.current_traj_index + 0.0001
+ )
+ x1 = closest_position[0]
+ y1 = closest_position[1]
+ x2 = next_position[0]
+ y2 = next_position[1]
+ error_dist = -((x - x1) * (y2 - y1) - (y - y1) * (x2 - x1)) / np.sqrt(
+ (y2 - y1) ** 2 + (x2 - x1) ** 2
+ )
+
+
+ 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)
+ - trajectory.get_heading_by_index(trajectory.get_index_from_distance(traj_dist))) / 0.05
+
+ #Determine steering_command
+ steering_cmd = error_heading + cross_track_component + StanleyController.K_D_YAW * (r_traj - r_meas)
+ steering_cmd = np.clip(steering_cmd, -np.pi / 9, np.pi / 9)
+
+
+ #Calculate error, where x is in orientation of buggy, y is cross track error
+ 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]
+ self.debug_error_publisher.publish(error_pose)
+
+ # Publish reference position for debugging
+ try:
+ reference_navsat = NavSatFix()
+ lat, lon = utm.to_latlon(closest_position[0], closest_position[1], 17, "T")
+ reference_navsat.latitude = lat
+ reference_navsat.longitude = lon
+ self.debug_reference_pos_publisher.publish(reference_navsat)
+ except Exception as e:
+ self.node.get_logger().warn(
+ "[Stanley] Unable to convert closest track position lat lon; Error: " + str(e)
+ )
+
+ return steering_cmd
+
+
+
+
+
+
+
+
+
+
+
diff --git a/rb_ws/src/buggy/buggy/simulator/engine.py b/rb_ws/src/buggy/buggy/simulator/engine.py
index efaf6db..b3cb774 100644
--- a/rb_ws/src/buggy/buggy/simulator/engine.py
+++ b/rb_ws/src/buggy/buggy/simulator/engine.py
@@ -10,7 +10,7 @@
import numpy as np
import utm
sys.path.append("/rb_ws/src/buggy/buggy")
-from util import Constants
+from rb_ws.src.buggy.buggy.util.constants import Constants
class Simulator(Node):
@@ -63,12 +63,12 @@ def __init__(self):
self.timer = self.create_timer(timer_period, self.loop)
self.steering_subscriber = self.create_subscription(
- Float64, "buggy/input/steering", self.update_steering_angle, 1
+ Float64, "input/steering", self.update_steering_angle, 1
)
# To read from velocity
self.velocity_subscriber = self.create_subscription(
- Float64, "velocity", self.update_velocity, 1
+ Float64, "sim/velocity", self.update_velocity, 1
)
# for X11 matplotlib (direction included)
@@ -76,10 +76,10 @@ def __init__(self):
# simulate the INS's outputs (noise included)
# this is published as a BuggyState (UTM and radians)
- self.pose_publisher = self.create_publisher(Odometry, "nav/odom", 1)
+ self.pose_publisher = self.create_publisher(Odometry, "self/state", 1)
self.navsatfix_noisy_publisher = self.create_publisher(
- NavSatFix, "state/pose_navsat_noisy", 1
+ NavSatFix, "self/pose_navsat_noisy", 1
)
def update_steering_angle(self, data: Float64):
@@ -155,13 +155,12 @@ def publish(self):
odom.header.stamp = time_stamp
odom_pose = Pose()
- east, north = utm.from_latlon(lat_noisy, long_noisy)
+ east, north, _, _ = utm.from_latlon(lat_noisy, long_noisy)
odom_pose.position.x = float(east)
odom_pose.position.y = float(north)
odom_pose.position.z = float(260)
- odom_pose.orientation.z = np.sin(np.deg2rad(self.heading) / 2)
- odom_pose.orientation.w = np.cos(np.deg2rad(self.heading) / 2)
+ odom_pose.orientation.z = np.deg2rad(self.heading)
# NOTE: autonsystem only cares about magnitude of velocity, so we don't need to split into components
odom_twist = Twist()
@@ -183,6 +182,8 @@ def main(args=None):
rclpy.init(args=args)
sim = Simulator()
rclpy.spin(sim)
+
+ sim.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
diff --git a/rb_ws/src/buggy/buggy/util.py b/rb_ws/src/buggy/buggy/util/constants.py
similarity index 100%
rename from rb_ws/src/buggy/buggy/util.py
rename to rb_ws/src/buggy/buggy/util/constants.py
diff --git a/rb_ws/src/buggy/buggy/util/pose.py b/rb_ws/src/buggy/buggy/util/pose.py
new file mode 100644
index 0000000..f12a4ff
--- /dev/null
+++ b/rb_ws/src/buggy/buggy/util/pose.py
@@ -0,0 +1,203 @@
+import numpy as np
+
+from geometry_msgs.msg import Pose as ROSPose
+from scipy.spatial.transform import Rotation
+
+
+class Pose:
+ """
+ A data structure for storing 2D poses, as well as a set of
+ convenience methods for transforming/manipulating poses
+
+ TODO: All Pose objects are with respect to World coordinates.
+ """
+
+ __x = None
+ __y = None
+ __theta = None
+
+ @staticmethod
+ def rospose_to_pose(rospose: ROSPose):
+ """
+ Converts a geometry_msgs/Pose to a pose3d/Pose
+
+ Args:
+ posestamped (geometry_msgs/Pose): pose to convert
+
+ Returns:
+ Pose: converted pose
+ """
+ (_, _, yaw) = Rotation.from_quat(
+ [
+ rospose.orientation.x,
+ rospose.orientation.y,
+ rospose.orientation.z,
+ rospose.orientation.w,
+ ]
+ ).as_euler('xyz')
+
+ p = Pose(rospose.position.x, rospose.position.y, yaw)
+ return p
+
+ def heading_to_quaternion(heading):
+ q = Rotation.from_euler([0, 0, heading]).as_quat()
+ return q
+
+ def __init__(self, x, y, theta):
+ self.x = x
+ self.y = y
+ self.theta = theta
+
+ def __repr__(self) -> str:
+ return f"Pose(x={self.x}, y={self.y}, theta={self.theta})"
+
+ def copy(self):
+ return Pose(self.x, self.y, self.theta)
+
+ @property
+ def x(self):
+ return self.__x
+
+ @x.setter
+ def x(self, x):
+ self.__x = x
+
+ @property
+ def y(self):
+ return self.__y
+
+ @y.setter
+ def y(self, y):
+ self.__y = y
+
+ @property
+ def theta(self):
+ if self.__theta > np.pi or self.__theta < -np.pi:
+ raise ValueError("Theta is not in [-pi, pi]")
+ return self.__theta
+
+ @theta.setter
+ def theta(self, theta):
+ # normalize theta to [-pi, pi]
+ self.__theta = np.arctan2(np.sin(theta), np.cos(theta))
+
+ def to_mat(self):
+ """
+ Returns the pose as a 3x3 homogeneous transformation matrix
+ """
+ return np.array(
+ [
+ [np.cos(self.theta), -np.sin(self.theta), self.x],
+ [np.sin(self.theta), np.cos(self.theta), self.y],
+ [0, 0, 1],
+ ]
+ )
+
+ @staticmethod
+ def from_mat(mat):
+ """
+ Creates a pose from a 3x3 homogeneous transformation matrix
+ """
+ return Pose(mat[0, 2], mat[1, 2], np.arctan2(mat[1, 0], mat[0, 0]))
+
+ def invert(self):
+ """
+ Inverts the pose
+ """
+ return Pose.from_mat(np.linalg.inv(self.to_mat()))
+
+ def convert_pose_from_global_to_local_frame(self, pose):
+ """
+ Converts the inputted pose from the global frame to the local frame
+ (relative to the current pose)
+ """
+ return pose / self
+
+ def convert_pose_from_local_to_global_frame(self, pose):
+ """
+ Converts the inputted pose from the local frame to the global frame
+ (relative to the current pose)
+ """
+ return pose * self
+
+ def convert_point_from_global_to_local_frame(self, point):
+ """
+ Converts the inputted point from the global frame to the local frame
+ (relative to the current pose)
+ """
+ point_hg = np.array([point[0], point[1], 1])
+ point_hg_local = np.linalg.inv(self.to_mat()) @ point_hg
+ return (
+ point_hg_local[0] / point_hg_local[2],
+ point_hg_local[1] / point_hg_local[2],
+ )
+
+ def convert_point_from_local_to_global_frame(self, point):
+ """
+ Converts the inputted point from the local frame to the global frame
+ (relative to the current pose)
+ """
+ point_hg = np.array([point[0], point[1], 1])
+ point_hg_global = self.to_mat() @ point_hg
+ return (
+ point_hg_global[0] / point_hg_global[2],
+ point_hg_global[1] / point_hg_global[2],
+ )
+
+ def convert_point_array_from_global_to_local_frame(self, points):
+ """
+ Converts the inputted point array from the global frame to the local frame
+ (relative to the current pose)
+
+ Args:
+ points (np.ndarray) [Size: (N,2)]: array of points to convert
+ """
+ points_hg = np.array([points[:, 0], points[:, 1], np.ones(len(points))])
+ points_hg_local = np.linalg.inv(self.to_mat()) @ points_hg.T
+ return (points_hg_local[:2, :] / points_hg_local[2, :]).T
+
+ def convert_point_array_from_local_to_global_frame(self, points):
+ """
+ Converts the inputted point array from the local frame to the global frame
+ (relative to the current pose)
+
+ Args:
+ points (np.ndarray) [Size: (N,2)]: array of points to convert
+ """
+ points_hg = np.array([points[:, 0], points[:, 1], np.ones(len(points))])
+ points_hg_global = self.to_mat() @ points_hg.T
+ return (points_hg_global[:2, :] / points_hg_global[2, :]).T
+
+ def rotate(self, angle):
+ """
+ Rotates the pose by the given angle
+ """
+ self.theta += angle
+
+ def translate(self, x, y):
+ """
+ Translates the pose by the given x and y distances
+ """
+ self.x += x
+ self.y += y
+
+ def __add__(self, other):
+ return Pose(self.x + other.x, self.y + other.y, self.theta + other.theta)
+
+ def __sub__(self, other):
+ return Pose(self.x - other.x, self.y - other.y, self.theta - other.theta)
+
+ def __neg__(self):
+ return Pose(-self.x, -self.y, -self.theta)
+
+ def __mul__(self, other):
+ p1_mat = self.to_mat()
+ p2_mat = other.to_mat()
+
+ return Pose.from_mat(p1_mat @ p2_mat)
+
+ def __truediv__(self, other):
+ p1_mat = self.to_mat()
+ p2_mat = other.to_mat()
+
+ return Pose.from_mat(np.linalg.inv(p2_mat) @ p1_mat)
diff --git a/rb_ws/src/buggy/buggy/util/trajectory.py b/rb_ws/src/buggy/buggy/util/trajectory.py
new file mode 100644
index 0000000..72fbcae
--- /dev/null
+++ b/rb_ws/src/buggy/buggy/util/trajectory.py
@@ -0,0 +1,362 @@
+import json
+
+# from buggy.msg import TrajectoryMsg
+
+import numpy as np
+from scipy.interpolate import Akima1DInterpolator, CubicSpline
+
+import utm
+
+
+class Trajectory:
+ """A wrapper around a trajectory JSON file that does some under-the-hood math. Will
+ interpolate the data points and calculate other information such as distance along the trajectory
+ and instantaneous curvature.
+
+ Currently, the trajectory is assumed to be a straight line between each waypoint. This is
+ not a good assumption, but it's good enough for now. This means that the curvature is zero
+ anywhere between waypoints.
+
+ This class also has a method that will find the closest point on the trajectory to a given
+ point in the world. This is useful for finding where the buggy is on the trajectory.
+
+ Use https://rdudhagra.github.io/eracer-portal/ to make trajectories and save the JSON file
+ """
+
+ def __init__(self, json_filepath=None, positions = None, interpolator="CubicSpline") -> None:
+ """
+ Args:
+ json_filepath (String): file path to the path json file (begins at /rb_ws)
+ positions [[float, float]]: reference trajectory in world coordinates
+ current_speed (float): current speed of the buggy
+
+ Returns:
+ float (desired steering angle)
+ """
+ self.distances = np.zeros((0, 1)) # (N/dt x 1) [d, d, ...]
+ self.positions = np.zeros((0, 2)) # (N x 2) [(x,y), (x,y), ...]
+ self.indices = None # (N x 1) [0, 1, 2, ...]
+ self.interpolation = None # scipy.interpolate.PPoly
+
+ # read positions from file
+ if positions is None:
+ positions = []
+ # Load the json file
+ with open(json_filepath, "r") as f:
+ data = json.load(f)
+
+ # Iterate through the waypoints and extract the positions
+ num_waypoints = len(data)
+ for i in range(0, num_waypoints):
+
+ waypoint = data[i]
+
+ lat = waypoint["lat"]
+ lon = waypoint["lon"]
+
+ # Convert to world coordinates
+ x, y, _, _ = utm.from_latlon(lat, lon)
+ positions.append([x, y])
+
+ positions = np.array(positions)
+
+ num_indices = positions.shape[0]
+
+ if interpolator == "Akima":
+ self.positions = positions
+ self.indices = np.arange(num_indices)
+ self.interpolation = Akima1DInterpolator(self.indices, self.positions)
+ self.interpolation.extrapolate = True
+ elif interpolator == "CubicSpline":
+ temp_traj = Trajectory(positions=positions, interpolator="Akima")
+ tot_len = temp_traj.distances[-1]
+ interp_dists = np.linspace(0, tot_len, num_indices)
+
+ self.indices = np.arange(num_indices)
+ self.positions = [
+ temp_traj.get_position_by_distance(interp_dist)
+ for interp_dist in interp_dists
+ ]
+ self.positions = np.array(self.positions)
+
+ self.interpolation = CubicSpline(self.indices, self.positions)
+ self.interpolation.extrapolate = True
+
+ # TODO: check units
+ # Calculate the distances along the trajectory
+ dt = 0.01 #dt is time step (in seconds (?))
+ ts = np.arange(len(self.positions) - 1, step=dt) # times corresponding to each position (?)
+ drdt = self.interpolation(
+ ts, nu=1
+ ) # Calculate derivatives of interpolated path wrt indices
+ ds = np.sqrt(drdt[:, 0] ** 2 + drdt[:, 1] ** 2) * dt # distances to each interpolated point
+ s = np.cumsum(np.hstack([[0], ds[:-1]]))
+ self.distances = s
+ self.dt = dt
+
+ def get_num_points(self):
+ """Gets the number of points along the trajectory
+
+ Returns:
+ int: number of points
+ """
+ return len(self.positions)
+
+ def get_heading_by_index(self, index):
+ """Gets the heading at given index along trajectory,
+ interpolating if necessary
+
+ Args:
+ index (float): index along the trajectory
+
+ Returns:
+ float: (theta) in rads
+ """
+ # theta = np.interp(index, self.indices, self.positions[:, 2])
+ dxdt, dydt = self.interpolation(index, nu=1).reshape((-1, 2)).T
+ theta = np.arctan2(dydt, dxdt)
+
+ return theta
+
+ def get_heading_by_distance(self, distance):
+ """Gets the heading at given distance along trajectory,
+ interpolating if necessary
+
+ Args:
+ distance (float): distance along the trajectory in meters
+
+ Returns:
+ float: (theta) in rads
+ """
+ index = self.get_index_from_distance(distance)
+ return self.get_heading_by_index(index)
+
+ def get_position_by_index(self, index):
+ """Gets the position at a given index along the trajectory,
+ interpolating if necessary
+
+ Args:
+ index (float): index along the trajectory
+
+ Returns:
+ tuple: (x, y)
+ """
+ # Interpolate the position
+
+ return self.interpolation(index)
+
+ def get_position_by_distance(self, distance):
+ """Gets the position at a given distance along the trajectory,
+ interpolating if necessary
+
+ Args:
+ distance (float): distance along the trajectory in meters
+
+ Returns:
+ tuple: (x, y)
+ """
+ # Interpolate the position
+ index = self.get_index_from_distance(distance)
+ return self.get_position_by_index(index)
+
+ def get_steering_angle_by_index(self, index, wheelbase):
+ """Gets the bicycle-model steering angle at a given distance along the trajectory,
+ interpolating if necessary. Assumes that the origin point of the buggy is at the
+ center of the rear axle.
+
+ Args:
+ index (float): index along the trajectory
+ wheelbase (float): wheelbase of the buggy in meters
+
+ Returns:
+ float: steering angle in rads
+ """
+ curvature = self.get_curvature_by_index(index)
+ return np.arctan(wheelbase * curvature)
+
+ def get_steering_angle_by_distance(self, distance, wheelbase):
+ """Gets the bicycle-model steering angle at a given distance along the trajectory,
+ interpolating if necessary. Assumes that the origin point of the buggy is at the
+ center of the rear axle.
+
+ Args:
+ distance (float): distance along the trajectory in meters
+ wheelbase (float): wheelbase of the buggy in meters
+
+ Returns:
+ float: steering angle in rads
+ """
+ index = self.get_index_from_distance(distance)
+ return self.get_steering_angle_by_index(index, wheelbase)
+
+ def get_index_from_distance(self, distance):
+ """Gets the index at a given distance along the trajectory,
+ interpolating if necessary
+
+ Args:
+ distance (float): distance along the trajectory in meters
+
+ Returns:
+ int: index along the trajectory
+ """
+ # Interpolate the index
+ index = np.interp(
+ distance,
+ self.distances,
+ np.linspace(0, len(self.distances), len(self.distances)),
+ )
+
+ return index * self.dt
+
+ def get_distance_from_index(self, index):
+ """Gets the distance at a given index along the trajectory,
+ interpolating if necessary
+
+ Args:
+ index (float): index along the trajectory
+
+ Returns:
+ float: distance along the trajectory in meters
+ """
+ # Interpolate the distance
+ distance = np.interp(
+ index / self.dt, np.arange(len(self.distances)), self.distances
+ )
+
+ return distance
+
+ def get_curvature_by_index(self, index):
+ """Gets the curvature at a given index along the trajectory,
+ interpolating if necessary
+
+ Args:
+ index (float): index along the trajectory
+
+ Returns:
+ float: curvature
+ """
+ # Interpolate the curvature
+ dxdt, dydt = self.interpolation(index, nu=1).reshape((-1, 2)).T
+ ddxdtt, ddydtt = self.interpolation(index, nu=2).reshape((-1, 2)).T
+
+ curvature = (dxdt * ddydtt - dydt * ddxdtt) / (
+ (dxdt**2 + dydt**2) ** (3 / 2)
+ )
+
+ return curvature
+
+ def get_curvature_by_distance(self, distance):
+ """Gets the curvature at a given distance along the trajectory,
+ interpolating if necessary
+
+ Args:
+ distance (float): distance along the trajectory in meters
+
+ Returns:
+ float: curvature
+ """
+ # Interpolate the curvature
+ index = self.get_index_from_distance(distance)
+
+ return self.get_curvature_by_index(index)
+
+ def get_dynamics_by_index(self, index, wheelbase):
+ """Gets the dynamics at a given index along the trajectory,
+ interpolating if necessary. Convenience function that returns
+ all of the dynamics at once rather than requiring multiple
+ calls to other helper functions. In this way, we can reuse calls
+ to the interpolator, improving performance.
+
+ Args:
+ index (float): index along the trajectory
+
+ Returns:
+ tuple: (x, y, theta, steering_angle)
+ """
+ # Interpolate the dynamics
+ x, y = self.interpolation(index).reshape((-1, 2)).T
+ dxdt, dydt = self.interpolation(index, nu=1).reshape((-1, 2)).T
+ ddxdtt, ddydtt = self.interpolation(index, nu=2).reshape((-1, 2)).T
+
+ curvature = (dxdt * ddydtt - dydt * ddxdtt) / (
+ (dxdt**2 + dydt**2) ** (3 / 2)
+ )
+ theta = np.arctan2(dydt, dxdt)
+
+ return np.stack((x, y, theta, np.arctan(wheelbase * curvature)), axis=-1)
+
+ def get_unit_normal_by_index(self, index):
+ """Gets the index of the closest point on the trajectory to the given point
+
+ Args:
+ index: Nx1 numpy array: indexes along trajectory
+ Returns:
+ Nx2 numpy array: unit normal of the trajectory at index
+ """
+
+ derivative = self.interpolation(index, nu=1)
+ unit_derivative = derivative / np.linalg.norm(derivative, axis=1)[:, None]
+
+ # (x, y), rotated by 90 deg ccw = (-y, x)
+ unit_normal = np.vstack((-unit_derivative[:, 1], unit_derivative[:, 0])).T
+ return unit_normal
+
+ def get_closest_index_on_path(
+ self, x, y, start_index=0, end_index=None, subsample_resolution=1000
+ ):
+ """Gets the index of the closest point on the trajectory to the given point
+
+ Args:
+ x (float): x coordinate
+ y (float): y coordinate
+ start_index (int, optional): index to start searching from. Defaults to 0.
+ end_index (int, optional): index to end searching at. Defaults to None (disable).
+ subsample_resolution: resolution of the resulting interpolation
+ Returns:
+ float: index along the trajectory
+ """
+ # If end_index is not specified, use the length of the trajectory
+ if end_index is None:
+ end_index = len(self.positions) #sketch, 0-indexing where??
+
+ # Floor/ceil the start/end indices
+ start_index = max(0, int(np.floor(start_index)))
+ end_index = int(np.ceil(end_index))
+
+ # Calculate the distance from the point to each point on the trajectory
+ distances = (self.positions[start_index : end_index + 1, 0] - x) ** 2 + (
+ self.positions[start_index : end_index + 1, 1] - y
+ ) ** 2 #Don't need to squareroot as it is a relative distance
+
+ min_ind = np.argmin(distances) + start_index
+
+ start_index = max(0, min_ind - 1) #Protection in case min_ind is too low
+ end_index = min(len(self.positions), min_ind + 1) #Prtoecting in case min_ind too high
+ #Theoretically start_index and end_index are just two apart
+
+ # Now interpolate at a higher resolution to get a more accurate result
+ r_interp = self.interpolation(
+ np.linspace(start_index, end_index, subsample_resolution + 1))
+ x_interp, y_interp = r_interp[:, 0], r_interp[:, 1] #x_interp, y_interp are numpy column vectors
+
+ distances = (x_interp - x) ** 2 + (y_interp - y) ** 2 #Again distances are relative
+
+ # Return the rational index of the closest point
+ return (
+ np.argmin(distances) / subsample_resolution * (end_index - start_index)
+ + start_index
+ )
+
+ """ def pack(self, x, y) -> TrajectoryMsg:
+ traj = TrajectoryMsg()
+ traj.easting = self.positions[:, 0]
+ traj.northing = self.positions[:, 1]
+ traj.time = time.time()
+ traj.cur_idx = self.get_closest_index_on_path(x,y)
+ return traj
+
+ def unpack(trajMsg : TrajectoryMsg):
+ pos = np.array([trajMsg.easting, trajMsg.northing]).transpose(1, 0)
+ cur_idx = trajMsg.cur_idx
+ return Trajectory(positions=pos), cur_idx """
+
diff --git a/rb_ws/src/buggy/buggy/visualization/telematics.py b/rb_ws/src/buggy/buggy/visualization/telematics.py
new file mode 100644
index 0000000..9d1fd80
--- /dev/null
+++ b/rb_ws/src/buggy/buggy/visualization/telematics.py
@@ -0,0 +1,63 @@
+#! /usr/bin/env python3
+# Runs the conversion script for all telematics data
+
+import rclpy
+import utm
+from nav_msgs.msg import Odometry
+from rclpy.node import Node
+from sensor_msgs.msg import NavSatFix
+
+
+class Telematics(Node):
+ """
+ Converts subscribers and publishers that need to be reformated, so that they are readible.
+ """
+
+ def __init__(self):
+ """Generate all the subscribers and publishers that need to be reformatted.
+ """
+ super().__init__('telematics')
+
+ # Implements behavior of callback_args from rospy.Subscriber
+ def wrap_args(callback, callback_args):
+ return lambda msg: callback(msg, callback_args)
+
+ self.self_publisher = self.create_publisher(NavSatFix, "/self/state_navsatfix", 10)
+ self.self_subscriber = self.create_subscription(Odometry, "/self/state", wrap_args(self.convert_buggystate, self.self_publisher), 1)
+
+ self.other_publisher = self.create_publisher(NavSatFix, "/other/state_navsatfix", 10)
+ self.other_subscriber = self.create_subscription(Odometry, "/other/state", wrap_args(self.convert_buggystate, self.other_publisher), 1)
+
+ # TODO Make this a static method?
+ def convert_buggystate(self, msg, publisher):
+ """Converts BuggyState/Odometry message to NavSatFix and publishes to specified publisher
+
+ Args:
+ msg (Odometry): Buggy state to convert
+ publisher (Publisher): Publisher to send NavSatFix message to
+ """
+ try:
+ y = msg.pose.pose.position.y
+ x = msg.pose.pose.position.x
+ lat, long = utm.to_latlon(x, y, 17, "T")
+ down = msg.pose.pose.position.z
+ new_msg = NavSatFix()
+ new_msg.header = msg.header
+ new_msg.latitude = lat
+ new_msg.longitude = long
+ new_msg.altitude = down
+ publisher.publish(new_msg)
+
+ except Exception as e:
+ self.get_logger().warn(
+ "Unable to convert other buggy position to lon lat" + str(e)
+ )
+
+
+if __name__ == "__main__":
+ rclpy.init()
+ telem = Telematics()
+ rclpy.spin(telem)
+
+ telem.destroy_node()
+ rclpy.shutdown()
\ No newline at end of file
diff --git a/rb_ws/src/buggy/launch/controller.xml b/rb_ws/src/buggy/launch/controller.xml
new file mode 100644
index 0000000..52f0ac7
--- /dev/null
+++ b/rb_ws/src/buggy/launch/controller.xml
@@ -0,0 +1,4 @@
+
+
+
+
\ No newline at end of file
diff --git a/rb_ws/src/buggy/launch/sim_2d_single.xml b/rb_ws/src/buggy/launch/sim_2d_single.xml
index 0398279..2c62b1b 100755
--- a/rb_ws/src/buggy/launch/sim_2d_single.xml
+++ b/rb_ws/src/buggy/launch/sim_2d_single.xml
@@ -1,17 +1,23 @@
-
+
+
+
+
+
+
+
+
+
diff --git a/rb_ws/src/buggy/setup.py b/rb_ws/src/buggy/setup.py
index 08a05de..a8a022e 100644
--- a/rb_ws/src/buggy/setup.py
+++ b/rb_ws/src/buggy/setup.py
@@ -26,6 +26,7 @@
'console_scripts': [
'hello_world = buggy.hello_world:main',
'sim_single = buggy.simulator.engine:main',
+ 'controller = buggy.controller.controller_node:main',
'buggy_state_converter = buggy.buggy_state_converter:main',
'watchdog = buggy.watchdog.watchdog_node:main',
],