GPS Conversion and AprilTag comparison

launch/auto.launch.py

@@ -9,7 +9,7 @@ def generate_launch_description():
     #Auto control node
     start_auto_node = Node(
         package='control_pkg',
-        executable='auto',
+        executable='old_auto',
         name='auto'
     )
 
@@ -28,13 +28,39 @@ def generate_launch_description():
         name='imu'
     )
 
+    #AprilTag detection node
+    start_apriltag_node = Node(
+        package='jetson_pkg',
+        executable='apriltag',
+        name='apriltag',
+        parameters=[
+            {'cap':'rtsp://admin:hyflex@192.168.1.131:80/cam/realmonitor?channel=1&subtype=0'},
+            {'fx':1071.1362274102335},
+            {'fy':1102.1406887400624},
+            {'cx':953.030188084331},
+            {'cy':468.0382502048589}
+        ]
+    )
+
+    #location calculate node
+    start_location_node = Node(
+        package='control_pkg',
+        executable='location_calculate',
+        name='location_node'
+    )
+
+
 
 
     #Declare launch description and populate
     ld = LaunchDescription()
 
+    #declare launch actions
+    ld.add_action(start_location_node)
+
     #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()

launch/lidar.launch.py

@@ -6,27 +6,33 @@
 def generate_launch_description():
     sick_scan_pkg_prefix = get_package_share_directory('sick_scan_xd')
     lms_launch_file_path = os.path.join(sick_scan_pkg_prefix, 'launch/sick_lms_1xx.launch')
-    laser_scan_node = Node(
+    laserscan_node = Node(
         package='sick_scan_xd',
         executable='sick_generic_caller',
         output='screen',
         arguments=[
             lms_launch_file_path,
             'hostname:=192.168.1.134',
-            'tf_base_frame_id:=lidar_laser',
             'scan_freq:=25',
             'ang_res:=0.5',
-            ]
+            'tf_base_frame_id:=lidar_laser'
+        ]
     )
 
+    location_calc_node = Node(
+        package="axle_manager",
+        executable="location_calculate"
+    )
+
     obs_node = Node(
         package='sensors_pkg',
         executable='obs',
         name='obs',
     )
 
     return LaunchDescription([
-        laser_scan_node,
+        laserscan_node,
+        location_calc_node,
         obs_node
     ])
 

launch/waypoint_auto.launch.py

@@ -0,0 +1,18 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    start_waypoint = Node(
+        package='waypoint',
+        executable='waypoint',
+        name='waypoint'
+    )
+
+    #Auto control node
+    start_auto_node = Node(
+        package='control_pkg',
+        executable='auto',
+        name='auto'
+    )
+
+    return LaunchDescription([start_waypoint, start_auto_node])

requirements.txt

@@ -0,0 +1,3 @@
+apriltag
+cv2
+numpy

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

@@ -19,8 +19,8 @@ def __init__(self):
                 ('leftChannel',1),
                 ('rightChannel',2),
                 ('maxSpeed',500),
-                ('accelRate',31860),
-                ('brakeRate',10620),
+                ('accelRate',31860 // 6),
+                ('brakeRate',10620 // 6),
                 ('pivotDevice',"FAC"),
                 ('pivotExtendChannel',1),
                 ('pivotRetractChannel',2),
@@ -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/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'
         ],
     },
-)
+)