debugging, nearly done

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

@@ -24,22 +24,23 @@ def __init__(self):
         
         """
         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")
+        start_dist = self.get_parameter("dist").value
 
-        self.declare_parameter("traj_name", "buggycourse_path.json")
-        traj_name = self.get_parameter("traj_name")
+        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")
-        match self.get_parameter("controller_name"):
+        match self.get_parameter("controller_name").value:
             case "stanley":
-                self.controller = StanleyController(start_index = start_index, buggy_name = self.get_namespace(), node=self) #IMPORT STANLEY
+                self.controller = StanleyController(start_index = start_index, namespace = self.get_namespace(), node=self) #IMPORT STANLEY
             case _:
                 self.get_logger().error("Invalid Controller Name!")
                 raise Exception("Invalid Controller Argument")
@@ -49,14 +50,14 @@ def __init__(self):
             "debug/init_safety_check", 1
         )
         self.steer_publisher = self.create_publisher(
-            Float64, "/buggy/steering", 1
+            Float64, "input/steering", 1
         )
         self.heading_publisher = self.create_publisher(
-            Float32, "/auton/debug/heading", 1
+            Float32, "auton/debug/heading", 1
         )
 
         # Subscribers
-        self.odom_subscriber = self.create_subscription(Odometry, 'self/buggy/state', self.odom_listener, 1)
+        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()
@@ -106,7 +107,9 @@ def init_check(self):
         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)))
-        self.heading_publisher.publish(Float32(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):
@@ -123,30 +126,32 @@ def init_check(self):
     def loop(self):
         if not self.passed_init:
             self.passed_init = self.init_check()
-            self.init_check_publisher.publish(self.passed_init)
+            msg = Bool()
+            msg.data = self.passed_init
+            self.init_check_publisher.publish(msg)
             if self.passed_init:
-                self.get_logger.info("Passed Initialization Check")
+                self.get_logger().info("Passed Initialization Check")
             else:
                 return
 
-        self.heading_publisher.publish(Float32(np.rad2deg(self.odom.pose.pose.orientation.z)))
+        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(steering_angle_deg))
+        self.steer_publisher.publish(Float64(data=float(steering_angle_deg)))
 
 
 
 def main(args=None):
     rclpy.init(args=args)
 
-    watchdog = Controller()
+    controller = Controller()
 
-    rclpy.spin(watchdog)
+    rclpy.spin(controller)
 
     # Destroy the node explicitly
     # (optional - otherwise it will be done automatically
     # when the garbage collector destroys the node object)
-    watchdog.destroy_node()
+    controller.destroy_node()
     rclpy.shutdown()
 
 

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

@@ -6,7 +6,7 @@
 from nav_msgs.msg import Odometry
 
 sys.path.append("/rb_ws/src/buggy/buggy")
-from rb_ws.src.buggy.buggy.util.trajectory import Trajectory
+from util.trajectory import Trajectory
 from controller.controller_superclass import Controller
 from util.pose import Pose
 
@@ -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(
-            "/auton/debug/reference_navsat", NavSatFix, queue_size=1
+            NavSatFix, "auton/debug/reference_navsat", 1
         )
         self.debug_error_publisher = self.node.create_publisher(
-            "/auton/debug/error", ROSPose, queue_size=1
+            ROSPose, "auton/debug/error", 1
         )
 
     def compute_control(self, state_msg : Odometry, trajectory : Trajectory):
@@ -115,7 +115,7 @@ def compute_control(self, state_msg : Odometry, trajectory : Trajectory):
 
 
         #Calculate error, where x is in orientation of buggy, y is cross track error
-        current_pose = Pose(current_rospose.pose.x, current_rospose.pose.y, heading)
+        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
 

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 util.util import Constants
 
 class Simulator(Node):
 
@@ -63,23 +63,23 @@ 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)
         self.plot_publisher = self.create_publisher(Pose, "sim_2d/utm", 1)
 
         # 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__":

rb_ws/src/buggy/buggy/util/pose.py

@@ -1,8 +1,7 @@
 import numpy as np
 
 from geometry_msgs.msg import Pose as ROSPose
-from tf.transformations import euler_from_quaternion
-from tf.transformations import quaternion_from_euler
+from scipy.spatial.transform import Rotation
 
 
 class Pose:
@@ -28,20 +27,20 @@ def rospose_to_pose(rospose: ROSPose):
         Returns:
             Pose: converted pose
         """
