Forgot to switch my local off my fork

phoenix.repos

@@ -39,13 +39,13 @@ 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
+      url: git@github.com:ISC-Project-Phoenix/GPS-publisher.git
       version: master
     hybrid_pp:
       type: git
@@ -63,11 +63,7 @@ repositories:
       type: git
       url: git@github.com:iscumd/teleop_ack_joy.git
       version: master
-    vectornav:
-      type: git
-      url: https://github.com/dawonn/vectornav.git
-      version: ros2
-    gps_publisher:
+    design:
       type: git
-      url: git@github.com:Redderpears/gps-publisher.git
-      version: master
+      url:  git@github.com:ISC-Project-Phoenix/design.git
+      version: main

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

@@ -0,0 +1,51 @@
+# 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
+
+
+def generate_launch_description():
+    # Launch arguments
+    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
+
+    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,
+        }], )
+
+    return LaunchDescription([
+        # Launch Arguments
+        DeclareLaunchArgument('use_sim_time',
+                              default_value='true',
+                              description='Use simulation clock if true'),
+        # Nodes
+        plan,
+
+    ])

phoenix_robot/config/navsat_transform/navsat_transform/not_Sure.yaml

@@ -0,0 +1,51 @@
+navsat_transform_node:
+    ros__parameters:
+# 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: false
+
+# 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: false
+
+# 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
+        datum: [55.944904, -3.186693, 0.0]
+

phoenix_robot/launch/common.launch.py

@@ -145,6 +145,16 @@ def generate_launch_description():
         }.items(),
     )
 
+    GPS_waypoints = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([
+            os.path.join(pkg_phoenix_gazebo, 'launch'),
+            '/include/gps_waypoint_publisher/gps_waypoint_publisher.launch.py' 
+        ]),
+        launch_arguments={
+            'use_sim_time': use_sim_time,
+        }.items(),
+    )
+
     return LaunchDescription([
         # Launch Arguments
         DeclareLaunchArgument(
@@ -173,4 +183,5 @@ def generate_launch_description():
         poly_plan,
         poly_plan_ai,
         vectornav,
+        GPS_waypoints,
     ])

phoenix_robot/launch/include/navsat_transform/navsat_transform.launch.py

@@ -0,0 +1,92 @@
+# 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.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch.conditions import IfCondition, UnlessCondition
+from launch.substitutions import PathJoinSubstitution
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    # 1. Define Arguments
+    # We change the default to include .yaml so we can just join paths easily
+    location_arg = DeclareLaunchArgument(
+        'location_file',
+        default_value='dearborn.yaml', 
+        description='Name of location file (e.g., dearborn.yaml)'
+    )
+
+    use_sim_time_arg = DeclareLaunchArgument(
+        'use_sim_time',
+        default_value='false',
+        description='Use simulation time'
+    )
+
+    # 2. Setup Configuration Variables
+    pkg_phoenix_robot = get_package_share_directory('phoenix_robot')
+    location_file = LaunchConfiguration('location_file')
+    use_sim_time = LaunchConfiguration('use_sim_time')
+
+    # 3. Build Paths using Substitutions (No Python string manipulation needed)
+    common_yaml_path = PathJoinSubstitution([
+        pkg_phoenix_robot, 
+        'config', 'navsat_transform', 'navsat_common.yaml'
+    ])
+
+    location_yaml_path = PathJoinSubstitution([
+        pkg_phoenix_robot, 
+        'config', 'navsat_transform', 'locations', location_file
+    ])
+
+    # 4. Define the Node
+    navsat_node = Node(
+        package='robot_localization', # Fixed package name
+        executable='navsat_transform_node',
+        name='navsat_transform_node',
+        output='screen',
+        parameters=[
+            common_yaml_path,   # Defaults
+            location_yaml_path, # Location Specifics
+            {'use_sim_time': use_sim_time}
+        ],
+        remappings=[
+            ('imu/data', '/vectornav/imu'), 
+            ('gps/fix', '/gps/fix'), 
+            ('odometry/filtered', '/odometry/global'),
+            ('odometry/gps', '/odometry/gps'),
+            ('gps/filtered', '/gps/filtered')
+        ]
+    )
+
+    return LaunchDescription([
+        location_arg,
+        use_sim_time_arg,
+        navsat_node
+    ])

phoenix_robot/launch/include/robot_localization/robot_localization.launch.py

@@ -51,7 +51,21 @@ def generate_launch_description():
             ('/odometry/filtered', '/odom'),
         ],
     )
-
+    # The NavSat Transform Node
+    start_navsat_transform_cmd = Node(
+        package='robot_localization',
+        executable='navsat_transform_node',
+        name='navsat_transform',
+        output='screen',
+        # Point this to your YAML config file!
+        parameters=[os.path.join(get_package_share_directory('phoenix_robot'), 'config', 'navsat_transform.yaml')],
+        remappings=[
+            # these topics need to match what the topic that the robot is using
+            ('imu', '/phoenix/gps/imu'),             # Matches your EKF imu0
+            ('gps/fix', '/phoenix/navsat'),          # From your Vectornav
+            ('odometry/filtered', '/odometry/global') # The output of your EKF
+        ]
+    )
     return LaunchDescription([
         # Launch Arguments
         DeclareLaunchArgument('use_sim_time',
@@ -62,4 +76,5 @@ def generate_launch_description():
                               description='Name of the config file to load'),
         # Nodes
         rl,
+        start_navsat_transform_cmd,
     ])

phoenix_robot/launch/include/vectornav/vectornav.launch.py

@@ -58,6 +58,7 @@ def generate_launch_description():
         package='vectornav',
         executable='vn_sensor_msgs',
         output='screen',
+        # confirm remappings for the vectornav.
         remappings=[
             ('/vectornav/imu', '/phoenix/imu'),
             ('vectornav/gnss', '/phoenix/navsat'),