Redtoo dev

phoenix.repos

@@ -39,14 +39,14 @@ repositories:
       type: git
       url: git@github.com:luxonis/depthai-ros.git
       version: 82ce67c
-    polynomial_planner:
+    vectornav:
       type: git
-      url: git@github.com:ISC-Project-Phoenix/Polynomial_planner.git
-      version: master
-    road_detectors:
+      url: https://github.com/dawonn/vectornav.git
+      version: ros2
+    gps_publisher:
       type: git
-      url: git@github.com:ISC-Project-Phoenix/road_detectors.git
-      version: master
+      url: git@github.com:ISC-Project-Phoenix/GPS-publisher.git
+      version: redtoo_dev_experiements
     hybrid_pp:
       type: git
       url: git@github.com:ISC-Project-Phoenix/hybrid_pp.git
@@ -62,4 +62,12 @@ repositories:
     teleop_ack_joy:
       type: git
       url: git@github.com:iscumd/teleop_ack_joy.git
-      version: master
+      version: master
+    design:
+      type: git
+      url:  git@github.com:ISC-Project-Phoenix/design.git
+      version: main
+    teleop_ack_rc:
+      type: git
+      url:  git@github.com:ISC-Project-Phoenix/teleop_ack_rc.git
+      verison: main

phoenix_description/urdf/phoenix.urdf

@@ -377,6 +377,41 @@
         <child link="lidar_link"/>
     </joint>
 
+    <!-- TODO finalize location for GPS -->
+    <link name="vectornav">
+        <visual>
+            <origin rpy="0 0 0" xyz="0 0 0" />
+            <geometry>
+                <box size="0.05 0.05 0.05"/>
+            </geometry>
+        </visual>
+    </link>
+
+    <joint name="vectornav_to_rear_axle" type="fixed">
+        <origin rpy="0 0 0" xyz="1.0 0 0.5"/> 
+        <parent link="rear_axle"/>
+        <child link="vectornav"/>
+    </joint>
+
+    <!-- Dummy link to match the GPS frame_id -->
+    <link name="imu_link">
+        <visual>
+            <geometry>
+                <box size="0.001 0.001 0.001"/>
+            </geometry>
+        </visual>
+        <inertial>
+            <mass value="0.001"/>
+            <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
+        </inertial>
+    </link>
+
+    <joint name="imu_link_to_vectornav" type="fixed">
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <parent link="vectornav"/>
+        <child link="imu_link"/>
+    </joint>
+
 <!-- Gazebo Plugins -->
 
   <gazebo>
@@ -530,6 +565,27 @@
             </ros>
         </device>
 
+        <!-- GPS -->
+        <plugin type="webots_ros2_driver::Ros2IMU">
+            <topicName>/vectornav/imu</topicName>
+            <frameName>vectornav</frameName>
+            <alwaysOn>true</alwaysOn>
+
+            <inertialUnitName>vectornav_inert</inertialUnitName>
+            <gyroName>vectornav_gyro</gyroName>
+            <accelerometerName>vectornav_accel</accelerometerName>
+        </plugin>
+
+        <device reference="vectornav_gps" type="GPS">
+            <ros>
+                <frameName>vectornav</frameName>
+                <topicName>/vectornav/gnss</topicName>
+                <updateRate>10</updateRate>
+                <alwaysOn>true</alwaysOn>
+                <parameter name="gpsReference">40.0 -86.9 0</parameter>
+            </ros>
+        </device>
+
         <!-- Custom driver for odom and actuation-->
         <plugin type="wb_io_ros::WbIoRos" />
     </webots>

phoenix_gazebo/config/navsat_transform/navsat_transform.yaml