-        (_, _, yaw) = euler_from_quaternion(
+        (_, _, 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 = quaternion_from_euler(0, 0, heading)
+        q = Rotation.from_euler([0, 0, heading]).as_quat()
         return q
 
     def __init__(self, x, y, theta):

rb_ws/src/buggy/buggy/util/trajectory.py

@@ -3,7 +3,7 @@
 import uuid
 import matplotlib.pyplot as plt
 
-from buggy.msg import TrajectoryMsg
+# from buggy.msg import TrajectoryMsg
 
 import numpy as np
 from scipy.interpolate import Akima1DInterpolator, CubicSpline
@@ -58,7 +58,7 @@ def __init__(self, json_filepath=None, positions = None, interpolator="CubicSpli
                 lon = waypoint["lon"]
 
                 # Convert to world coordinates
-                x, y = utm.from_latlon(lat, lon) #TODO: Before Merging, make sure that we can do this nonlocalized
+                x, y, _, _ = utm.from_latlon(lat, lon) #TODO: Before Merging, make sure that we can do this nonlocalized
                 positions.append([x, y])
 
             positions = np.array(positions)
@@ -350,7 +350,7 @@ def get_closest_index_on_path(
             + start_index
         )
 
-    def pack(self, x, y) -> TrajectoryMsg:
+    """ def pack(self, x, y) -> TrajectoryMsg:
         traj = TrajectoryMsg()
         traj.easting = self.positions[:, 0]
         traj.northing = self.positions[:, 1]
@@ -361,5 +361,5 @@ def pack(self, x, y) -> TrajectoryMsg:
     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
+        return Trajectory(positions=pos), cur_idx """
 

rb_ws/src/buggy/launch/sim_2d_single.xml

@@ -1,17 +1,23 @@
 <launch>
 
     <node pkg="foxglove_bridge" exec="foxglove_bridge" name="foxglove" namespace="SC"/>
-    <node pkg="buggy" exec="hello_world" name="hello_world" namespace="SC" />
+
     <node pkg="buggy" exec="sim_single" name="SC_sim_single" namespace="SC">
         <param name="velocity" value="12"/>
         <param name="pose" value="Hill1_SC"/>
     </node>
+    <node pkg="buggy" exec="buggy_state_converter" name="SC_state_converter" namespace="SC"/>
+    <node pkg="buggy" exec="controller" name="SC_controller" namespace="SC">
+        <param name="dist" value="0.0"/>
+        <param name="traj_name" value="buggycourse_safe.json"/>
+        <param name="controller" value="stanley"/>
+    </node>
+
+    <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 pkg="buggy" exec="buggy_state_converter" name="SC_state_converter" namespace="SC"/>
-    <node pkg="buggy" exec="buggy_state_converter" name="NAND_state_converter" namespace="NAND"/>
 
     <!-- <node name="foxglove" pkg="foxglove_bridge" type="foxglove_bridge" />
     <node name="hello_world" pkg="buggy" type="hello_world" /> -->

rb_ws/src/buggy/setup.py

@@ -26,7 +26,7 @@
         'console_scripts': [
             'hello_world = buggy.hello_world:main',
             'sim_single = buggy.simulator.engine:main',
-            'controller = buggy.controller.controller_node:main'
+            'controller = buggy.controller.controller_node:main',
             'buggy_state_converter = buggy.buggy_state_converter:main',
             'watchdog = buggy.watchdog.watchdog_node:main',
         ],