Comp run 1

launch/auto.launch.py

@@ -28,13 +28,19 @@ def generate_launch_description():
         name='imu'
     )
 
-
+    #AprilTag detection node
+    start_apriltag_node = Node(
+        package='jetson_pkg',
+        executable='apriltag',
+        name='apriltag'
+    )
 
     #Declare launch description and populate
     ld = LaunchDescription()
 
     #declare launch actions
     ld.add_action(start_axle_manager_node)
     ld.add_action(start_auto_node)
+    ld.add_action(start_apriltag_node)
 
     return ld

launch/fqr.launch.py

@@ -28,7 +28,7 @@ def generate_launch_description():
         name='imu'
     )
 
-
+    
 
     #Declare launch description and populate
     ld = LaunchDescription()

rosinstall.sh

@@ -13,9 +13,9 @@ sudo apt update
 sudo apt install ros-dev-tools
 sudo apt update
 sudo apt upgrade
-sudo apt install ros-iron-ros-base
-source /opt/ros/iron/setup.bash
-echo "source /opt/ros/iron/setup.bash" >> ~/.bashrc
+sudo apt install ros-jazzy-ros-base
+source /opt/ros/jazzy/setup.bash
+echo "source /opt/ros/jazzy/setup.bash" >> ~/.bashrc
 echo "export ROS_DOMAIN_ID=42" >> ~/.bashrc
-sudo apt install python3-colcon-common-extensions python3-pip ros-iron-joy-linux
+sudo apt install python3-colcon-common-extensions python3-pip ros-jazzy-joy-linux
 pip install setuptools==58.2.0 serial

src/axle_manager/axle_manager/hdc2460_node.py

@@ -18,7 +18,7 @@ def __init__(self):
                 ('serialBitRate',115200),
                 ('leftChannel',1),
                 ('rightChannel',2),
-                ('maxSpeed',500),
+                ('maxSpeed',500/2),
                 ('accelRate',31860),
                 ('brakeRate',10620),
                 ('pivotDevice',"FAC"),
@@ -50,11 +50,11 @@ def __init__(self):
         rightChannel_param = self.get_parameter('rightChannel').value
         rightChannel = int(rightChannel_param) if rightChannel_param is not None else 2
         maxSpeed_param = self.get_parameter('maxSpeed').value
-        maxSpeed = int(maxSpeed_param) if maxSpeed_param is not None else 500
+        maxSpeed = int(maxSpeed_param) if maxSpeed_param is not None else 250 #500 (changed 12/8/2025)
         accelRate_param = self.get_parameter('accelRate').value
-        accelRate = int(accelRate_param) if accelRate_param is not None else 31860
+        accelRate = int(accelRate_param) if accelRate_param is not None else 15930 #31860 (changed 12/8/2025)
         brakeRate_param = self.get_parameter('brakeRate').value
-        brakeRate = int(brakeRate_param) if brakeRate_param is not None else 10620
+        brakeRate = int(brakeRate_param) if brakeRate_param is not None else 5310 #10620 (changed 12/8/2025)
         self.pivotDevice = str(self.get_parameter('pivotDevice').value)
         pivotLeft_param = self.get_parameter('pivotExtendChannel').value
         self.pivotLeft = int(pivotLeft_param) if pivotLeft_param is not None else 1

src/axle_manager/axle_manager/location_calculate_node.py

