Merge branch 'main' into config_file

Dockerfile

@@ -24,7 +24,7 @@ RUN echo 'source "/opt/ros/humble/setup.bash" --' >> ~/.bashrc && \
     echo 'cd rb_ws' >> ~/.bashrc && \
     echo 'colcon build --symlink-install' >> ~/.bashrc && \
     echo 'source install/local_setup.bash' >> ~/.bashrc && \
-    echo 'cchmod -R +x  src/buggy/scripts/'
+    echo 'chmod -R +x src/buggy/scripts/' >> ~/.bashrc
 
 # add mouse to tmux
 RUN echo 'set -g mouse on' >> ~/.tmux.conf

rb_ws/src/buggy/scripts/buggy_state_converter.py

@@ -13,50 +13,51 @@ def __init__(self):
         self.get_logger().info('INITIALIZED.')
 
         namespace = self.get_namespace()
+        if namespace == "/SC":
+            self.SC_raw_state_subscriber = self.create_subscription(
+                Odometry, "/raw_state", self.convert_SC_state_callback, 10
+            )
+
+            self.NAND_other_raw_state_subscriber = self.create_subscription(
+                Odometry, "/NAND_raw_state", self.convert_NAND_other_state_callback, 10
+            )
 
-        # Create publisher and subscriber for "self" namespace
-        self.self_raw_state_subscriber = self.create_subscription(
-            Odometry, "self/raw_state", self.self_raw_state_callback, 10
-        )
-        self.self_state_publisher = self.create_publisher(Odometry, "self/state", 10)
+            self.other_state_publisher = self.create_publisher(Odometry, "/other/state", 10)
 
-        # Check if namespace is "SC" to add an "other" subscriber/publisher
-        if namespace == "/SC":
-            self.other_raw_state_subscriber = self.create_subscription(
-                Odometry, "other/raw_state", self.other_raw_state_callback, 10
+        elif namespace == "/NAND":
+            self.NAND_raw_state_subscriber = self.create_subscription(
+                Odometry, "/raw_state", self.convert_NAND_state_callback, 10
             )
-            self.other_state_publisher = self.create_publisher(Odometry, "other/state", 10)
+
+        else:
+            self.get_logger().warn(f"Namespace not recognized for buggy state conversion: {namespace}")
+
+        self.self_state_publisher = self.create_publisher(Odometry, "/state", 10)
 
         # Initialize pyproj Transformer for ECEF -> UTM conversion for /SC
         self.ecef_to_utm_transformer = pyproj.Transformer.from_crs(
             "epsg:4978", "epsg:32617", always_xy=True
         )  # TODO: Confirm UTM EPSG code, using EPSG:32617 for UTM Zone 17N
 
-    def self_raw_state_callback(self, msg):
-        """ Callback for processing self/raw_state messages and publishing to self/state """
-        namespace = self.get_namespace()
-
-        self.get_logger().error("CALLED_SELF_CALLBACK")
-
-        if namespace == "/SC":
-            converted_msg = self.convert_SC_state(msg)
-        elif namespace == "/NAND":
-            converted_msg = self.convert_NAND_state(msg)
-        else:
-            self.get_logger().warn(f"Namespace not recognized for buggy state conversion: {namespace}")
-            return
+    def convert_SC_state_callback(self, msg):
+        """ Callback for processing SC/raw_state messages and publishing to self/state """
+        converted_msg = self.convert_SC_state(msg)
+        self.self_state_publisher.publish(converted_msg)
 
+    def convert_NAND_state_callback(self, msg):
+        """ Callback for processing NAND/raw_state messages and publishing to self/state """
+        converted_msg = self.convert_NAND_state(msg)
         self.self_state_publisher.publish(converted_msg)
 
-    def other_raw_state_callback(self, msg):
-        """ Callback for processing other/raw_state messages and publishing to other/state """
-        # Convert the SC message and publish to other/state
-        self.get_logger().error("CALLED_OTHER_CALLBACK")
+
+    def convert_NAND_other_state_callback(self, msg):
+        """ Callback for processing SC/NAND_raw_state messages and publishing to other/state """
         converted_msg = self.convert_NAND_other_state(msg)
         self.other_state_publisher.publish(converted_msg)
 
+
     def convert_SC_state(self, msg):
-        """ 
+        """
         Converts self/raw_state in SC namespace to clean state units and structure
 
         Takes in ROS message in nav_msgs/Odometry format
@@ -131,7 +132,6 @@ def convert_NAND_state(self, msg):
         converted_msg.twist.covariance = msg.twist.covariance
 
         # ---- 4. Linear Velocities in m/s ----
-        # TODO: Check if scalar velocity is coming in from msg.twist.twist.linear.x
         # Convert scalar speed to velocity x/y components using heading (orientation.z)
         speed = msg.twist.twist.linear.x        # m/s scalar velocity
         heading = msg.pose.pose.orientation.z   # heading in radians

rb_ws/src/buggy/scripts/controller/controller_node.py

@@ -58,7 +58,7 @@ def __init__(self):
             Float64, "input/steering", 1
         )
         self.heading_publisher = self.create_publisher(
-            Float32, "auton/debug/heading", 1
+            Float32, "debug/heading", 1
         )
 
         # Subscribers
@@ -142,7 +142,7 @@ def loop(self):
         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)))
+        self.steer_publisher.publish(Float64(data=float(steering_angle_deg.item())))
 
 
 

rb_ws/src/buggy/scripts/controller/stanley_controller.py