@@ -0,0 +1,66 @@
+navsat_transform_node:
+    ros__parameters:
+        # Match the base frame to what your EKF uses
+        base_link_frame: base_footprint
+
+        # Force the subscriber to accept Webots' Best Effort QoS
+        # this is a quick fix to allow GPS to work in the EKF and NAVSAT. 
+        # ideally Reliable is better. TODO
+        qos_overrides:
+          /vectornav/imu:
+            subscription:
+              reliability: best_effort
+          /vectornav/gnss:
+            subscription:
+              reliability: best_effort
+
+# Frequency of the main run loop
+        frequency: 30.0
+
+# Delay time, in seconds, before we calculate the transform from the UTM frame to your world frame. This is especially
+# important if you have use_odometry_yaw set to true. Defaults to 0.
+        delay: 3.0
+
+# PLEASE READ: Like all nodes in robot_localization, this node assumes that your IMU data is reported in the ENU frame.
+# Many IMUs report data in the NED frame, so you'll want to verify that your data is in the correct frame before using
+# it. 
+
+# If your IMU does not account for magnetic declination, enter the value for your location here. If you don't know it,
+# see http://www.ngdc.noaa.gov/geomag-web/ (make sure to convert the value to radians). This parameter is mandatory.
+        magnetic_declination_radians: 0.0
+
+# Your IMU's yaw, once the magentic_declination_radians value is added to it, should report 0 when facing east. If it
+# doesn't, enter the offset here. Defaults to 0.
+        yaw_offset: 0.0
+
+# If this is true, the altitude is set to 0 in the output odometry message. Defaults to false.
+        zero_altitude: true
+
+# If this is true, the transform world_frame->utm transform is broadcast for use by other nodes. Defaults to false.
+        broadcast_utm_transform: false
+
+# If this is true, the utm->world_frame transform will be published instead of the world_frame->utm transform. 
+# Note that broadcast_utm_transform still has to be enabled. Defaults to false.
+        broadcast_utm_transform_as_parent_frame: false
+
+# If this is true, all received odometry data is converted back to a lat/lon and published as a NavSatFix message as
+# /gps/filtered. Defaults to false.
+        publish_filtered_gps: true
+
+# If this is true, the node ignores the IMU data and gets its heading from the odometry source (typically the
+# /odometry/filtered topic coming from one of robot_localization's state estimation nodes). BE CAREFUL when using this!
+# The yaw value in your odometry source *must* be world-referenced, e.g., you cannot use your odometry source for yaw
+# if your yaw data is based purely on integrated velocities. Defaults to false.
+        use_odometry_yaw: false
+
+# If true, will retrieve the datum from the 'datum' parameter below, if available. If no 'datum' parameter exists,
+# navsat_transform_node will wait until the user calls the 'datum' service with the SetDatum service message.
+        wait_for_datum: true
+
+# Instead of using the first GPS location and IMU-based heading for the local-frame origin, users can specify the
+# origin (datum) using this parameter. The fields in the parameter represent latitude and longitude in decimal degrees,
+# and heading in radians. As navsat_transform_node assumes an ENU standard, a 0 heading corresponds to east.
+# wait this might actually be usefully for translating over maps lol -AFB
+# keep in mind this needs to be updated for the actual location of where every the kart is in.
+        datum: [55.944904, -3.186693, 0.0]
+

phoenix_gazebo/config/robot_localization/robot_localization.yaml

@@ -4,7 +4,7 @@ ekf_filter_node:
     # The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
     # computation until it receives at least one message from one of the inputs. It will then run continuously at the
     # frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
-    frequency: 30.0
+    frequency: 60.0
 
     # The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict
     # cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the
@@ -169,18 +169,31 @@ ekf_filter_node:
     # offset from base_link.
 #    odom0_pose_use_child_frame: false
 
-    imu0: /camera/mid/imu
+    imu0: /vectornav/imu
     imu0_config: [false, false, false,
                   true, true,  true,
                   false, false, false,
                   true,  true,  true,
                   true,  true,  true]
+    # MUST BE FALSE so the EKF treats the yaw as an absolute compass heading
     imu0_differential: false
+
+    # MUST BE FALSE so 0 radians is locked to East (standard ENU)
     imu0_relative: false
+
+
     imu0_queue_size: 10
+    imu0_pose_use_child_frame: true
       #imu0_pose_rejection_threshold: 0.8
       #imu0_twist_rejection_threshold: 0.8
       #imu0_linear_acceleration_rejection_threshold: 0.8
+
+    # need for the webots issue. tbh webots is probably the root cause. 
+    # Force the EKF to accept the Best Effort IMU data
+    qos_overrides:
+      /vectornav/imu:
+        subscription:
+          reliability: best_effort
 
     # [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set
     # this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame.

phoenix_gazebo/launch/common.launch.py

@@ -43,6 +43,19 @@ def generate_launch_description():
         }.items(),
         condition=UnlessCondition(use_wheel)
     )
+    # Your RC node
+
+    teleop_ack_rc = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(get_package_share_directory('teleop_ack_rc'),
+                        'launch',
+                        'teleop_ack_rc.launch.py')
+        ),
+        launch_arguments={
+            'use_sim_time': use_sim_time,
+        }.items(),
+        condition=UnlessCondition(use_wheel)  # optional: same condition as teleop_ack_joy
+    )
 
     logi_g29 = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