@@ -0,0 +1,162 @@
+import rclpy 
+from rclpy.node import Node
+
+from .wheel_odometry_interpret import update_odometry
+from geometry_msgs.msg import Twist, Point
+from sensor_msgs.msg import Imu
+import math
+
+
+class LocationCalculate(Node):
+    def __init__(self):
+        super().__init__('location_calculate_publisher')
+        self.publisher_ = self.create_publisher(Point, '/location_calculate', 10)
+
+
+        #start publisher callback with timer
+        self.time = 0.0
+        self.timer = self.create_timer(0.1, self.callback)
+
+
+        #initialize imu data
+        self.x_imu = 0.0
+        self.y_imu = 0.0
+        self.orientation_imu = 0.0
+        self.last_x_imu = 0.0
+        self.last_y_imu = 0.0
+        self.last_o_imu = 0.0
+        self.time_imu = 0.0
+
+        #start IMU subscriber
+        self.create_subscription(Imu , 'interpreted_imu', self.sub_callback_IMU, 10)
+
+        #initialize april tag data
+        self.x_april = 0.0
+        self.y_april = 0.0
+        self.orientation_april = 0.0
+        self.time_april = 0.0
+
+        #start april tag subscriber
+        self.create_subscription(Twist, '/apriltag', self.sub_callback_april, 10)
+
+
+        #inintialize wheel odometry data
+        self.left_speed = 0.0
+        self.right_speed = 0.0
+        self.time_wheel_odom = 0.0
+
+        #start wheel speed subscription
+        self.create_subscription(Twist , '/wheel/speed', self.sub_callback_odom, 10)
+
+
+
+    def sub_callback_IMU(self, msg:Imu):
+        self.get_logger().info("Get IMU data")
+
+        #set equal to published imu data
+        self.x_imu = msg.orientation.x
+        self.y_imu = msg.orientation.y
+
+        # convert to change in angle
+        angle_change = msg.angular_velocity * (180 / math.pi) * .1
+
+        self.orientation_imu += angle_change
+
+        #set equal to timer (time since last update)
+        self.time_imu = self.time
+
+    def sub_callback_april(self, msg:Twist):
+        self.get_logger().info("Get apriltag data")
+
+        #set equal to published april tag data
+        self.x_april = msg.linear.x
+        self.y_april = msg.linear.z
+        self.orientation_april = msg.angular.y
+
+        #set equal to timer (time since last update)
+        self.time_april = self.time
+
+    def sub_callback_odom(self , msg:Twist):
+        self.get_logger().info("Get odom data")
+
+        #subscriber callback for wheel odometry
+        #Set wheel speed in RPM for left and right wheels
+        self.left_speed = msg.linear.x
+        self.right_speed = msg.linear.y
+
+        #set equal to timer (time since last update)
+        self.time_wheel_odom = self.time
+
+    def callback(self):
+        self.time += 0.1
+
+        #set wheel odometry data
+        #make publisher in hdc2460_node
+
+        #test to see which data method to use. first april tag, then IMU, then wheel odometry
+        if self.time_april > self.time - 0.2:
+            self.location_x = self.x_april
+            self.location_y = self.y_april
+            self.orientation = self.orientation_april
+
+            #remember imu position at last april tag read for IMU error when there is no april tag
+            self.last_x_imu = self.x_imu
+            self.last_y_imu = self.y_imu
+            self.last_o_imu = self.orientation_imu
+
+
+        # if no recent april tag use IMU
+        elif self.time_imu > self.time - 0.2:
+            #use the difference of last IMU and current IMU as well as the last april tag read to find current location
+            shift_dist = math.sqrt(math.pow(self.x_imu - self.last_x_imu, 2) + math.pow(self.y_imu - self.last_y_imu, 2))
+            shift_rel_dir = math.atan((self.y_imu - self.last_y_imu)/(self.x_imu - self.last_x_imu)) - (self.last_o_imu * math.pi / 180)
+            shift_ori = self.orientation_imu - self.last_o_imu
+
+            self.location_x = math.cos(shift_rel_dir + self.orientation_april * math.pi / 180) * shift_dist + self.x_april
+            self.location_y = math.sin(shift_rel_dir + self.orientation_april * math.pi / 180) * shift_dist + self.y_april
+            self.orientation = shift_ori + self.orientation_april
+
+
+
+        #if no imu/april tages use wheel odometry    
+        else:
+            change_orientation, distance_avg, direct = update_odometry(self.left_speed, self.right_speed)
+            #use wheel odometry
+            # If going straight
+            if change_orientation == 0:
+                #update just location
+                self.location_y = self.location_y + distance_avg * math.sin(self.orientation * math.pi / 180)
+                self.location_x = self.location_x + distance_avg * math.cos(self.orientation * math.pi / 180)
+            # if not going straight
+            else:
+                #updates location x,y
+                self.location_y = self.location_y + math.sin((self.orientation + .5 * change_orientation) * math.pi / 180) * distance_avg * direct
+                self.location_x = self.location_x + math.cos((self.orientation + .5 * change_orientation) * math.pi / 180) * distance_avg * direct
+                #updates orientation
+                self.orientation = (self.orientation + change_orientation) % 360
+
+        msg = Point()
+
+        #update "true" location
+        msg.x = self.location_x
+        msg.y = self.location_y
+        msg.z = self.orientation
+
+        self.get_logger().info("Publishing location calculate")
+        self.publisher_.publish(msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    location_calculate_node = LocationCalculate()
+
+    rclpy.spin(location_calculate_node)
+
+    location_calculate_node.destroy_node()
+
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/axle_manager/axle_manager/wheel_odometry_interpret.py

@@ -0,0 +1,62 @@
+import math
+
+#angle with reference to start direction maybe radians maybe degrees
+# orientation = 90
+
+#location relative to start in m
+#
+#+y = foreward, -y = backwards at orientation 90
+#+x = right, -x = left at orientation 90
+# location_x = 0
+# location_y = 0
+
+#minutes between signals
+# time_frame = 1/600
+
+#wheel circumference in meters
+wc = 0.43 * math.pi
+
+#wheel track in meters
+wt = .915
+
+def update_odometry(left:float, right:float, time_frame=1/600):
+    #use subscription to sensor_picker to grap accurate position and orientation
+
+    #put in to update right and left here
+
+
+    #math to determine variables
+    distance_right = right * time_frame * wc
+    distance_left = left * time_frame * wc
+
+    change_orientation = (distance_right - distance_left) / wt
+
+    distance_avg = (distance_left + distance_right) / 2.0
+
+
+    if distance_left != distance_right:
+        turning_radius = distance_left / change_orientation + wt / 2.0
+        direct = 2 * turning_radius * turning_radius * (1 - math.cos(change_orientation * math.pi / 180)) / (change_orientation / 360 * turning_radius)
+    else:
+        direct = 0.0
+
+    return change_orientation, distance_avg, direct
+
+
+    # If going straight
+    # if distance_left == 0:
+    #     #update just location
+    #     location_y = location_y + distance_avg * math.sin(orientation * math.pi / 180)
+    #     location_x = location_x + distance_avg * math.cos(orientation * math.pi / 180)
+    # else:
+    #     #direct is the ratio of direct distance to the distance along a circle
+    #     direct = 2 * turning_radius * turning_radius * (1 - math.cos(change_orientation * math.pi / 180)) / (change_orientation/360 * turning_radius)
+
+    #     #updates location x,y
+    #     location_y = location_y + math.sin((orientation + .5 * change_orientation) * math.pi / 180) * distance_avg * direct
+    #     location_x = location_x + math.cos((orientation + .5 * change_orientation) * math.pi / 180) * distance_avg * direct
+    #     #updates orientation
+    #     orientation = (orientation + change_orientation) % 360
+
+    #publish location and orientation
+

src/axle_manager/setup.py

@@ -22,7 +22,8 @@
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
-             'axle_manager = axle_manager.hdc2460_node:main'
+            'axle_manager = axle_manager.hdc2460_node:main',
+            'location_calculate = axle_manager.location_calculate_node:main'
         ],
     },
-)
+)