@@ -30,10 +30,10 @@ class StanleyController(Controller):
     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
+            NavSatFix, "controller/debug/reference_navsat", 1
         )
         self.debug_error_publisher = self.node.create_publisher(
-            ROSPose, "auton/debug/error", 1
+            ROSPose, "controller/debug/stanley_error", 1
         )
 
     def compute_control(self, state_msg : Odometry, trajectory : Trajectory):

rb_ws/src/buggy/scripts/path_planner/path_planner.py

@@ -9,10 +9,10 @@
 
 from nav_msgs.msg import Odometry
 from std_msgs.msg import Float64
+from geometry_msgs.msg import Pose
 from buggy.msg import TrajectoryMsg
 
 sys.path.append("/rb_ws/src/buggy/scripts")
-from util.pose import Pose
 from util.trajectory import Trajectory
 
 class PathPlanner(Node):
@@ -64,7 +64,7 @@ def __init__(self) -> None:
             self.left_curb = Trajectory(json_filepath="/rb_ws/src/buggy/paths/" + curb_name) #TODO: Fixed filepath, not good
 
         #Publishers
-        self.other_buggy_xtrack_publisher = self.create_publisher(Float64, "self/debug/other_buggy_xtrack", 10)
+        self.other_buggy_xtrack_publisher = self.create_publisher(Float64, "debug/other_buggy_xtrack", 10)
         self.traj_publisher = self.create_publisher(TrajectoryMsg, "self/cur_traj", 10)
 
         #Subscribers
@@ -82,12 +82,12 @@ def __init__(self) -> None:
 
     def self_pose_callback(self, msg):
         with self.self_pose_lock:
-            self.self_pose = Pose.rospose_to_pose(msg.pose.pose)
+            self.self_pose = msg.pose.pose
 
     def other_pose_callback(self, msg):
         with self.other_pose_lock:
-            self.other_pose = Pose.rospose_to_pose(msg.pose.pose)
-            self.get_logger().debug("RECEIVED FROM NAND")
+            self.other_pose = msg.pose.pose
+            self.get_logger().debug("Received position of other buggy.")
 
     def timer_callback(self):
         with self.self_pose_lock and self.other_pose_lock:
@@ -157,7 +157,7 @@ def compute_traj(
             Trajectory: list of x,y coords for ego-buggy to follow.
         """
         # grab slice of nominal trajectory
-        nominal_idx = self.nominal_traj.get_closest_index_on_path(self_pose.x, self_pose.y)
+        nominal_idx = self.nominal_traj.get_closest_index_on_path(self_pose.position.x, self_pose.position.y)
         nominal_dist_along = self.nominal_traj.get_distance_from_index(nominal_idx)
 
         nominal_slice = np.empty((self.RESOLUTION, 2))
@@ -174,22 +174,22 @@ def compute_traj(
             ))
 
         # get index of the other buggy along the trajetory and convert to distance
-        other_idx = self.nominal_traj.get_closest_index_on_path(other_pose.x, other_pose.y)
+        other_idx = self.nominal_traj.get_closest_index_on_path(other_pose.position.x, other_pose.position.y)
         other_dist = self.nominal_traj.get_distance_from_index(other_idx)
         nominal_slice_to_other_dist = np.abs(nominal_slice_dists - other_dist)
 
         passing_offsets = self.offset_func(nominal_slice_to_other_dist)
 
         # compute signed cross-track distance between NAND and nominal
-        nominal_to_other = np.array((other_pose.x, other_pose.y)) - \
+        nominal_to_other = np.array((other_pose.position.x, other_pose.position.y)) - \
             np.array(self.nominal_traj.get_position_by_index(other_idx))
 
         # dot product with the unit normal to produce left-positive signed distance
         other_normal = self.nominal_traj.get_unit_normal_by_index(np.array(other_idx.ravel()))
         other_cross_track_dist = np.sum(
             nominal_to_other * other_normal, axis=1)
 
-        self.other_buggy_xtrack_publisher.publish(Float64(data=float(other_cross_track_dist)))
+        self.other_buggy_xtrack_publisher.publish(Float64(data=float(other_cross_track_dist.item())))
 
         # here, use passing offsets to weight NAND's cross track signed distance:
         # if the sample point is far from SC, the cross track distance doesn't matter
@@ -202,7 +202,7 @@ def compute_traj(
         # clamp passing offset distances to distance to the curb
         if self.left_curb is not None:
             # grab slice of curb correponding to slice of nominal trajectory.
-            curb_idx = self.left_curb.get_closest_index_on_path(self_pose.x, self_pose.y)
+            curb_idx = self.left_curb.get_closest_index_on_path(self_pose.position.x, self_pose.position.y)
             curb_dist_along = self.left_curb.get_distance_from_index(curb_idx)
             curb_idx_end = self.left_curb.get_closest_index_on_path(nominal_slice[-1, 0], nominal_slice[-1, 1])
             curb_dist_along_end = self.left_curb.get_distance_from_index(curb_idx_end)
@@ -229,7 +229,7 @@ def compute_traj(
         positions = nominal_slice + (passing_offsets[:, None] * nominal_normals)
 
         local_traj = Trajectory(json_filepath=None, positions=positions)
-        self.traj_publisher.publish(local_traj.pack(self_pose.x, self_pose.y ))
+        self.traj_publisher.publish(local_traj.pack(self_pose.position.x, self_pose.position.y ))
 
 
 def main(args=None):