@@ -133,5 +146,6 @@ def generate_launch_description():
         teleop_ack_joy,
         logi_g29,
         rviz,
-        robot_state_controller
+        robot_state_controller,
+        # teleop_ack_rc
     ])

phoenix_gazebo/launch/include/gps_waypoint_publisher/gps_waypoint_publisher.launch.py

@@ -0,0 +1,63 @@
+# MIT License
+#
+# Copyright (c) 2021 Intelligent Systems Club
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+
+from launch_ros.actions import Node
+from launch.substitutions import PythonExpression
+
+
+
+def generate_launch_description():
+    # Launch arguments
+    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
+
+    # Hardcoded paths, so you should update these to use path join at some point
+    sim_waypoints = '/home/redtoo/Documents/ws-gps-phoenix/src/gps_publisher/data/gps_waypoints_purdue_sim_mk2.txt'
+    real_waypoints = '/home/isc/Documents/dev/phnx_ws_2026/src/gps_publisher/data/gps_waypoints_parking_lot_mk1.txt'
+
+    # This expression picks the real path if use_sim_time is 'false', else sim path
+    chosen_file = PythonExpression([
+        "'", real_waypoints, "' if '", use_sim_time, "' == 'false' else '", sim_waypoints, "'"
+    ])
+
+    plan = Node(
+        package='yet_another_gps_publisher',
+        executable='yet_another_gps_publisher',
+        name='yet_another_gps_publisher',
+        output='screen',
+        parameters=[{
+            'use_sim_time': use_sim_time,
+            'waypoint_file_path': chosen_file
+        }], )
+
+    return LaunchDescription([
+        # Launch Arguments
+        DeclareLaunchArgument('use_sim_time',
+                              default_value='true',
+                              description='Use simulation clock if true'),
+        # Nodes
+        plan,
+
+    ])

phoenix_gazebo/launch/include/robot_localization/robot_localization.launch.py

@@ -31,30 +31,59 @@
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 
 
 def generate_launch_description():
-    # Launch arguments
-    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
-    rl_p = get_package_share_directory('robot_localization')
+    pkg_share = get_package_share_directory('phoenix_robot')
+
+    rl_config = PathJoinSubstitution([pkg_share, 'config', 'robot_localization.yaml'])
+    navsat_config = PathJoinSubstitution([pkg_share, 'config', 'navsat_transform.yaml'])
 
-    rl = Node(
+    # LOCAL EKF (odom -> base_link)
+    # No gps instead a smooth transtion. 
+    local_ekf = Node(
         package='robot_localization',
         executable='ekf_node',
-        name='ekf_filter_node',
+        name='ekf_filter_node_odom',
         output='screen',
-        parameters=[os.path.join(get_package_share_directory("phoenix_gazebo"), 'config', 'robot_localization',
-                                 'robot_localization.yaml')],
-        remappings=[
-            ('/odometry/filtered', '/odom'),
-        ],
+        parameters=[rl_config, {'use_sim_time': LaunchConfiguration('use_sim_time')}],
+        remappings=[('/odometry/filtered', '/odom')]
+    )
+
+    # GLOBAL EKF (map -> odom)
+    # This node provides the 'map' frame. It consumes the GPS odometry.
+    global_ekf = Node(
+        package='robot_localization',
+        executable='ekf_node',
+        name='ekf_filter_node_map',
+        output='screen',
+        parameters=[rl_config, {'use_sim_time': LaunchConfiguration('use_sim_time')}],
+        remappings=[('/odometry/filtered', '/odometry/global')]
     )
 
+    # 3. NAVSAT TRANSFORM NODE
+    # Converts Lat/Lon to X/Y and bridges the GPS into the Global EKF
+    navsat_transform = Node(
+        package='robot_localization',
+        executable='navsat_transform_node',
+        name='navsat_transform',
+        output='screen',
+        parameters=[navsat_config, {'use_sim_time': LaunchConfiguration('use_sim_time')}],
+        remappings=[
+            ('imu', '/phoenix/imu'),           # Data from VectorNav
+            ('gps/fix', '/phoenix/navsat'),    # Data from VectorNav
+            ('odometry/filtered', '/odom'),    # Input from LOCAL EKF
+            ('odometry/gps', '/odometry/gps')  # Output to GLOBAL EKF
+        ]
+    )
     return LaunchDescription([
         # Launch Arguments
         DeclareLaunchArgument('use_sim_time',
                               default_value='true',
                               description='Use simulation clock if true'),
         # Nodes
-        rl,
+        local_ekf,
+        global_ekf,
+        navsat_transform,
     ])