From 0ab5e828904923d8b4fb289514509ce514e57c66 Mon Sep 17 00:00:00 2001 From: redto0 Date: Wed, 7 Jan 2026 17:22:50 -0500 Subject: [PATCH 01/45] adding repo depancy --- phoenix.repos | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/phoenix.repos b/phoenix.repos index de83e8f..184621a 100644 --- a/phoenix.repos +++ b/phoenix.repos @@ -62,4 +62,8 @@ repositories: teleop_ack_joy: type: git url: git@github.com:iscumd/teleop_ack_joy.git - version: master \ No newline at end of file + version: master + vectornav: + type: git + url: https://github.com/dawonn/vectornav.git + version: ros2 \ No newline at end of file From 0da2001411834b73f631488d9e5d00ecfdff4fa7 Mon Sep 17 00:00:00 2001 From: redto0 Date: Thu, 8 Jan 2026 01:02:09 -0500 Subject: [PATCH 02/45] FEAT: Adding vectornav launch files --- phoenix_robot/config/vectornav/vectornav.yaml | 81 ++++++++++++++++++ .../include/vectornav/vectornav.launch.py | 83 +++++++++++++++++++ 2 files changed, 164 insertions(+) create mode 100644 phoenix_robot/config/vectornav/vectornav.yaml create mode 100644 phoenix_robot/launch/include/vectornav/vectornav.launch.py diff --git a/phoenix_robot/config/vectornav/vectornav.yaml b/phoenix_robot/config/vectornav/vectornav.yaml new file mode 100644 index 0000000..558a69e --- /dev/null +++ b/phoenix_robot/config/vectornav/vectornav.yaml @@ -0,0 +1,81 @@ +# borrowed form ISC-kilo-OHM +# make changes accordingly + +vectornav: + ros__parameters: + # port: "/dev/ttyUSB0" #TODO make this correct + # raw line form my device /dev/serial/by-id/usb-FTDI_USB-RS232_Cable_FTZ8EVOV-if00-port0 + # TODO vertify on phoenix pc. + port: "/dev/serial/by-id/usb-FTDI_USB-RS232_Cable_FTZ8EVOV-if00-port*" + baud: 115200 + reconnect_ms: 500 + # Reference vnproglib-1.2.0.0 headers for enum definitions + # Async Output Type (ASCII) + AsyncDataOutputType: 0 # VNOFF + # Async output Frequency (Hz) + AsyncDataOutputFrequency: 20 + # Sync control + syncInMode: 3 # SYNCINMODE_COUNT + syncInEdge: 0 # SYNCINEDGE_RISING + syncInSkipFactor: 0 + syncOutMode: 0 # SYNCOUTMODE_NONE + syncOutPolarity: 0 # SYNCOUTPOLARITY_NEGATIVE + syncOutSkipFactor: 0 + syncOutPulseWidth_ns: 100000000 + # Communication protocol control + serialCount: 0 # COUNTMODE_NONE + serialStatus: 0 # STATUSMODE_OFF + spiCount: 0 # COUNTMODE_NONE + spiStatus: 0 # STATUSMODE_OFF + serialChecksum: 1 # CHECKSUMMODE_CHECKSUM + spiChecksum: 0 # CHECKSUMMODE_OFF + errorMode: 1 # ERRORMODE_SEND + # Binary output register 1 + BO1: + asyncMode: 3 # ASYNCMODE_BOTH + rateDivisor: 40 # 20 Hz + commonField: 0x7FFF + timeField: 0x0000 # TIMEGROUP_NONE + imuField: 0x0000 # IMUGROUP_NONE + # set gpsField directly in source to enforce or condition + attitudeField: 0x0000 # ATTITUDEGROUP_NONE + # set insField directly in source to enforce or condition + gps2Field: 0x0000 # GPSGROUP_NONE + # Binary output register 2 + BO2: + asyncMode: 0 # ASYNCMODE_NONE + rateDivisor: 40 + commonField: 0x7FFF # COMMONGROUP_NONE + timeField: 0x0000 # TIMEGROUP_NONE + imuField: 0x0000 # IMUGROUP_NONE + gpsField: 0x0000 # GPSGROUP_NONE + attitudeField: 0x0000 # ATTITUDEGROUP_NONE + insField: 0x0000 # INSGROUP_NONE + gps2Field: 0x0000 # GPSGROUP_NONE + # Binary output register 3 + BO3: + asyncMode: 0 # ASYNCMODE_NONE + rateDivisor: 0 + commonField: 0x0000 # COMMONGROUP_NONE + timeField: 0x0000 # TIMEGROUP_NONE + imuField: 0x0000 # IMUGROUP_NONE + gpsField: 0x0000 # GPSGROUP_NONE + attitudeField: 0x0000 # ATTITUDEGROUP_NONE + insField: 0x0000 # INSGROUP_NONE + gps2Field: 0x0000 # GPSGROUP_NONE + frame_id: "imu_link" #TODO make sure correct + +vn_sensor_msgs: + ros__parameters: + orientation_covariance: [0.01, 0.0, 0.0, + 0.0, 0.01, 0.0, + 0.0, 0.0, 0.01] + angular_velocity_covariance: [0.01, 0.0, 0.0, + 0.0, 0.01, 0.0, + 0.0, 0.0, 0.01] + linear_acceleration_covariance: [0.01, 0.0, 0.0, + 0.0, 0.01, 0.0, + 0.0, 0.0, 0.01] + magnetic_covariance: [0.01, 0.0, 0.0, + 0.0, 0.01, 0.0, + 0.0, 0.0, 0.01] \ No newline at end of file diff --git a/phoenix_robot/launch/include/vectornav/vectornav.launch.py b/phoenix_robot/launch/include/vectornav/vectornav.launch.py new file mode 100644 index 0000000..ab5b08d --- /dev/null +++ b/phoenix_robot/launch/include/vectornav/vectornav.launch.py @@ -0,0 +1,83 @@ +# 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 +from nav2_common.launch import RewrittenYaml + +def generate_launch_description(): + # Launch arguments + use_sim_time = LaunchConfiguration('use_sim_time', default='false') + config_file_name = LaunchConfiguration('config_file_name') + configured_params = RewrittenYaml(source_file=vn300_conf, + root_key='', + param_rewrites=param_substitutions, + convert_types=True) + + + # Vectornav + start_vectornav_cmd = Node( + package='vectornav', + executable='vectornav', + name='vectornav', + output='screen', + parameters=[PathJoinSubstitution( + [get_package_share_directory('phoenix_robot'), 'config', 'vectornav', config_file_name]) + ], + remappings=[ + ('/odometry/filtered', '/odom'), + ], + ) + + # Node that converts raw vectornav data to ros msgs + start_vectornav_sensor_msgs_cmd = Node( + package='vectornav', + executable='vn_sensor_msgs', + output='screen', + remappings=[('/vectornav/imu', '/kohm/gps/imu'), ('vectornav/gnss', + '/kohm/navsat'), ('/vectornav/magnetic', '/kohm/mag')], + parameters=[PathJoinSubstitution( + [get_package_share_directory('phoenix_robot'), 'config', 'vn_sensor_msgs', config_file_name])], + ) + + return LaunchDescription([ + # Launch Arguments + DeclareLaunchArgument('use_sim_time', + default_value='false', + description='Use simulation clock if true'), + DeclareLaunchArgument('config_file_name', + default_value='robot_localization.yaml', + description='Name of the config file to load'), + # Nodes + start_vectornav_cmd, + start_vectornav_sensor_msgs_cmd, + ]) From 78301e72b850c7b6a11b5307f2ae6527385e4027 Mon Sep 17 00:00:00 2001 From: redto0 Date: Thu, 8 Jan 2026 01:03:13 -0500 Subject: [PATCH 03/45] FEAT: Adding vect nav to phoenix_robot_launch --- phoenix_robot/CMakeLists.txt | 1 + phoenix_robot/launch/common.launch.py | 16 ++++++++++++++-- phoenix_robot/package.xml | 1 + 3 files changed, 16 insertions(+), 2 deletions(-) diff --git a/phoenix_robot/CMakeLists.txt b/phoenix_robot/CMakeLists.txt index 896bca1..dd96cf0 100644 --- a/phoenix_robot/CMakeLists.txt +++ b/phoenix_robot/CMakeLists.txt @@ -19,6 +19,7 @@ find_package(phnx_io_ros REQUIRED) find_package(twist_to_ackermann REQUIRED) find_package(robot_localization REQUIRED) find_package(rtabmap_odom REQUIRED) +find_package(vectornav REQUIRED) find_package(hybrid_pp REQUIRED) find_package(polynomial_planner REQUIRED) diff --git a/phoenix_robot/launch/common.launch.py b/phoenix_robot/launch/common.launch.py index 1bd3025..7b29311 100644 --- a/phoenix_robot/launch/common.launch.py +++ b/phoenix_robot/launch/common.launch.py @@ -135,6 +135,17 @@ def generate_launch_description(): condition=UnlessCondition(use_ai) ) + vectornav = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + os.path.join(pkg_phoenix_robot, 'launch'), + '/include/vectornav/vectornav.launch.py' + ]), + launch_arguments={ + 'use_sim_time': use_sim_time, + }.items(), + condition=UnlessCondition(use_ai) + ) + return LaunchDescription([ # Launch Arguments DeclareLaunchArgument( @@ -155,11 +166,12 @@ def generate_launch_description(): robot_state_controller, state_publishers, ekf, - camera, + # camera, pir, pp, obj_detector_ai, obj_detector_cv, poly_plan, - poly_plan_ai + poly_plan_ai, + vectornav, ]) \ No newline at end of file diff --git a/phoenix_robot/package.xml b/phoenix_robot/package.xml index fcf58a7..8d9fa25 100644 --- a/phoenix_robot/package.xml +++ b/phoenix_robot/package.xml @@ -23,6 +23,7 @@ twist_to_ackermann robot_localization rtabmap_odom + vectornav hybrid_pp From c3bfa92b6ed6bcc409551d0531b2661050c98599 Mon Sep 17 00:00:00 2001 From: redto0 Date: Thu, 8 Jan 2026 01:31:05 -0500 Subject: [PATCH 04/45] BUG: fixing the parameters i copy pasted --- phoenix_robot/launch/common.launch.py | 1 - phoenix_robot/launch/include/vectornav/vectornav.launch.py | 7 +------ 2 files changed, 1 insertion(+), 7 deletions(-) diff --git a/phoenix_robot/launch/common.launch.py b/phoenix_robot/launch/common.launch.py index 7b29311..9b132df 100644 --- a/phoenix_robot/launch/common.launch.py +++ b/phoenix_robot/launch/common.launch.py @@ -143,7 +143,6 @@ def generate_launch_description(): launch_arguments={ 'use_sim_time': use_sim_time, }.items(), - condition=UnlessCondition(use_ai) ) return LaunchDescription([ diff --git a/phoenix_robot/launch/include/vectornav/vectornav.launch.py b/phoenix_robot/launch/include/vectornav/vectornav.launch.py index ab5b08d..04b8c3e 100644 --- a/phoenix_robot/launch/include/vectornav/vectornav.launch.py +++ b/phoenix_robot/launch/include/vectornav/vectornav.launch.py @@ -32,16 +32,11 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from nav2_common.launch import RewrittenYaml def generate_launch_description(): # Launch arguments use_sim_time = LaunchConfiguration('use_sim_time', default='false') config_file_name = LaunchConfiguration('config_file_name') - configured_params = RewrittenYaml(source_file=vn300_conf, - root_key='', - param_rewrites=param_substitutions, - convert_types=True) # Vectornav @@ -75,7 +70,7 @@ def generate_launch_description(): default_value='false', description='Use simulation clock if true'), DeclareLaunchArgument('config_file_name', - default_value='robot_localization.yaml', + default_value='vectrnav.yaml', description='Name of the config file to load'), # Nodes start_vectornav_cmd, From 6ee923630763330edb2451c2854721eb7d028c16 Mon Sep 17 00:00:00 2001 From: redto0 Date: Thu, 8 Jan 2026 01:42:01 -0500 Subject: [PATCH 05/45] BUG FIX? Removed * from vectornav dev path --- phoenix_robot/config/vectornav/vectornav.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/phoenix_robot/config/vectornav/vectornav.yaml b/phoenix_robot/config/vectornav/vectornav.yaml index 558a69e..f69900a 100644 --- a/phoenix_robot/config/vectornav/vectornav.yaml +++ b/phoenix_robot/config/vectornav/vectornav.yaml @@ -6,7 +6,7 @@ vectornav: # port: "/dev/ttyUSB0" #TODO make this correct # raw line form my device /dev/serial/by-id/usb-FTDI_USB-RS232_Cable_FTZ8EVOV-if00-port0 # TODO vertify on phoenix pc. - port: "/dev/serial/by-id/usb-FTDI_USB-RS232_Cable_FTZ8EVOV-if00-port*" + port: "/dev/serial/by-id/usb-FTDI_USB-RS232_Cable_FTZ8EVOV-if00-port0" baud: 115200 reconnect_ms: 500 # Reference vnproglib-1.2.0.0 headers for enum definitions From 2f856b8ee9c3a796bf3f45146f77a3968e31c108 Mon Sep 17 00:00:00 2001 From: redto0 Date: Sat, 10 Jan 2026 15:58:09 -0500 Subject: [PATCH 06/45] adding the ekf config for the gps --- .../robot_localization_gps_encoder_imu.yaml | 278 ++++++++++++++++++ 1 file changed, 278 insertions(+) create mode 100644 phoenix_robot/config/robot_localization/robot_localization_gps_encoder_imu.yaml diff --git a/phoenix_robot/config/robot_localization/robot_localization_gps_encoder_imu.yaml b/phoenix_robot/config/robot_localization/robot_localization_gps_encoder_imu.yaml new file mode 100644 index 0000000..370bd38 --- /dev/null +++ b/phoenix_robot/config/robot_localization/robot_localization_gps_encoder_imu.yaml @@ -0,0 +1,278 @@ + +### ekf config file ### +ekf_filter_node: + ros__parameters: + # 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 + + # 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 + # filter will generate new output. Defaults to 1 / frequency if not specified. + #sensor_timeout: 1.0 + + # ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is + # set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar + # environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected + # by, for example, an IMU. Defaults to false if unspecified. + two_d_mode: true + + # Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for + # future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if + # unspecified. + transform_time_offset: 0.0 + + # Use this parameter to provide specify how long the tf listener should wait for a transform to become available. + # Defaults to 0.0 if unspecified. + transform_timeout: 0.0 + + # If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is + # unhappy with any settings or data. + print_diagnostics: true + + # Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by + # debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious + # effects on the performance of the node. Defaults to false if unspecified. + debug: false + + # Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path. + #debug_out_file: /path/to/debug/file.txt + + # Whether we'll allow old measurements to cause a re-publication of the updated state + permit_corrected_publication: false + + # Whether to publish the acceleration state. Defaults to false if unspecified. + publish_acceleration: false + + # Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified. + publish_tf: true + + # REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and + # earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames. + # The robot's position in the odom frame will drift over time, but is accurate in the short term and should be + # continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom + # frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your + # robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based + # localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame. + # ekf_localization_node and ukf_localization_node are not concerned with the earth frame. + # Here is how to use the following settings: + # 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system. + # 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of + # odom_frame. + # 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set + # "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes. + # 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates + # from landmark observations) then: + # 3a. Set your "world_frame" to your map_frame value + # 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state + # estimation node from robot_localization! However, that instance should *not* fuse the global data. + #map_frame: map # Defaults to "map" if unspecified + odom_frame: odom # Defaults to "odom" if unspecified + base_link_frame: base_footprint # Defaults to "base_link" if unspecified + world_frame: odom # Defaults to the value of odom_frame if unspecified + + # The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry, + # geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped, + # sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0, + # odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no + # default values, and must be specified. + odom0: /odom_can # Just fuse vel from encoder + + # Each sensor reading updates some or all of the filter's state. These options give you greater control over which + # values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only + # want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the + # values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types + # do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message + # has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false + # if unspecified, effectively making this parameter required for each sensor. + odom0_config: [false, false, false, # x y z + false, false, false, # r p y + true, true, false, # vx vy vz + false, false, false, # vr vp vy + false, false, false] # ax ay az + + # If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase + # the size of the subscription queue so that more measurements are fused. + odom0_queue_size: 10 + + # [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under- + # report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they + # arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also + # measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't + # always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose + # data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then + # integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true + # for twist measurements has no effect. + odom0_differential: false + + # [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point" + # for all future measurements. While you can achieve the same effect with the differential paremeter, the key + # difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before + # integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true. + odom0_relative: false + + # [ADVANCED] Whether to use the starting pose of child_frame_id as the origin of odometry. + # Note: this is different from setting odom0_relative to true, as when child_frame is different from + # base_link_frame, the rotation of base_link will be coupled into the translation of child_frame. + # Set to true for fusing secondary odometry sources that are rigidly connected to base_link but has a non-zero + # offset from base_link. + odom0_pose_use_child_frame: false + + # [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to + # control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to + # numeric_limits::max() if unspecified. It is strongly recommended that these parameters be removed if not + # required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation. + # For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying + # the thresholds. + #odom0_pose_rejection_threshold: 5.0 + #odom0_twist_rejection_threshold: 1.0 + + # odom1: /odom_tracker TODO add when obj tracker updated + + # Each sensor reading updates some or all of the filter's state. These options give you greater control over which + # values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only + # want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the + # values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types + # do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message + # has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false + # if unspecified, effectively making this parameter required for each sensor. + # odom1_config: [false, false, false, # x y z + # false, false, false, # r p y + # true, false, false, # vx vy vz + # false, false, false, # vr vp vy + # false, false, false] # ax ay az + + # If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase + # the size of the subscription queue so that more measurements are fused. + # odom1_queue_size: 2 + + # [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under- + # report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they + # arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also + # measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't + # always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose + # data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then + # integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true + # for twist measurements has no effect. + # odom0_differential: false + + # [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point" + # for all future measurements. While you can achieve the same effect with the differential paremeter, the key + # difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before + # integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true. + # odom0_relative: false + + # [ADVANCED] Whether to use the starting pose of child_frame_id as the origin of odometry. + # Note: this is different from setting odom0_relative to true, as when child_frame is different from + # base_link_frame, the rotation of base_link will be coupled into the translation of child_frame. + # Set to true for fusing secondary odometry sources that are rigidly connected to base_link but has a non-zero + # offset from base_link. + # odom0_pose_use_child_frame: false + + imu0: /camera/mid/imu + imu0_config: [false, false, false, + true, true, true, + false, false, false, + true, true, true, + true, true, true] + imu0_differential: false + imu0_relative: false + imu0_queue_size: 10 + #imu0_pose_rejection_threshold: 0.8 + #imu0_twist_rejection_threshold: 0.8 + #imu0_linear_acceleration_rejection_threshold: 0.8 + + # [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. + imu0_remove_gravitational_acceleration: true # We need this for the oak-d and gazebo + + # imu1: /camera/right/imu + # imu1_config: [false, false, false, + # true, true, true, + # false, false, false, + # true, true, true, + # true, true, true] + # imu1_differential: false + # imu1_relative: false + # imu1_queue_size: 10 + # #imu0_pose_rejection_threshold: 0.8 + # #imu0_twist_rejection_threshold: 0.8 + # #imu0_linear_acceleration_rejection_threshold: 0.8 + # + # # [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. + # imu1_remove_gravitational_acceleration: true # We need this for the oak-d and gazebo + + # [ADVANCED] The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no + # acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During + # correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be + # problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When + # this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially + # noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance + # for the velocity variable in question, or decrease the variance of the variable in question in the measurement + # itself. In addition, users can also take advantage of the control command being issued to the robot at the time we + # make the prediction. If control is used, it will get converted into an acceleration term, which will be used during + # predicition. Note that if an acceleration measurement for the variable in question is available from one of the + # inputs, the control term will be ignored. + # Whether or not we use the control input during predicition. Defaults to false. + #use_control: false # cannot use because we are ackermann + + # Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to + # false. + #stamped_control: false + + # The last issued control command will be used in prediction for this period. Defaults to 0.2. + #control_timeout: 0.2 + + # Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw. + #control_config: [true, false, false, false, false, true] + + # Places limits on how large the acceleration term will be. Should match your robot's kinematics. + #acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4] + + # Acceleration and deceleration limits are not always the same for robots. + #deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5] + + # If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these + # gains + #acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9] + + # If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these + # gains + #deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0] + + # [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is + # exposed as a configuration parameter. This matrix represents the noise we add to the total error after each + # prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be. + # However, if users find that a given variable is slow to converge, one approach is to increase the + # process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error + # to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are + # ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if + # unspecified. + # Note: the specification of covariance matrices can be cumbersome, so all matrix parameters in this package support + # both full specification or specification of only the diagonal values. + process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015] + + # [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal + # value (variance) to a large value will result in rapid convergence for initial measurements of the variable in + # question. Users should take care not to use large values for variables that will not be measured directly. The values + # are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the diagonal values below + # if unspecified. In this example, we specify only the diagonal of the matrix. + initial_estimate_covariance: [1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9] + + use_sim_time: false From 0b08a9371b51c4bc8812cfb49769fc42a937bf00 Mon Sep 17 00:00:00 2001 From: redto0 Date: Sat, 10 Jan 2026 17:24:04 -0500 Subject: [PATCH 07/45] typo fix --- .../launch/include/vectornav/vectornav.launch.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/phoenix_robot/launch/include/vectornav/vectornav.launch.py b/phoenix_robot/launch/include/vectornav/vectornav.launch.py index 04b8c3e..2ed4427 100644 --- a/phoenix_robot/launch/include/vectornav/vectornav.launch.py +++ b/phoenix_robot/launch/include/vectornav/vectornav.launch.py @@ -58,8 +58,11 @@ def generate_launch_description(): package='vectornav', executable='vn_sensor_msgs', output='screen', - remappings=[('/vectornav/imu', '/kohm/gps/imu'), ('vectornav/gnss', - '/kohm/navsat'), ('/vectornav/magnetic', '/kohm/mag')], + remappings=[ + ('/vectornav/imu', '/phoenix/imu'), + ('vectornav/gnss', '/phoenix/navsat'), + ('/vectornav/magnetic', '/phoenix/mag'), + ], parameters=[PathJoinSubstitution( [get_package_share_directory('phoenix_robot'), 'config', 'vn_sensor_msgs', config_file_name])], ) @@ -70,7 +73,7 @@ def generate_launch_description(): default_value='false', description='Use simulation clock if true'), DeclareLaunchArgument('config_file_name', - default_value='vectrnav.yaml', + default_value='vectornav.yaml', description='Name of the config file to load'), # Nodes start_vectornav_cmd, From d19c8e7eae42340c0ef505e25367e7a825cb97f8 Mon Sep 17 00:00:00 2001 From: redto0 Date: Sat, 10 Jan 2026 20:56:53 -0500 Subject: [PATCH 08/45] adding vec nav IMU --- .../robot_localization/robot_localization_gps_encoder_imu.yaml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/phoenix_robot/config/robot_localization/robot_localization_gps_encoder_imu.yaml b/phoenix_robot/config/robot_localization/robot_localization_gps_encoder_imu.yaml index 370bd38..eabd9b4 100644 --- a/phoenix_robot/config/robot_localization/robot_localization_gps_encoder_imu.yaml +++ b/phoenix_robot/config/robot_localization/robot_localization_gps_encoder_imu.yaml @@ -170,7 +170,8 @@ ekf_filter_node: # offset from base_link. # odom0_pose_use_child_frame: false - imu0: /camera/mid/imu + #imu0: /camera/mid/imu + imu0: /phoenix/gps/imu imu0_config: [false, false, false, true, true, true, false, false, false, From fe59c02b7c40d4413de560b495dcff030b97deb9 Mon Sep 17 00:00:00 2001 From: redto0 Date: Mon, 2 Feb 2026 20:06:55 -0500 Subject: [PATCH 09/45] nav_sat yaml file --- .../navsat_transform/navsat_transform/yaml | 51 +++++++++++++++++++ 1 file changed, 51 insertions(+) create mode 100644 phoenix_robot/config/navsat_transform/navsat_transform/yaml diff --git a/phoenix_robot/config/navsat_transform/navsat_transform/yaml b/phoenix_robot/config/navsat_transform/navsat_transform/yaml new file mode 100644 index 0000000..c7a6ac6 --- /dev/null +++ b/phoenix_robot/config/navsat_transform/navsat_transform/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] + From 05cca1b034b61cccf45ddf911cadc9d8f949473d Mon Sep 17 00:00:00 2001 From: redto0 Date: Mon, 2 Feb 2026 20:07:14 -0500 Subject: [PATCH 10/45] repos updating --- phoenix.repos | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/phoenix.repos b/phoenix.repos index 184621a..9fb6207 100644 --- a/phoenix.repos +++ b/phoenix.repos @@ -66,4 +66,8 @@ repositories: vectornav: type: git url: https://github.com/dawonn/vectornav.git - version: ros2 \ No newline at end of file + version: ros2 + gps_publisher: + type: git + url: git@github.com:Redderpears/gps-publisher.git + version: master From 6d2d827c67e505ea69ecfcf7e78f957cfc7f2925 Mon Sep 17 00:00:00 2001 From: redto0 Date: Wed, 18 Feb 2026 22:25:39 -0500 Subject: [PATCH 11/45] correct remote for gps publisher --- phoenix.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/phoenix.repos b/phoenix.repos index 9fb6207..1bbd233 100644 --- a/phoenix.repos +++ b/phoenix.repos @@ -69,5 +69,5 @@ repositories: version: ros2 gps_publisher: type: git - url: git@github.com:Redderpears/gps-publisher.git + url: git@github.com:ISC-Project-Phoenix/GPS-publisher.git version: master From 84e6274073ef885107c9a94fcf8044b81f45aa38 Mon Sep 17 00:00:00 2001 From: redto0 Date: Mon, 23 Feb 2026 14:44:15 -0500 Subject: [PATCH 12/45] idk anymore chat --- .../gps_waypoint_publisher.launch.py | 51 ++++++++++ .../navsat_transform/navsat_transform.yaml | 51 ++++++++++ .../navsat_transform.launch.py | 92 +++++++++++++++++++ 3 files changed, 194 insertions(+) create mode 100644 phoenix_gazebo/launch/include/gps_waypoint_publisher/gps_waypoint_publisher.launch.py create mode 100644 phoenix_robot/config/navsat_transform/navsat_transform/navsat_transform.yaml create mode 100644 phoenix_robot/launch/include/navsat_transform/navsat_transform.launch.py diff --git a/phoenix_gazebo/launch/include/gps_waypoint_publisher/gps_waypoint_publisher.launch.py b/phoenix_gazebo/launch/include/gps_waypoint_publisher/gps_waypoint_publisher.launch.py new file mode 100644 index 0000000..20546c5 --- /dev/null +++ b/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, + + ]) diff --git a/phoenix_robot/config/navsat_transform/navsat_transform/navsat_transform.yaml b/phoenix_robot/config/navsat_transform/navsat_transform/navsat_transform.yaml new file mode 100644 index 0000000..c7a6ac6 --- /dev/null +++ b/phoenix_robot/config/navsat_transform/navsat_transform/navsat_transform.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] + diff --git a/phoenix_robot/launch/include/navsat_transform/navsat_transform.launch.py b/phoenix_robot/launch/include/navsat_transform/navsat_transform.launch.py new file mode 100644 index 0000000..7a18b8f --- /dev/null +++ b/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 + ]) \ No newline at end of file From a97e69c7cfc03b942ef4507b3fbe71564438219f Mon Sep 17 00:00:00 2001 From: redto0 Date: Fri, 27 Feb 2026 00:18:02 -0500 Subject: [PATCH 13/45] removing poly plan and CV nodes from phoenix.repos --- phoenix.repos | 18 +++++------------- 1 file changed, 5 insertions(+), 13 deletions(-) diff --git a/phoenix.repos b/phoenix.repos index 1bbd233..c04ab75 100644 --- a/phoenix.repos +++ b/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,3 @@ 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: - type: git - url: git@github.com:ISC-Project-Phoenix/GPS-publisher.git - version: master From 10a0f053d918ce73b07be0aa46d86450939dd43e Mon Sep 17 00:00:00 2001 From: redto0 Date: Fri, 27 Feb 2026 00:20:31 -0500 Subject: [PATCH 14/45] adding design into the repo --- phoenix.repos | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/phoenix.repos b/phoenix.repos index c04ab75..53290c8 100644 --- a/phoenix.repos +++ b/phoenix.repos @@ -63,3 +63,7 @@ repositories: type: git url: git@github.com:iscumd/teleop_ack_joy.git version: master + design: + type: git + url: git@github.com:ISC-Project-Phoenix/design.git + version: main \ No newline at end of file From 2a105df99a749481287daed2b1aff3a1fb2d3c23 Mon Sep 17 00:00:00 2001 From: redto0 Date: Fri, 27 Feb 2026 00:39:48 -0500 Subject: [PATCH 15/45] idks --- .../navsat_transform/{yaml => not_Sure.yaml} | 0 .../robot_localization.launch.py | 17 ++++++++++++++++- .../include/vectornav/vectornav.launch.py | 1 + 3 files changed, 17 insertions(+), 1 deletion(-) rename phoenix_robot/config/navsat_transform/navsat_transform/{yaml => not_Sure.yaml} (100%) diff --git a/phoenix_robot/config/navsat_transform/navsat_transform/yaml b/phoenix_robot/config/navsat_transform/navsat_transform/not_Sure.yaml similarity index 100% rename from phoenix_robot/config/navsat_transform/navsat_transform/yaml rename to phoenix_robot/config/navsat_transform/navsat_transform/not_Sure.yaml diff --git a/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py b/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py index 14644f8..822021f 100644 --- a/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py +++ b/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, ]) diff --git a/phoenix_robot/launch/include/vectornav/vectornav.launch.py b/phoenix_robot/launch/include/vectornav/vectornav.launch.py index 2ed4427..dcb83ec 100644 --- a/phoenix_robot/launch/include/vectornav/vectornav.launch.py +++ b/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'), From cdcb92e5141693ee8cc6de397359d797090d8262 Mon Sep 17 00:00:00 2001 From: redto0 Date: Sat, 28 Feb 2026 11:51:46 -0500 Subject: [PATCH 16/45] add GPS publisher into robot launch --- phoenix_robot/launch/common.launch.py | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/phoenix_robot/launch/common.launch.py b/phoenix_robot/launch/common.launch.py index 9b132df..82932c5 100644 --- a/phoenix_robot/launch/common.launch.py +++ b/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, ]) \ No newline at end of file From 8795cabe0b005e01b7fbe3f068971e06800a1b7f Mon Sep 17 00:00:00 2001 From: redto0 Date: Thu, 5 Mar 2026 00:09:48 -0500 Subject: [PATCH 17/45] navsat parameters --- .../navsat_transform/navsat_transform.yaml | 52 +++++++++++++++++++ 1 file changed, 52 insertions(+) create mode 100644 phoenix_gazebo/config/navsat_transform/navsat_transform.yaml diff --git a/phoenix_gazebo/config/navsat_transform/navsat_transform.yaml b/phoenix_gazebo/config/navsat_transform/navsat_transform.yaml new file mode 100644 index 0000000..2c76f4d --- /dev/null +++ b/phoenix_gazebo/config/navsat_transform/navsat_transform.yaml @@ -0,0 +1,52 @@ +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: 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] + From 1477f95e412d59aa2c44ec003ff8ba6c3b55b436 Mon Sep 17 00:00:00 2001 From: redto0 Date: Thu, 5 Mar 2026 00:10:00 -0500 Subject: [PATCH 18/45] adding vect nav IMU --- .../config/robot_localization/robot_localization.yaml | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/phoenix_gazebo/config/robot_localization/robot_localization.yaml b/phoenix_gazebo/config/robot_localization/robot_localization.yaml index c0888cf..9376381 100644 --- a/phoenix_gazebo/config/robot_localization/robot_localization.yaml +++ b/phoenix_gazebo/config/robot_localization/robot_localization.yaml @@ -169,15 +169,21 @@ 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 From fbedbb8dcafe393a18042a87dd22c2b1c914b2b0 Mon Sep 17 00:00:00 2001 From: redto0 Date: Thu, 5 Mar 2026 00:10:14 -0500 Subject: [PATCH 19/45] launch files update --- .../robot_localization.launch.py | 26 +++++++++++++++++-- 1 file changed, 24 insertions(+), 2 deletions(-) diff --git a/phoenix_gazebo/launch/include/robot_localization/robot_localization.launch.py b/phoenix_gazebo/launch/include/robot_localization/robot_localization.launch.py index 931f489..b88bddb 100644 --- a/phoenix_gazebo/launch/include/robot_localization/robot_localization.launch.py +++ b/phoenix_gazebo/launch/include/robot_localization/robot_localization.launch.py @@ -38,18 +38,39 @@ def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time', default='true') rl_p = get_package_share_directory('robot_localization') + # Paths to the separate config files + rl_config_path = os.path.join( + get_package_share_directory("phoenix_gazebo"), + 'config', 'robot_localization', 'robot_localization.yaml' + ) + navsat_config_path = os.path.join( + get_package_share_directory("phoenix_gazebo"), + 'config', 'robot_localization', 'navsat_transform.yaml' + ) rl = Node( package='robot_localization', executable='ekf_node', name='ekf_filter_node', output='screen', - parameters=[os.path.join(get_package_share_directory("phoenix_gazebo"), 'config', 'robot_localization', - 'robot_localization.yaml')], + parameters=[rl_config_path], remappings=[ ('/odometry/filtered', '/odom'), ], ) + navsat_transform = Node( + package='robot_localization', + executable='navsat_transform_node', + name='navsat_transform_node', + output='screen', + parameters=[navsat_config_path], + remappings=[ + ('imu', '/camera/mid/imu'), # Using Webots IMU topic + ('gps/fix', '/gps/fix'), + ('odometry/filtered', '/odom'), # This feeds the EKF pose to navsat + ] + ) + return LaunchDescription([ # Launch Arguments DeclareLaunchArgument('use_sim_time', @@ -57,4 +78,5 @@ def generate_launch_description(): description='Use simulation clock if true'), # Nodes rl, + navsat_transform, ]) From cc966afb20ca4b869b5934c10303df810ee4d2a7 Mon Sep 17 00:00:00 2001 From: redto0 Date: Thu, 5 Mar 2026 00:11:32 -0500 Subject: [PATCH 20/45] updating SIM for GPS --- phoenix_description/urdf/phoenix.urdf | 36 +++++++++++++++++++ .../webots_project/protos/Phoenix2.proto | 20 +++++++++++ 2 files changed, 56 insertions(+) diff --git a/phoenix_description/urdf/phoenix.urdf b/phoenix_description/urdf/phoenix.urdf index 4932605..ef12138 100644 --- a/phoenix_description/urdf/phoenix.urdf +++ b/phoenix_description/urdf/phoenix.urdf @@ -377,6 +377,22 @@ + + + + + + + + + + + + + + + + @@ -530,6 +546,26 @@ + + + /vectornav/imu + vectornav + true + + vectornav_inert + vectornav_gyro + vectornav_accel + + + + + vectornav + /vectornav/gnss + 10 + true + + + diff --git a/phoenix_gazebo/webots_project/protos/Phoenix2.proto b/phoenix_gazebo/webots_project/protos/Phoenix2.proto index 859a86c..8dd3f1d 100755 --- a/phoenix_gazebo/webots_project/protos/Phoenix2.proto +++ b/phoenix_gazebo/webots_project/protos/Phoenix2.proto @@ -151,6 +151,26 @@ PROTO Phoenix2 [ ] } + # VectorNav VN-300 + Solid { + translation 1.0 0.0 0.5 # Matches your URDF gps_link position + children [ + GPS { + name "vectornav_gps" + } + Gyro { + name "vectornav_gyro" + } + Accelerometer { + name "vectornav_accel" + } + InertialUnit { + name "vectornav_inert" + } + ] + name "vectornav" + } + # Main body visual Pose { translation 0.65 3.64684e-06 -0.13 From 1a8e81891a5ea004d5a3c618de984069826baef9 Mon Sep 17 00:00:00 2001 From: redto0 Date: Thu, 5 Mar 2026 12:49:15 -0500 Subject: [PATCH 21/45] sim changes for gps to not work --- .../config/navsat_transform/navsat_transform.yaml | 14 ++++++++++++++ .../robot_localization/robot_localization.yaml | 9 ++++++++- .../webots_project/protos/Phoenix2.proto | 3 +++ 3 files changed, 25 insertions(+), 1 deletion(-) diff --git a/phoenix_gazebo/config/navsat_transform/navsat_transform.yaml b/phoenix_gazebo/config/navsat_transform/navsat_transform.yaml index 2c76f4d..60e8efa 100644 --- a/phoenix_gazebo/config/navsat_transform/navsat_transform.yaml +++ b/phoenix_gazebo/config/navsat_transform/navsat_transform.yaml @@ -1,5 +1,19 @@ 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 diff --git a/phoenix_gazebo/config/robot_localization/robot_localization.yaml b/phoenix_gazebo/config/robot_localization/robot_localization.yaml index 9376381..6107f33 100644 --- a/phoenix_gazebo/config/robot_localization/robot_localization.yaml +++ b/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 @@ -187,6 +187,13 @@ ekf_filter_node: #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. diff --git a/phoenix_gazebo/webots_project/protos/Phoenix2.proto b/phoenix_gazebo/webots_project/protos/Phoenix2.proto index 8dd3f1d..e692008 100755 --- a/phoenix_gazebo/webots_project/protos/Phoenix2.proto +++ b/phoenix_gazebo/webots_project/protos/Phoenix2.proto @@ -169,6 +169,9 @@ PROTO Phoenix2 [ } ] name "vectornav" + + boundingObject Box { size 0.1 0.1 0.1 } + physics Physics { mass 0.5 } } # Main body visual From cba4dedb3976d4382a28462276cc70b39c17d8b5 Mon Sep 17 00:00:00 2001 From: redto0 Date: Sat, 14 Mar 2026 23:27:33 -0400 Subject: [PATCH 22/45] ADDIING RC CONTROLS --- phoenix.repos | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/phoenix.repos b/phoenix.repos index 53290c8..adbce8a 100644 --- a/phoenix.repos +++ b/phoenix.repos @@ -46,7 +46,7 @@ repositories: gps_publisher: type: git url: git@github.com:ISC-Project-Phoenix/GPS-publisher.git - version: master + version: redtoo_dev_experiements hybrid_pp: type: git url: git@github.com:ISC-Project-Phoenix/hybrid_pp.git @@ -66,4 +66,8 @@ repositories: design: type: git url: git@github.com:ISC-Project-Phoenix/design.git - version: main \ No newline at end of file + version: main + RC: + type: git + url: git@github.com:estickel/teleop_ack_rc.git + verison: main \ No newline at end of file From 284ef9556501500fc5c55c638447daace340a3c2 Mon Sep 17 00:00:00 2001 From: redto0 Date: Sun, 15 Mar 2026 00:44:27 -0400 Subject: [PATCH 23/45] FEAT WIP: Adding RC to launch files - updated the phoenix.repos for proper directory name - added the launch file for RC ack - updated common launch for the RC. - TODO correct the launch the launch parameters. they dont work idk why --- phoenix.repos | 2 +- phoenix_robot/launch/common.launch.py | 10 ++++ .../teleop_ack_rc/teleop_ack_rc.launch.py | 52 +++++++++++++++++++ 3 files changed, 63 insertions(+), 1 deletion(-) create mode 100644 phoenix_robot/launch/include/teleop_ack_rc/teleop_ack_rc.launch.py diff --git a/phoenix.repos b/phoenix.repos index adbce8a..1c8927d 100644 --- a/phoenix.repos +++ b/phoenix.repos @@ -67,7 +67,7 @@ repositories: type: git url: git@github.com:ISC-Project-Phoenix/design.git version: main - RC: + teleop_ack_rc: type: git url: git@github.com:estickel/teleop_ack_rc.git verison: main \ No newline at end of file diff --git a/phoenix_robot/launch/common.launch.py b/phoenix_robot/launch/common.launch.py index 82932c5..b254504 100644 --- a/phoenix_robot/launch/common.launch.py +++ b/phoenix_robot/launch/common.launch.py @@ -154,6 +154,15 @@ def generate_launch_description(): 'use_sim_time': use_sim_time, }.items(), ) + teleop_ack_rc = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_phoenix_gazebo, 'launch', 'include', 'teleop_ack_rc/teleop_ack_rc.launch.py')), + launch_arguments={ + 'use_sim_time': use_sim_time, + 'max_speed': max_throttle_speed + }.items(), + condition=UnlessCondition(use_wheel) + ) return LaunchDescription([ # Launch Arguments @@ -184,4 +193,5 @@ def generate_launch_description(): poly_plan_ai, vectornav, GPS_waypoints, + teleop_ack_rc, ]) \ No newline at end of file diff --git a/phoenix_robot/launch/include/teleop_ack_rc/teleop_ack_rc.launch.py b/phoenix_robot/launch/include/teleop_ack_rc/teleop_ack_rc.launch.py new file mode 100644 index 0000000..edc4563 --- /dev/null +++ b/phoenix_robot/launch/include/teleop_ack_rc/teleop_ack_rc.launch.py @@ -0,0 +1,52 @@ +# Copyright (c) 2026 Intelligent Systems Club +# License: TBD. Contact for usage. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + +from launch_ros.actions import Node + +def generate_launch_description(): + use_sim_time = LaunchConfiguration('use_sim_time', default='true') + port = LaunchConfiguration('port', default='/dev/ttyUSB0') + max_speed = LaunchConfiguration('max_speed', default='4.0') + max_steering_angle = LaunchConfiguration('max_steering_angle', default='0.2733') + + pir = Node(package='teleop_ack_rc', + executable='teleop_ack_rc', + name='teleop_ack_rc', + parameters=[{ + 'use_sim_time': use_sim_time, + # TODO KEEP THESE HERE SINCE THE OTHER JOY USES THEM IDK IF WE WILL + # 'throttle_axis': throttle_axis, + # 'steering_axis': steering_axis, + # 'min_axis_input': min_axis_input, + # 'max_axis_input': max_axis_input, + # 'min_steering_angle': min_steering_angle, + 'port': port, + 'max_speed': max_speed, + 'max_steering_angle': max_steering_angle, + }], + ) + + return LaunchDescription([ + # Launch Arguments + DeclareLaunchArgument('use_sim_time', + default_value='true', + description='Use simulation clock if true'), + DeclareLaunchArgument('port', + default_value='/dev/ttyUSB0', + description='Serial port for RC receiver'), + DeclareLaunchArgument('max_speed', + default_value='4.0', + description='Maximum speed in m/s'), + DeclareLaunchArgument('max_steering_angle', + default_value='0.2733', + description='Maximum steering angle in radians'), + pir + ]) From e95b0c5bf6980d6d32c5a6002bddf1e7cf6b070f Mon Sep 17 00:00:00 2001 From: Elijah Stickel Date: Sun, 15 Mar 2026 15:49:09 -0400 Subject: [PATCH 24/45] FIX for RC controls and launch - updated parameters in common launch - updated launch path for robot locationization - added joy to RC ack Co-authored-by: redto0 --- phoenix_robot/launch/common.launch.py | 20 +++++++++---------- .../robot_localization.launch.py | 2 +- .../teleop_ack_rc/teleop_ack_rc.launch.py | 18 +++++++++++++---- 3 files changed, 25 insertions(+), 15 deletions(-) diff --git a/phoenix_robot/launch/common.launch.py b/phoenix_robot/launch/common.launch.py index b254504..6cfefd5 100644 --- a/phoenix_robot/launch/common.launch.py +++ b/phoenix_robot/launch/common.launch.py @@ -23,6 +23,7 @@ def generate_launch_description(): 'drive_mode_switch_button', default='7') use_sim_time = LaunchConfiguration('use_sim_time', default='false') use_ai = LaunchConfiguration('use_ai', default='false') + # max_throttle_speed = LaunchConfiguration('max_speed', default='4.0') state_publishers = IncludeLaunchDescription( PythonLaunchDescriptionSource([ @@ -156,12 +157,11 @@ def generate_launch_description(): ) teleop_ack_rc = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(pkg_phoenix_gazebo, 'launch', 'include', 'teleop_ack_rc/teleop_ack_rc.launch.py')), + os.path.join(pkg_phoenix_robot, 'launch', 'include', 'teleop_ack_rc/teleop_ack_rc.launch.py')), launch_arguments={ 'use_sim_time': use_sim_time, - 'max_speed': max_throttle_speed + 'max_speed': "4.0" }.items(), - condition=UnlessCondition(use_wheel) ) return LaunchDescription([ @@ -187,11 +187,11 @@ def generate_launch_description(): # camera, pir, pp, - obj_detector_ai, - obj_detector_cv, - poly_plan, - poly_plan_ai, - vectornav, - GPS_waypoints, - teleop_ack_rc, + # obj_detector_ai, + # obj_detector_cv, + # poly_plan, + # poly_plan_ai, + # vectornav, + # GPS_waypoints, + # teleop_ack_rc, ]) \ No newline at end of file diff --git a/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py b/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py index 822021f..b5dc6f1 100644 --- a/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py +++ b/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py @@ -58,7 +58,7 @@ def generate_launch_description(): 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')], + parameters=[os.path.join(get_package_share_directory('phoenix_robot'), 'config','navsat_transform', 'navsat_transform' ,'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 diff --git a/phoenix_robot/launch/include/teleop_ack_rc/teleop_ack_rc.launch.py b/phoenix_robot/launch/include/teleop_ack_rc/teleop_ack_rc.launch.py index edc4563..4355923 100644 --- a/phoenix_robot/launch/include/teleop_ack_rc/teleop_ack_rc.launch.py +++ b/phoenix_robot/launch/include/teleop_ack_rc/teleop_ack_rc.launch.py @@ -13,11 +13,11 @@ def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time', default='true') - port = LaunchConfiguration('port', default='/dev/ttyUSB0') + port = LaunchConfiguration('port', default='/dev/ttyACM1') max_speed = LaunchConfiguration('max_speed', default='4.0') max_steering_angle = LaunchConfiguration('max_steering_angle', default='0.2733') - pir = Node(package='teleop_ack_rc', + ack_joy = Node(package='teleop_ack_rc', executable='teleop_ack_rc', name='teleop_ack_rc', parameters=[{ @@ -33,6 +33,15 @@ def generate_launch_description(): 'max_steering_angle': max_steering_angle, }], ) + joy = Node( + package='joy', + executable='joy_node', + name='joy', + output='screen', + parameters=[{ + 'use_sim_time': use_sim_time, + }], ) + return LaunchDescription([ # Launch Arguments @@ -40,7 +49,7 @@ def generate_launch_description(): default_value='true', description='Use simulation clock if true'), DeclareLaunchArgument('port', - default_value='/dev/ttyUSB0', + default_value='/dev/ttyACM1', description='Serial port for RC receiver'), DeclareLaunchArgument('max_speed', default_value='4.0', @@ -48,5 +57,6 @@ def generate_launch_description(): DeclareLaunchArgument('max_steering_angle', default_value='0.2733', description='Maximum steering angle in radians'), - pir + ack_joy, + joy ]) From 4a6034dec8ba095eb854ff60facdf5ed48eec047 Mon Sep 17 00:00:00 2001 From: redto0 Date: Thu, 19 Mar 2026 01:14:15 -0400 Subject: [PATCH 25/45] feat: implement Dual-EKF architecture and NavSat alignment - added robot_localization_dual_ekf.yaml - updated launch files for this. --- .../robot_localization.launch.py | 54 ++++++++------- .../navsat_transform/navsat_transform.yaml | 2 +- .../robot_localization_dual_ekf.yaml | 69 +++++++++++++++++++ .../robot_localization.launch.py | 61 +++++++++------- 4 files changed, 135 insertions(+), 51 deletions(-) create mode 100644 phoenix_robot/config/robot_localization/robot_localization_dual_ekf.yaml diff --git a/phoenix_gazebo/launch/include/robot_localization/robot_localization.launch.py b/phoenix_gazebo/launch/include/robot_localization/robot_localization.launch.py index b88bddb..391b151 100644 --- a/phoenix_gazebo/launch/include/robot_localization/robot_localization.launch.py +++ b/phoenix_gazebo/launch/include/robot_localization/robot_localization.launch.py @@ -34,49 +34,55 @@ 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']) - # Paths to the separate config files - rl_config_path = os.path.join( - get_package_share_directory("phoenix_gazebo"), - 'config', 'robot_localization', 'robot_localization.yaml' - ) - navsat_config_path = os.path.join( - get_package_share_directory("phoenix_gazebo"), - 'config', 'robot_localization', 'navsat_transform.yaml' + # LOCAL EKF (odom -> base_link) + # No gps instead a smooth transtion. + local_ekf = Node( + package='robot_localization', + executable='ekf_node', + name='ekf_filter_node_odom', + output='screen', + parameters=[rl_config, {'use_sim_time': LaunchConfiguration('use_sim_time')}], + remappings=[('/odometry/filtered', '/odom')] ) - rl = Node( + + # 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', + name='ekf_filter_node_map', output='screen', - parameters=[rl_config_path], - remappings=[ - ('/odometry/filtered', '/odom'), - ], + 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_node', + name='navsat_transform', output='screen', - parameters=[navsat_config_path], + parameters=[navsat_config, {'use_sim_time': LaunchConfiguration('use_sim_time')}], remappings=[ - ('imu', '/camera/mid/imu'), # Using Webots IMU topic - ('gps/fix', '/gps/fix'), - ('odometry/filtered', '/odom'), # This feeds the EKF pose to navsat + ('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, ]) diff --git a/phoenix_robot/config/navsat_transform/navsat_transform/navsat_transform.yaml b/phoenix_robot/config/navsat_transform/navsat_transform/navsat_transform.yaml index c7a6ac6..6583660 100644 --- a/phoenix_robot/config/navsat_transform/navsat_transform/navsat_transform.yaml +++ b/phoenix_robot/config/navsat_transform/navsat_transform/navsat_transform.yaml @@ -17,7 +17,7 @@ navsat_transform_node: # 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 + yaw_offset: 1.5707963 # If this is true, the altitude is set to 0 in the output odometry message. Defaults to false. zero_altitude: false diff --git a/phoenix_robot/config/robot_localization/robot_localization_dual_ekf.yaml b/phoenix_robot/config/robot_localization/robot_localization_dual_ekf.yaml new file mode 100644 index 0000000..df48590 --- /dev/null +++ b/phoenix_robot/config/robot_localization/robot_localization_dual_ekf.yaml @@ -0,0 +1,69 @@ +ekf_filter_node_odom: + ros__parameters: + frequency: 30.0 + two_d_mode: true + publish_tf: true + + # Frame configuration + map_frame: map + odom_frame: odom + base_link_frame: base_footprint # + world_frame: odom # Only local data fused + + # Continuous Inputs + odom0: /odom_can # + odom0_config: [false, false, false, + false, false, false, + true, true, false, + false, false, false, + false, false, false] + # x y z + # r p y + # vx vy vz + # vr vp vy + # ax ay az + + imu0: /phoenix/gps/imu # + imu0_config: [false, false, false, + true, true, true, + false, false, false, + true, true, true, + true, true, true] + imu0_remove_gravitational_acceleration: true + +ekf_filter_node_map: + ros__parameters: + frequency: 30.0 + two_d_mode: true + publish_tf: true + + # Frame configuration + map_frame: map + odom_frame: odom + base_link_frame: base_footprint + world_frame: map # Global frame for GPS fusion + + # Inputs (Same as odom + GPS) + odom0: /odom_can + odom0_config: [false, false, false, # x y z + false, false, false, # x y z + true, true, false, # vx vy vz + false, false, false, # vr vp vy + false, false, false] # ax ay az + + imu0: /phoenix/gps/imu + imu0_config: [false, false, false, + true, true, true, + false, false, false, + true, true, true, + true, true, true] + imu0_relative: false # Use absolute IMU data for global EKF + + # GPS Odometry produced by NavSat Transform + odom1: /odometry/gps + odom1_config: [true, true, false, # X, Y (Global position) + false, false, false, + false, false, false, + false, false, false, + false, false, false] + odom1_differential: false \ No newline at end of file diff --git a/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py b/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py index b5dc6f1..91bb653 100644 --- a/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py +++ b/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py @@ -25,7 +25,6 @@ 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 @@ -35,46 +34,56 @@ def generate_launch_description(): - # Launch arguments - use_sim_time = LaunchConfiguration('use_sim_time', default='false') - config_file_name = LaunchConfiguration('config_file_name') + pkg_share = get_package_share_directory('phoenix_robot') + + # /home/redtoo/Documents/ws-phnx-gps/src/Phoenix/phoenix_robot/config/robot_localization/robot_localization_dual_ekf.yaml + rl_config = PathJoinSubstitution([pkg_share, 'config', 'robot_localization', 'robot_localization_dual_ekf.yaml']) + navsat_config = PathJoinSubstitution([pkg_share, 'config', 'navsat_transform', 'navsat_transform','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=[PathJoinSubstitution( - [get_package_share_directory('phoenix_robot'), 'config', 'robot_localization', config_file_name]) - ], - remappings=[ - ('/odometry/filtered', '/odom'), - ], + parameters=[rl_config, {'use_sim_time': LaunchConfiguration('use_sim_time')}], + remappings=[('/odometry/filtered', '/odom')] ) - # The NavSat Transform Node - start_navsat_transform_cmd = Node( + + # GLOBAL EKF (map -> odom) + # This node provides the 'map' frame and is fed by the NavSat Transform Node + 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')] + ) + + # 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', - # Point this to your YAML config file! - parameters=[os.path.join(get_package_share_directory('phoenix_robot'), 'config','navsat_transform', 'navsat_transform' ,'navsat_transform.yaml')], + parameters=[navsat_config, {'use_sim_time': LaunchConfiguration('use_sim_time')}], 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 + ('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='false', + default_value='true', description='Use simulation clock if true'), - DeclareLaunchArgument('config_file_name', - default_value='robot_localization.yaml', - description='Name of the config file to load'), # Nodes - rl, - start_navsat_transform_cmd, + local_ekf, + global_ekf, + navsat_transform, ]) From 5caf05eb5cad60a12547a2dd065489f32ca05b10 Mon Sep 17 00:00:00 2001 From: Alexander Boccaccio Date: Sun, 22 Mar 2026 11:12:18 -0400 Subject: [PATCH 26/45] added back the imports fix pluys other --- .../robot_localization.launch.py | 1 + phoenix_robot/launch/common.launch.py | 23 ++++++++++++++----- .../robot_localization.launch.py | 2 +- 3 files changed, 19 insertions(+), 7 deletions(-) diff --git a/phoenix_gazebo/launch/include/robot_localization/robot_localization.launch.py b/phoenix_gazebo/launch/include/robot_localization/robot_localization.launch.py index 391b151..a4c333d 100644 --- a/phoenix_gazebo/launch/include/robot_localization/robot_localization.launch.py +++ b/phoenix_gazebo/launch/include/robot_localization/robot_localization.launch.py @@ -31,6 +31,7 @@ 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(): diff --git a/phoenix_robot/launch/common.launch.py b/phoenix_robot/launch/common.launch.py index 6cfefd5..6543aed 100644 --- a/phoenix_robot/launch/common.launch.py +++ b/phoenix_robot/launch/common.launch.py @@ -157,6 +157,7 @@ def generate_launch_description(): ) teleop_ack_rc = IncludeLaunchDescription( PythonLaunchDescriptionSource( + # /home/isc-learning2/Documents/dev/testing-2026-ws/src/Phoenix/phoenix_robot/launch/include/teleop_ack_rc/teleop_ack_rc.launch.py os.path.join(pkg_phoenix_robot, 'launch', 'include', 'teleop_ack_rc/teleop_ack_rc.launch.py')), launch_arguments={ 'use_sim_time': use_sim_time, @@ -164,6 +165,15 @@ def generate_launch_description(): }.items(), ) + teleop_ack_joy = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_phoenix_gazebo, 'launch', 'include', 'teleop_ack_joy/teleop_ack_joy.launch.py')), + launch_arguments={ + 'use_sim_time': use_sim_time, + 'max_speed': "4.0", + }.items() + ) + return LaunchDescription([ # Launch Arguments DeclareLaunchArgument( @@ -187,11 +197,12 @@ def generate_launch_description(): # camera, pir, pp, - # obj_detector_ai, - # obj_detector_cv, - # poly_plan, - # poly_plan_ai, + obj_detector_ai, + obj_detector_cv, + poly_plan, + poly_plan_ai, # vectornav, - # GPS_waypoints, - # teleop_ack_rc, + GPS_waypoints, + teleop_ack_rc, + # teleop_ack_joy, ]) \ No newline at end of file diff --git a/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py b/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py index 91bb653..a5b75c3 100644 --- a/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py +++ b/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py @@ -31,7 +31,7 @@ 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(): pkg_share = get_package_share_directory('phoenix_robot') From 29da3ef06b4d4fe8f2b49618c9135a8083dba9bf Mon Sep 17 00:00:00 2001 From: Elijah Stickel Date: Sun, 22 Mar 2026 11:13:06 -0400 Subject: [PATCH 27/45] Fixed: PathJoinSubstitution updated teleop_ack_Rc included in gazeebo and robot --- phoenix_gazebo/launch/common.launch.py | 18 ++++- .../robot_localization.launch.py | 4 + .../teleop_ack_rc/teleop_ack_rc.launch.py | 30 ++++++++ phoenix_robot/launch/common.launch.py | 3 +- .../teleop_ack_rc/teleop_ack_rc.launch.py | 76 ++++++------------- 5 files changed, 73 insertions(+), 58 deletions(-) create mode 100644 phoenix_gazebo/launch/include/teleop_ack_rc/teleop_ack_rc.launch.py diff --git a/phoenix_gazebo/launch/common.launch.py b/phoenix_gazebo/launch/common.launch.py index 0900949..c34aca6 100644 --- a/phoenix_gazebo/launch/common.launch.py +++ b/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( @@ -130,8 +143,9 @@ def generate_launch_description(): # Nodes sim, - teleop_ack_joy, + #teleop_ack_joy, logi_g29, rviz, - robot_state_controller + robot_state_controller, + teleop_ack_rc ]) diff --git a/phoenix_gazebo/launch/include/robot_localization/robot_localization.launch.py b/phoenix_gazebo/launch/include/robot_localization/robot_localization.launch.py index a4c333d..3df74ea 100644 --- a/phoenix_gazebo/launch/include/robot_localization/robot_localization.launch.py +++ b/phoenix_gazebo/launch/include/robot_localization/robot_localization.launch.py @@ -31,8 +31,12 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node +<<<<<<< HEAD from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +======= +from launch.substitutions import PathJoinSubstitution +>>>>>>> 81e19a3 (Fixed: PathJoinSubstitution updated teleop_ack_Rc included in gazeebo and robot) def generate_launch_description(): pkg_share = get_package_share_directory('phoenix_robot') diff --git a/phoenix_gazebo/launch/include/teleop_ack_rc/teleop_ack_rc.launch.py b/phoenix_gazebo/launch/include/teleop_ack_rc/teleop_ack_rc.launch.py new file mode 100644 index 0000000..ad621b6 --- /dev/null +++ b/phoenix_gazebo/launch/include/teleop_ack_rc/teleop_ack_rc.launch.py @@ -0,0 +1,30 @@ +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + pkg_teleop_ack_rc = get_package_share_directory('teleop_ack_rc') + + use_sim_time = LaunchConfiguration('use_sim_time', default='true') + + return LaunchDescription([ + # RC Joy Node + Node( + package='teleop_ack_rc', + executable='rc_joy', + name='rc_joy_node', + output='screen', + parameters=[os.path.join(pkg_teleop_ack_rc, 'config', 'params.yaml')] + ), + + # Teleop Ack RC Node (logic bridge) + Node( + package='teleop_ack_rc', + executable='teleop_ack_rc', + name='teleop_ack_rc_node', + output='screen', + parameters=[os.path.join(pkg_teleop_ack_rc, 'config', 'params.yaml')] + ) + ]) \ No newline at end of file diff --git a/phoenix_robot/launch/common.launch.py b/phoenix_robot/launch/common.launch.py index 6543aed..21beed1 100644 --- a/phoenix_robot/launch/common.launch.py +++ b/phoenix_robot/launch/common.launch.py @@ -202,7 +202,6 @@ def generate_launch_description(): poly_plan, poly_plan_ai, # vectornav, - GPS_waypoints, + # GPS_waypoints, teleop_ack_rc, - # teleop_ack_joy, ]) \ No newline at end of file diff --git a/phoenix_robot/launch/include/teleop_ack_rc/teleop_ack_rc.launch.py b/phoenix_robot/launch/include/teleop_ack_rc/teleop_ack_rc.launch.py index 4355923..ad621b6 100644 --- a/phoenix_robot/launch/include/teleop_ack_rc/teleop_ack_rc.launch.py +++ b/phoenix_robot/launch/include/teleop_ack_rc/teleop_ack_rc.launch.py @@ -1,62 +1,30 @@ -# Copyright (c) 2026 Intelligent Systems Club -# License: TBD. Contact for usage. - import os - -from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration - from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory def generate_launch_description(): - use_sim_time = LaunchConfiguration('use_sim_time', default='true') - port = LaunchConfiguration('port', default='/dev/ttyACM1') - max_speed = LaunchConfiguration('max_speed', default='4.0') - max_steering_angle = LaunchConfiguration('max_steering_angle', default='0.2733') - - ack_joy = Node(package='teleop_ack_rc', - executable='teleop_ack_rc', - name='teleop_ack_rc', - parameters=[{ - 'use_sim_time': use_sim_time, - # TODO KEEP THESE HERE SINCE THE OTHER JOY USES THEM IDK IF WE WILL - # 'throttle_axis': throttle_axis, - # 'steering_axis': steering_axis, - # 'min_axis_input': min_axis_input, - # 'max_axis_input': max_axis_input, - # 'min_steering_angle': min_steering_angle, - 'port': port, - 'max_speed': max_speed, - 'max_steering_angle': max_steering_angle, - }], - ) - joy = Node( - package='joy', - executable='joy_node', - name='joy', - output='screen', - parameters=[{ - 'use_sim_time': use_sim_time, - }], ) + pkg_teleop_ack_rc = get_package_share_directory('teleop_ack_rc') + use_sim_time = LaunchConfiguration('use_sim_time', default='true') return LaunchDescription([ - # Launch Arguments - DeclareLaunchArgument('use_sim_time', - default_value='true', - description='Use simulation clock if true'), - DeclareLaunchArgument('port', - default_value='/dev/ttyACM1', - description='Serial port for RC receiver'), - DeclareLaunchArgument('max_speed', - default_value='4.0', - description='Maximum speed in m/s'), - DeclareLaunchArgument('max_steering_angle', - default_value='0.2733', - description='Maximum steering angle in radians'), - ack_joy, - joy - ]) + # RC Joy Node + Node( + package='teleop_ack_rc', + executable='rc_joy', + name='rc_joy_node', + output='screen', + parameters=[os.path.join(pkg_teleop_ack_rc, 'config', 'params.yaml')] + ), + + # Teleop Ack RC Node (logic bridge) + Node( + package='teleop_ack_rc', + executable='teleop_ack_rc', + name='teleop_ack_rc_node', + output='screen', + parameters=[os.path.join(pkg_teleop_ack_rc, 'config', 'params.yaml')] + ) + ]) \ No newline at end of file From 45b0e8ee1dff5c3183298239881a20e579b603b3 Mon Sep 17 00:00:00 2001 From: Alexander Boccaccio Date: Sun, 22 Mar 2026 11:33:16 -0400 Subject: [PATCH 28/45] adding rc to phoenix repos --- phoenix.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/phoenix.repos b/phoenix.repos index 1c8927d..593d2d0 100644 --- a/phoenix.repos +++ b/phoenix.repos @@ -69,5 +69,5 @@ repositories: version: main teleop_ack_rc: type: git - url: git@github.com:estickel/teleop_ack_rc.git + url: git@github.com:ISC-Project-Phoenix/teleop_ack_rc.git verison: main \ No newline at end of file From 06fe882f743283ee00ffc0ab356e813ec18454ed Mon Sep 17 00:00:00 2001 From: Elijah Stickel Date: Fri, 27 Mar 2026 10:32:50 -0400 Subject: [PATCH 29/45] Config: I added the config and updated the phnx_io_ros.launch.py to work with the config, vefified build and run in robot --- .../config/phoenix_io_ros/phoenix_io_ros.yaml | 5 ++++ .../include/phnx_io_ros/phnx_io_ros.launch.py | 25 ++++++++++--------- 2 files changed, 18 insertions(+), 12 deletions(-) create mode 100644 phoenix_robot/config/phoenix_io_ros/phoenix_io_ros.yaml diff --git a/phoenix_robot/config/phoenix_io_ros/phoenix_io_ros.yaml b/phoenix_robot/config/phoenix_io_ros/phoenix_io_ros.yaml new file mode 100644 index 0000000..a068581 --- /dev/null +++ b/phoenix_robot/config/phoenix_io_ros/phoenix_io_ros.yaml @@ -0,0 +1,5 @@ +/phnx_io_ros: + ros__parameters: + kP: 1.0 + kI: 1.0 + kD: 0.1 \ No newline at end of file diff --git a/phoenix_robot/launch/include/phnx_io_ros/phnx_io_ros.launch.py b/phoenix_robot/launch/include/phnx_io_ros/phnx_io_ros.launch.py index ac81094..76d2b87 100644 --- a/phoenix_robot/launch/include/phnx_io_ros/phnx_io_ros.launch.py +++ b/phoenix_robot/launch/include/phnx_io_ros/phnx_io_ros.launch.py @@ -1,22 +1,23 @@ import os - from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration - from launch_ros.actions import Node def generate_launch_description(): - - pir = Node(package='phnx_io_ros', - executable='phnx_io_ros', - parameters=[{ - }] - ) + # This just tells the script where your new folder is + config = os.path.join( + get_package_share_directory('phoenix_robot'), + 'config', 'phoenix_io_ros', 'phoenix_io_ros.yaml' + ) return LaunchDescription([ - # Launch Arguments - pir - ]) + Node( + package='phnx_io_ros', + executable='phnx_io_ros', + name='phnx_io_ros', + parameters=[config], + output='screen' + ) + ]) \ No newline at end of file From 392cce96c40f971b8098998b34573135b6e26f76 Mon Sep 17 00:00:00 2001 From: Elijah Stickel Date: Fri, 27 Mar 2026 10:37:40 -0400 Subject: [PATCH 30/45] phnx_io_ros config: added a configuration file for phoenix_io_ros for PID values --- phoenix_robot/config/phnx_io_ros/phoenix_io_ros.yaml | 5 +++++ 1 file changed, 5 insertions(+) create mode 100644 phoenix_robot/config/phnx_io_ros/phoenix_io_ros.yaml diff --git a/phoenix_robot/config/phnx_io_ros/phoenix_io_ros.yaml b/phoenix_robot/config/phnx_io_ros/phoenix_io_ros.yaml new file mode 100644 index 0000000..a068581 --- /dev/null +++ b/phoenix_robot/config/phnx_io_ros/phoenix_io_ros.yaml @@ -0,0 +1,5 @@ +/phnx_io_ros: + ros__parameters: + kP: 1.0 + kI: 1.0 + kD: 0.1 \ No newline at end of file From 637979456e2fe7a53f7b7976b1fd51c234854c0b Mon Sep 17 00:00:00 2001 From: redto0 Date: Fri, 27 Mar 2026 15:39:08 -0400 Subject: [PATCH 31/45] Revert "phnx_io_ros config: added a configuration file for phoenix_io_ros for PID values" This reverts commit 392cce96c40f971b8098998b34573135b6e26f76. --- phoenix_robot/config/phnx_io_ros/phoenix_io_ros.yaml | 5 ----- 1 file changed, 5 deletions(-) delete mode 100644 phoenix_robot/config/phnx_io_ros/phoenix_io_ros.yaml diff --git a/phoenix_robot/config/phnx_io_ros/phoenix_io_ros.yaml b/phoenix_robot/config/phnx_io_ros/phoenix_io_ros.yaml deleted file mode 100644 index a068581..0000000 --- a/phoenix_robot/config/phnx_io_ros/phoenix_io_ros.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/phnx_io_ros: - ros__parameters: - kP: 1.0 - kI: 1.0 - kD: 0.1 \ No newline at end of file From d7bdc9137001c4c34bea805a8b20cd6de3de002b Mon Sep 17 00:00:00 2001 From: redto0 Date: Fri, 27 Mar 2026 15:39:15 -0400 Subject: [PATCH 32/45] Revert "Config: I added the config and updated the phnx_io_ros.launch.py to work with the config, vefified build and run in robot" This reverts commit 06fe882f743283ee00ffc0ab356e813ec18454ed. --- .../config/phoenix_io_ros/phoenix_io_ros.yaml | 5 ---- .../include/phnx_io_ros/phnx_io_ros.launch.py | 25 +++++++++---------- 2 files changed, 12 insertions(+), 18 deletions(-) delete mode 100644 phoenix_robot/config/phoenix_io_ros/phoenix_io_ros.yaml diff --git a/phoenix_robot/config/phoenix_io_ros/phoenix_io_ros.yaml b/phoenix_robot/config/phoenix_io_ros/phoenix_io_ros.yaml deleted file mode 100644 index a068581..0000000 --- a/phoenix_robot/config/phoenix_io_ros/phoenix_io_ros.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/phnx_io_ros: - ros__parameters: - kP: 1.0 - kI: 1.0 - kD: 0.1 \ No newline at end of file diff --git a/phoenix_robot/launch/include/phnx_io_ros/phnx_io_ros.launch.py b/phoenix_robot/launch/include/phnx_io_ros/phnx_io_ros.launch.py index 76d2b87..ac81094 100644 --- a/phoenix_robot/launch/include/phnx_io_ros/phnx_io_ros.launch.py +++ b/phoenix_robot/launch/include/phnx_io_ros/phnx_io_ros.launch.py @@ -1,23 +1,22 @@ import os + from ament_index_python.packages import get_package_share_directory + from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration + from launch_ros.actions import Node def generate_launch_description(): - # This just tells the script where your new folder is - config = os.path.join( - get_package_share_directory('phoenix_robot'), - 'config', 'phoenix_io_ros', 'phoenix_io_ros.yaml' - ) + + pir = Node(package='phnx_io_ros', + executable='phnx_io_ros', + parameters=[{ + }] + ) return LaunchDescription([ - Node( - package='phnx_io_ros', - executable='phnx_io_ros', - name='phnx_io_ros', - parameters=[config], - output='screen' - ) - ]) \ No newline at end of file + # Launch Arguments + pir + ]) From 7f0a99b02f20abb0de92ea60f1c0cde9fd3929fe Mon Sep 17 00:00:00 2001 From: Mark Popkov Date: Fri, 27 Mar 2026 23:17:02 -0400 Subject: [PATCH 33/45] added yaml and edited commonlaunch and pir launch --- phoenix_robot/config/phnx_io_ros/phnx_io_ros.yaml | 6 ++++++ 1 file changed, 6 insertions(+) create mode 100755 phoenix_robot/config/phnx_io_ros/phnx_io_ros.yaml diff --git a/phoenix_robot/config/phnx_io_ros/phnx_io_ros.yaml b/phoenix_robot/config/phnx_io_ros/phnx_io_ros.yaml new file mode 100755 index 0000000..74a5fdc --- /dev/null +++ b/phoenix_robot/config/phnx_io_ros/phnx_io_ros.yaml @@ -0,0 +1,6 @@ +phnx_io_ros: + phnx_io_ros_node: + ros__parameters: + kP: 3.0 + kI: 2.0 + kD: 1.0 \ No newline at end of file From 8910ef616c235d0d8c67b83c06acbabb45fad4a1 Mon Sep 17 00:00:00 2001 From: Mark Popkov Date: Sat, 28 Mar 2026 15:24:04 -0400 Subject: [PATCH 34/45] -PID Yaml and updated launch.py; no broken files this time --- .../config/phnx_io_ros/phnx_io_ros.yaml | 11 +++++------ .../include/phnx_io_ros/phnx_io_ros.launch.py | 19 +++++++++---------- 2 files changed, 14 insertions(+), 16 deletions(-) diff --git a/phoenix_robot/config/phnx_io_ros/phnx_io_ros.yaml b/phoenix_robot/config/phnx_io_ros/phnx_io_ros.yaml index 74a5fdc..89d003a 100755 --- a/phoenix_robot/config/phnx_io_ros/phnx_io_ros.yaml +++ b/phoenix_robot/config/phnx_io_ros/phnx_io_ros.yaml @@ -1,6 +1,5 @@ -phnx_io_ros: - phnx_io_ros_node: - ros__parameters: - kP: 3.0 - kI: 2.0 - kD: 1.0 \ No newline at end of file +/phnx_io_ros: + ros__parameters: + kP: 1.0 + kI: 1.0 + kD: 0.1 \ No newline at end of file diff --git a/phoenix_robot/launch/include/phnx_io_ros/phnx_io_ros.launch.py b/phoenix_robot/launch/include/phnx_io_ros/phnx_io_ros.launch.py index ac81094..4f336d4 100644 --- a/phoenix_robot/launch/include/phnx_io_ros/phnx_io_ros.launch.py +++ b/phoenix_robot/launch/include/phnx_io_ros/phnx_io_ros.launch.py @@ -1,22 +1,21 @@ import os -from ament_index_python.packages import get_package_share_directory +from launch_ros.substitutions import FindPackageShare from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration +from launch.substitutions import PathJoinSubstitution from launch_ros.actions import Node def generate_launch_description(): - - pir = Node(package='phnx_io_ros', - executable='phnx_io_ros', - parameters=[{ - }] - ) - return LaunchDescription([ - # Launch Arguments - pir + Node( + package='phnx_io_ros', + executable='phnx_io_ros', + parameters=[PathJoinSubstitution([ + FindPackageShare('phoenix_robot'), 'config', 'phnx_io_ros', 'phnx_io_ros.yaml']) + ], + ), ]) From 65eea568439b2d6e1b5b77e9ae8f6ad9186b0190 Mon Sep 17 00:00:00 2001 From: redto0 Date: Thu, 16 Apr 2026 18:46:41 -0400 Subject: [PATCH 35/45] ROBOT config updates NavSat - updated NavSat paramters - changed over output topic for NAVSAT --- .../navsat_transform/navsat_transform.yaml | 2 +- .../navsat_transform/not_Sure.yaml | 6 +++--- phoenix_robot/launch/common.launch.py | 16 ++++++++-------- 3 files changed, 12 insertions(+), 12 deletions(-) diff --git a/phoenix_robot/config/navsat_transform/navsat_transform/navsat_transform.yaml b/phoenix_robot/config/navsat_transform/navsat_transform/navsat_transform.yaml index 6583660..9724a0f 100644 --- a/phoenix_robot/config/navsat_transform/navsat_transform/navsat_transform.yaml +++ b/phoenix_robot/config/navsat_transform/navsat_transform/navsat_transform.yaml @@ -31,7 +31,7 @@ navsat_transform_node: # 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 + publish_filtered_gps: false # 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! diff --git a/phoenix_robot/config/navsat_transform/navsat_transform/not_Sure.yaml b/phoenix_robot/config/navsat_transform/navsat_transform/not_Sure.yaml index c7a6ac6..9687827 100644 --- a/phoenix_robot/config/navsat_transform/navsat_transform/not_Sure.yaml +++ b/phoenix_robot/config/navsat_transform/navsat_transform/not_Sure.yaml @@ -17,10 +17,10 @@ navsat_transform_node: # 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 + yaw_offset: 90.0 # If this is true, the altitude is set to 0 in the output odometry message. Defaults to false. - zero_altitude: 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 @@ -31,7 +31,7 @@ navsat_transform_node: # 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 + publish_filtered_gps: false # 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! diff --git a/phoenix_robot/launch/common.launch.py b/phoenix_robot/launch/common.launch.py index 21beed1..1afff97 100644 --- a/phoenix_robot/launch/common.launch.py +++ b/phoenix_robot/launch/common.launch.py @@ -87,7 +87,7 @@ def generate_launch_description(): ]), launch_arguments={ 'use_sim_time': use_sim_time, - 'max_speed': '8.0', + 'max_speed': '4.0', 'min_speed': '0.5' }.items(), ) @@ -197,11 +197,11 @@ def generate_launch_description(): # camera, pir, pp, - obj_detector_ai, - obj_detector_cv, - poly_plan, - poly_plan_ai, - # vectornav, - # GPS_waypoints, + # obj_detector_ai, + # obj_detector_cv, + # poly_plan, + # poly_plan_ai, + vectornav, + GPS_waypoints, teleop_ack_rc, - ]) \ No newline at end of file + ]) From e5b0f6caf187977de511e2af9463e9653b911b7a Mon Sep 17 00:00:00 2001 From: redto0 Date: Fri, 17 Apr 2026 19:55:51 -0400 Subject: [PATCH 36/45] Fixed imports for robot locatioization Co-authored-by: Rhea-Botanist --- .../include/robot_localization/robot_localization.launch.py | 4 ---- 1 file changed, 4 deletions(-) diff --git a/phoenix_gazebo/launch/include/robot_localization/robot_localization.launch.py b/phoenix_gazebo/launch/include/robot_localization/robot_localization.launch.py index 3df74ea..a4c333d 100644 --- a/phoenix_gazebo/launch/include/robot_localization/robot_localization.launch.py +++ b/phoenix_gazebo/launch/include/robot_localization/robot_localization.launch.py @@ -31,12 +31,8 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -<<<<<<< HEAD from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -======= -from launch.substitutions import PathJoinSubstitution ->>>>>>> 81e19a3 (Fixed: PathJoinSubstitution updated teleop_ack_Rc included in gazeebo and robot) def generate_launch_description(): pkg_share = get_package_share_directory('phoenix_robot') From 7197271f2d4acb16458393cd93b5ef427eb42969 Mon Sep 17 00:00:00 2001 From: redto0 Date: Fri, 17 Apr 2026 22:26:04 -0400 Subject: [PATCH 37/45] Yaw offset --- .../robot_localization.launch.py | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py b/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py index a5b75c3..ff5fb40 100644 --- a/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py +++ b/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py @@ -59,7 +59,7 @@ def generate_launch_description(): name='ekf_filter_node_map', output='screen', parameters=[rl_config, {'use_sim_time': LaunchConfiguration('use_sim_time')}], - remappings=[('/odometry/filtered', '/odometry/global')] + remappings=[('/odometry/filtered', '/odometry/gps')] # TODO make this '/odom_global/filtered' and change in GPS ) # NAVSAT TRANSFORM NODE @@ -69,12 +69,16 @@ def generate_launch_description(): executable='navsat_transform_node', name='navsat_transform', output='screen', - parameters=[navsat_config, {'use_sim_time': LaunchConfiguration('use_sim_time')}], + parameters=[navsat_config, + {'use_sim_time': LaunchConfiguration('use_sim_time'), + 'yaw_offset': LaunchConfiguration('yaw_offset') + }], 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 + ('odometry/gps', '/odometry/gps'), # Output to GLOBAL EKF + ('gps/filtered', 'gps/filtered'), # Extra gps/filtered sensor_msgs/NavSatFix ] ) return LaunchDescription([ @@ -82,6 +86,10 @@ def generate_launch_description(): DeclareLaunchArgument('use_sim_time', default_value='true', description='Use simulation clock if true'), + DeclareLaunchArgument('yaw_offset', + default_value='-0.105', # flaot for dearborn, needto change for purdue + description='magnetic_declination_radians paramter'), + # Nodes local_ekf, global_ekf, From c4896ac576b1e675978adf1c5a27e820179d74eb Mon Sep 17 00:00:00 2001 From: redto0 Date: Tue, 5 May 2026 14:48:59 -0400 Subject: [PATCH 38/45] Alex's wonderful changes of many lines - remapped gps topics to match - added in imu frame for tf2 tree - over wrote imu0 to use defualt covarance matrix - this is because navsat needs non zero covarance - idk why just put the gps cord in the frame lit bro --- phoenix_description/urdf/phoenix.urdf | 19 +++++++++++++++++++ .../navsat_transform/navsat_transform.yaml | 8 ++++---- .../robot_localization_dual_ekf.yaml | 4 ++-- phoenix_robot/launch/common.launch.py | 2 +- 4 files changed, 26 insertions(+), 7 deletions(-) diff --git a/phoenix_description/urdf/phoenix.urdf b/phoenix_description/urdf/phoenix.urdf index ef12138..1e565d1 100644 --- a/phoenix_description/urdf/phoenix.urdf +++ b/phoenix_description/urdf/phoenix.urdf @@ -393,6 +393,25 @@ + + + + + + + + + + + + + + + + + + + diff --git a/phoenix_robot/config/navsat_transform/navsat_transform/navsat_transform.yaml b/phoenix_robot/config/navsat_transform/navsat_transform/navsat_transform.yaml index 9724a0f..aaa040a 100644 --- a/phoenix_robot/config/navsat_transform/navsat_transform/navsat_transform.yaml +++ b/phoenix_robot/config/navsat_transform/navsat_transform/navsat_transform.yaml @@ -23,21 +23,21 @@ navsat_transform_node: 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 + broadcast_utm_transform: true # 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 + broadcast_utm_transform_as_parent_frame: true # 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: 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 + use_odometry_yaw: true # 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. diff --git a/phoenix_robot/config/robot_localization/robot_localization_dual_ekf.yaml b/phoenix_robot/config/robot_localization/robot_localization_dual_ekf.yaml index df48590..7c2952c 100644 --- a/phoenix_robot/config/robot_localization/robot_localization_dual_ekf.yaml +++ b/phoenix_robot/config/robot_localization/robot_localization_dual_ekf.yaml @@ -25,7 +25,7 @@ ekf_filter_node_odom: imu0: /phoenix/gps/imu # imu0_config: [false, false, false, - true, true, true, + false, false, false, false, false, false, true, true, true, true, true, true] @@ -66,4 +66,4 @@ ekf_filter_node_map: false, false, false, false, false, false, false, false, false] - odom1_differential: false \ No newline at end of file + odom1_differential: false diff --git a/phoenix_robot/launch/common.launch.py b/phoenix_robot/launch/common.launch.py index 1afff97..0ebb2c1 100644 --- a/phoenix_robot/launch/common.launch.py +++ b/phoenix_robot/launch/common.launch.py @@ -203,5 +203,5 @@ def generate_launch_description(): # poly_plan_ai, vectornav, GPS_waypoints, - teleop_ack_rc, + # teleop_ack_rc, ]) From 403abd53948c163475c886347d0ac17f15355ea6 Mon Sep 17 00:00:00 2001 From: redto0 Date: Mon, 4 May 2026 17:12:01 -0400 Subject: [PATCH 39/45] Alex's wonderful changes of many lines --- .../phnx_io_ros/phnx_io_ros.launch.py.save | 23 +++++++++++++++++++ .../robot_localization.launch.py | 4 ++++ 2 files changed, 27 insertions(+) create mode 100644 phoenix_robot/launch/include/phnx_io_ros/phnx_io_ros.launch.py.save diff --git a/phoenix_robot/launch/include/phnx_io_ros/phnx_io_ros.launch.py.save b/phoenix_robot/launch/include/phnx_io_ros/phnx_io_ros.launch.py.save new file mode 100644 index 0000000..081c501 --- /dev/null +++ b/phoenix_robot/launch/include/phnx_io_ros/phnx_io_ros.launch.py.save @@ -0,0 +1,23 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + # This just tells the script where your new folder is + config = os.path.join( + get_package_share_directory('phoenix_robot'), + 'config/phnx_io_ros/phoenix_io_ros.yaml' + ) + + return LaunchDescription([ + Node( + package='phnx_io_ros', + executable='phnx_io_ros', + name='phnx_io_ros', + parameters=[config], + output='screen' + ) + ]) diff --git a/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py b/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py index ff5fb40..48fc1a7 100644 --- a/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py +++ b/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py @@ -59,7 +59,11 @@ def generate_launch_description(): name='ekf_filter_node_map', output='screen', parameters=[rl_config, {'use_sim_time': LaunchConfiguration('use_sim_time')}], +<<<<<<< HEAD remappings=[('/odometry/filtered', '/odometry/gps')] # TODO make this '/odom_global/filtered' and change in GPS +======= + remappings=[('/odometry/filtered', '/odometry/gps')] +>>>>>>> 442b6ae (Alex's wonderful changes of many lines) ) # NAVSAT TRANSFORM NODE From 0e84af393e50f9e78154ab15e464cebbb0806c1d Mon Sep 17 00:00:00 2001 From: redto0 Date: Wed, 6 May 2026 15:27:56 -0400 Subject: [PATCH 40/45] pushing changes and topic renames to the kart for the future --- .../robot_localization_dual_ekf.yaml | 2 +- phoenix_robot/launch/common.launch.py | 6 +++--- .../robot_localization/robot_localization.launch.py | 10 +++------- 3 files changed, 7 insertions(+), 11 deletions(-) diff --git a/phoenix_robot/config/robot_localization/robot_localization_dual_ekf.yaml b/phoenix_robot/config/robot_localization/robot_localization_dual_ekf.yaml index 7c2952c..e32602e 100644 --- a/phoenix_robot/config/robot_localization/robot_localization_dual_ekf.yaml +++ b/phoenix_robot/config/robot_localization/robot_localization_dual_ekf.yaml @@ -23,7 +23,7 @@ ekf_filter_node_odom: # vr vp vy # ax ay az - imu0: /phoenix/gps/imu # + imu0: /phoenix/imu imu0_config: [false, false, false, false, false, false, false, false, false, diff --git a/phoenix_robot/launch/common.launch.py b/phoenix_robot/launch/common.launch.py index 0ebb2c1..2e4cac2 100644 --- a/phoenix_robot/launch/common.launch.py +++ b/phoenix_robot/launch/common.launch.py @@ -33,7 +33,7 @@ def generate_launch_description(): launch_arguments={'use_sim_time': use_sim_time}.items(), ) - ekf = IncludeLaunchDescription( + robot_localization = IncludeLaunchDescription( PythonLaunchDescriptionSource([ os.path.join(pkg_phoenix_robot, 'launch') , '/include/robot_localization/robot_localization.launch.py' @@ -193,7 +193,7 @@ def generate_launch_description(): # Nodes robot_state_controller, state_publishers, - ekf, + robot_localization, # camera, pir, pp, @@ -203,5 +203,5 @@ def generate_launch_description(): # poly_plan_ai, vectornav, GPS_waypoints, - # teleop_ack_rc, + teleop_ack_rc, ]) diff --git a/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py b/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py index 48fc1a7..423d551 100644 --- a/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py +++ b/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py @@ -59,11 +59,7 @@ def generate_launch_description(): name='ekf_filter_node_map', output='screen', parameters=[rl_config, {'use_sim_time': LaunchConfiguration('use_sim_time')}], -<<<<<<< HEAD - remappings=[('/odometry/filtered', '/odometry/gps')] # TODO make this '/odom_global/filtered' and change in GPS -======= - remappings=[('/odometry/filtered', '/odometry/gps')] ->>>>>>> 442b6ae (Alex's wonderful changes of many lines) + remappings=[('/odometry/filtered', '/odometry/gps/filter')] # TODO make this '/odom_global/filtered' and change in GPS ) # NAVSAT TRANSFORM NODE @@ -71,7 +67,7 @@ def generate_launch_description(): navsat_transform = Node( package='robot_localization', executable='navsat_transform_node', - name='navsat_transform', + name='navsat_transform_node', output='screen', parameters=[navsat_config, {'use_sim_time': LaunchConfiguration('use_sim_time'), @@ -81,7 +77,7 @@ def generate_launch_description(): ('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 + ('odometry/gps', '/odometry/navsat_gps'), # Output to GLOBAL EKF ('gps/filtered', 'gps/filtered'), # Extra gps/filtered sensor_msgs/NavSatFix ] ) From 0eea535341adcfb1738ad6982c85640a3d467791 Mon Sep 17 00:00:00 2001 From: redto0 Date: Wed, 6 May 2026 19:38:09 -0400 Subject: [PATCH 41/45] topics renamed --- .../navsat_transform/not_Sure.yaml | 51 ---- .../robot_localization_encoder_only.yaml | 3 + .../robot_localization_gps_encoder_imu.yaml | 279 ------------------ phoenix_robot/launch/common.launch.py | 2 +- .../navsat_transform.launch.py | 92 ------ 5 files changed, 4 insertions(+), 423 deletions(-) delete mode 100644 phoenix_robot/config/navsat_transform/navsat_transform/not_Sure.yaml delete mode 100644 phoenix_robot/config/robot_localization/robot_localization_gps_encoder_imu.yaml delete mode 100644 phoenix_robot/launch/include/navsat_transform/navsat_transform.launch.py diff --git a/phoenix_robot/config/navsat_transform/navsat_transform/not_Sure.yaml b/phoenix_robot/config/navsat_transform/navsat_transform/not_Sure.yaml deleted file mode 100644 index 9687827..0000000 --- a/phoenix_robot/config/navsat_transform/navsat_transform/not_Sure.yaml +++ /dev/null @@ -1,51 +0,0 @@ -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: 90.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: false - -# 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] - diff --git a/phoenix_robot/config/robot_localization/robot_localization_encoder_only.yaml b/phoenix_robot/config/robot_localization/robot_localization_encoder_only.yaml index c266d88..93f0b63 100644 --- a/phoenix_robot/config/robot_localization/robot_localization_encoder_only.yaml +++ b/phoenix_robot/config/robot_localization/robot_localization_encoder_only.yaml @@ -1,4 +1,7 @@ ### ekf config file ### + +### THIS IS LAST YEARS EKF CONFIG LEAVE AS IS ### + ekf_filter_node: ros__parameters: # The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin diff --git a/phoenix_robot/config/robot_localization/robot_localization_gps_encoder_imu.yaml b/phoenix_robot/config/robot_localization/robot_localization_gps_encoder_imu.yaml deleted file mode 100644 index eabd9b4..0000000 --- a/phoenix_robot/config/robot_localization/robot_localization_gps_encoder_imu.yaml +++ /dev/null @@ -1,279 +0,0 @@ - -### ekf config file ### -ekf_filter_node: - ros__parameters: - # 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 - - # 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 - # filter will generate new output. Defaults to 1 / frequency if not specified. - #sensor_timeout: 1.0 - - # ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is - # set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar - # environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected - # by, for example, an IMU. Defaults to false if unspecified. - two_d_mode: true - - # Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for - # future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if - # unspecified. - transform_time_offset: 0.0 - - # Use this parameter to provide specify how long the tf listener should wait for a transform to become available. - # Defaults to 0.0 if unspecified. - transform_timeout: 0.0 - - # If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is - # unhappy with any settings or data. - print_diagnostics: true - - # Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by - # debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious - # effects on the performance of the node. Defaults to false if unspecified. - debug: false - - # Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path. - #debug_out_file: /path/to/debug/file.txt - - # Whether we'll allow old measurements to cause a re-publication of the updated state - permit_corrected_publication: false - - # Whether to publish the acceleration state. Defaults to false if unspecified. - publish_acceleration: false - - # Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified. - publish_tf: true - - # REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and - # earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames. - # The robot's position in the odom frame will drift over time, but is accurate in the short term and should be - # continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom - # frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your - # robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based - # localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame. - # ekf_localization_node and ukf_localization_node are not concerned with the earth frame. - # Here is how to use the following settings: - # 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system. - # 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of - # odom_frame. - # 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set - # "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes. - # 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates - # from landmark observations) then: - # 3a. Set your "world_frame" to your map_frame value - # 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state - # estimation node from robot_localization! However, that instance should *not* fuse the global data. - #map_frame: map # Defaults to "map" if unspecified - odom_frame: odom # Defaults to "odom" if unspecified - base_link_frame: base_footprint # Defaults to "base_link" if unspecified - world_frame: odom # Defaults to the value of odom_frame if unspecified - - # The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry, - # geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped, - # sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0, - # odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no - # default values, and must be specified. - odom0: /odom_can # Just fuse vel from encoder - - # Each sensor reading updates some or all of the filter's state. These options give you greater control over which - # values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only - # want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the - # values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types - # do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message - # has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false - # if unspecified, effectively making this parameter required for each sensor. - odom0_config: [false, false, false, # x y z - false, false, false, # r p y - true, true, false, # vx vy vz - false, false, false, # vr vp vy - false, false, false] # ax ay az - - # If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase - # the size of the subscription queue so that more measurements are fused. - odom0_queue_size: 10 - - # [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under- - # report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they - # arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also - # measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't - # always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose - # data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then - # integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true - # for twist measurements has no effect. - odom0_differential: false - - # [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point" - # for all future measurements. While you can achieve the same effect with the differential paremeter, the key - # difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before - # integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true. - odom0_relative: false - - # [ADVANCED] Whether to use the starting pose of child_frame_id as the origin of odometry. - # Note: this is different from setting odom0_relative to true, as when child_frame is different from - # base_link_frame, the rotation of base_link will be coupled into the translation of child_frame. - # Set to true for fusing secondary odometry sources that are rigidly connected to base_link but has a non-zero - # offset from base_link. - odom0_pose_use_child_frame: false - - # [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to - # control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to - # numeric_limits::max() if unspecified. It is strongly recommended that these parameters be removed if not - # required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation. - # For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying - # the thresholds. - #odom0_pose_rejection_threshold: 5.0 - #odom0_twist_rejection_threshold: 1.0 - - # odom1: /odom_tracker TODO add when obj tracker updated - - # Each sensor reading updates some or all of the filter's state. These options give you greater control over which - # values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only - # want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the - # values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types - # do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message - # has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false - # if unspecified, effectively making this parameter required for each sensor. - # odom1_config: [false, false, false, # x y z - # false, false, false, # r p y - # true, false, false, # vx vy vz - # false, false, false, # vr vp vy - # false, false, false] # ax ay az - - # If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase - # the size of the subscription queue so that more measurements are fused. - # odom1_queue_size: 2 - - # [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under- - # report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they - # arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also - # measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't - # always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose - # data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then - # integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true - # for twist measurements has no effect. - # odom0_differential: false - - # [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point" - # for all future measurements. While you can achieve the same effect with the differential paremeter, the key - # difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before - # integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true. - # odom0_relative: false - - # [ADVANCED] Whether to use the starting pose of child_frame_id as the origin of odometry. - # Note: this is different from setting odom0_relative to true, as when child_frame is different from - # base_link_frame, the rotation of base_link will be coupled into the translation of child_frame. - # Set to true for fusing secondary odometry sources that are rigidly connected to base_link but has a non-zero - # offset from base_link. - # odom0_pose_use_child_frame: false - - #imu0: /camera/mid/imu - imu0: /phoenix/gps/imu - imu0_config: [false, false, false, - true, true, true, - false, false, false, - true, true, true, - true, true, true] - imu0_differential: false - imu0_relative: false - imu0_queue_size: 10 - #imu0_pose_rejection_threshold: 0.8 - #imu0_twist_rejection_threshold: 0.8 - #imu0_linear_acceleration_rejection_threshold: 0.8 - - # [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. - imu0_remove_gravitational_acceleration: true # We need this for the oak-d and gazebo - - # imu1: /camera/right/imu - # imu1_config: [false, false, false, - # true, true, true, - # false, false, false, - # true, true, true, - # true, true, true] - # imu1_differential: false - # imu1_relative: false - # imu1_queue_size: 10 - # #imu0_pose_rejection_threshold: 0.8 - # #imu0_twist_rejection_threshold: 0.8 - # #imu0_linear_acceleration_rejection_threshold: 0.8 - # - # # [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. - # imu1_remove_gravitational_acceleration: true # We need this for the oak-d and gazebo - - # [ADVANCED] The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no - # acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During - # correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be - # problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When - # this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially - # noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance - # for the velocity variable in question, or decrease the variance of the variable in question in the measurement - # itself. In addition, users can also take advantage of the control command being issued to the robot at the time we - # make the prediction. If control is used, it will get converted into an acceleration term, which will be used during - # predicition. Note that if an acceleration measurement for the variable in question is available from one of the - # inputs, the control term will be ignored. - # Whether or not we use the control input during predicition. Defaults to false. - #use_control: false # cannot use because we are ackermann - - # Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to - # false. - #stamped_control: false - - # The last issued control command will be used in prediction for this period. Defaults to 0.2. - #control_timeout: 0.2 - - # Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw. - #control_config: [true, false, false, false, false, true] - - # Places limits on how large the acceleration term will be. Should match your robot's kinematics. - #acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4] - - # Acceleration and deceleration limits are not always the same for robots. - #deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5] - - # If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these - # gains - #acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9] - - # If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these - # gains - #deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0] - - # [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is - # exposed as a configuration parameter. This matrix represents the noise we add to the total error after each - # prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be. - # However, if users find that a given variable is slow to converge, one approach is to increase the - # process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error - # to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are - # ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if - # unspecified. - # Note: the specification of covariance matrices can be cumbersome, so all matrix parameters in this package support - # both full specification or specification of only the diagonal values. - process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015] - - # [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal - # value (variance) to a large value will result in rapid convergence for initial measurements of the variable in - # question. Users should take care not to use large values for variables that will not be measured directly. The values - # are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the diagonal values below - # if unspecified. In this example, we specify only the diagonal of the matrix. - initial_estimate_covariance: [1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9] - - use_sim_time: false diff --git a/phoenix_robot/launch/common.launch.py b/phoenix_robot/launch/common.launch.py index 2e4cac2..fb2751f 100644 --- a/phoenix_robot/launch/common.launch.py +++ b/phoenix_robot/launch/common.launch.py @@ -194,7 +194,7 @@ def generate_launch_description(): robot_state_controller, state_publishers, robot_localization, - # camera, + camera, pir, pp, # obj_detector_ai, diff --git a/phoenix_robot/launch/include/navsat_transform/navsat_transform.launch.py b/phoenix_robot/launch/include/navsat_transform/navsat_transform.launch.py deleted file mode 100644 index 7a18b8f..0000000 --- a/phoenix_robot/launch/include/navsat_transform/navsat_transform.launch.py +++ /dev/null @@ -1,92 +0,0 @@ -# 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 - ]) \ No newline at end of file From 2a40f0539fc3b8838850780e100ada369f29eb35 Mon Sep 17 00:00:00 2001 From: redto0 Date: Wed, 6 May 2026 20:15:28 -0400 Subject: [PATCH 42/45] lowering speed --- .../navsat_transform/navsat_transform/navsat_transform.yaml | 1 + phoenix_robot/launch/utilibot.launch.py | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/phoenix_robot/config/navsat_transform/navsat_transform/navsat_transform.yaml b/phoenix_robot/config/navsat_transform/navsat_transform/navsat_transform.yaml index aaa040a..4a158af 100644 --- a/phoenix_robot/config/navsat_transform/navsat_transform/navsat_transform.yaml +++ b/phoenix_robot/config/navsat_transform/navsat_transform/navsat_transform.yaml @@ -47,5 +47,6 @@ navsat_transform_node: # 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 +# NOTE WE datum: [55.944904, -3.186693, 0.0] diff --git a/phoenix_robot/launch/utilibot.launch.py b/phoenix_robot/launch/utilibot.launch.py index 201eb71..582f496 100644 --- a/phoenix_robot/launch/utilibot.launch.py +++ b/phoenix_robot/launch/utilibot.launch.py @@ -23,7 +23,7 @@ def generate_launch_description(): use_wheel = LaunchConfiguration('use_wheel', default='false') max_braking_speed = LaunchConfiguration('max_braking_speed', default='-4.0') - max_throttle_speed = LaunchConfiguration('max_throttle_speed', default='4.0') + max_throttle_speed = LaunchConfiguration('max_throttle_speed', default='2.0') max_steering_rad = LaunchConfiguration('max_steering_rad', default='0.2733') wheelbase = LaunchConfiguration('wheelbase', default='1.08') @@ -75,7 +75,7 @@ def generate_launch_description(): default_value='-6.0', description='Maximum braking speed'), DeclareLaunchArgument('max_throttle_speed', - default_value='6.0', + default_value='2.0', description='Maximum throttle speed'), DeclareLaunchArgument('wheelbase', default_value='1.08', From f7e63b0a075f311d60f511c8550a586d057c6c3d Mon Sep 17 00:00:00 2001 From: redto0 Date: Mon, 11 May 2026 23:28:25 -0400 Subject: [PATCH 43/45] sim working because we should have done this sooner! --- phoenix_description/urdf/phoenix.urdf | 1 + phoenix_gazebo/launch/common.launch.py | 4 +- .../gps_waypoint_publisher.launch.py | 12 ++++++ .../launch/include/webots/webots.launch.py | 6 ++- phoenix_gazebo/launch/sim.launch.py | 41 ++++++++++++++++--- .../robot_localization_dual_ekf.yaml | 26 +++++++++--- phoenix_robot/launch/common.launch.py | 1 + 7 files changed, 77 insertions(+), 14 deletions(-) diff --git a/phoenix_description/urdf/phoenix.urdf b/phoenix_description/urdf/phoenix.urdf index 1e565d1..52d25b9 100644 --- a/phoenix_description/urdf/phoenix.urdf +++ b/phoenix_description/urdf/phoenix.urdf @@ -582,6 +582,7 @@ /vectornav/gnss 10 true + 40.0 -86.9 0 diff --git a/phoenix_gazebo/launch/common.launch.py b/phoenix_gazebo/launch/common.launch.py index c34aca6..e710dfa 100644 --- a/phoenix_gazebo/launch/common.launch.py +++ b/phoenix_gazebo/launch/common.launch.py @@ -143,9 +143,9 @@ def generate_launch_description(): # Nodes sim, - #teleop_ack_joy, + teleop_ack_joy, logi_g29, rviz, robot_state_controller, - teleop_ack_rc + # teleop_ack_rc ]) diff --git a/phoenix_gazebo/launch/include/gps_waypoint_publisher/gps_waypoint_publisher.launch.py b/phoenix_gazebo/launch/include/gps_waypoint_publisher/gps_waypoint_publisher.launch.py index 20546c5..590ee53 100644 --- a/phoenix_gazebo/launch/include/gps_waypoint_publisher/gps_waypoint_publisher.launch.py +++ b/phoenix_gazebo/launch/include/gps_waypoint_publisher/gps_waypoint_publisher.launch.py @@ -25,12 +25,23 @@ 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/src/gps_waypoints_purdue_sim_mk1.txt' + real_waypoints = '/home/isc/Documents/dev/phnx_ws_2026/src/gps_publisher/src/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', @@ -38,6 +49,7 @@ def generate_launch_description(): output='screen', parameters=[{ 'use_sim_time': use_sim_time, + 'waypoint_file_path': chosen_file }], ) return LaunchDescription([ diff --git a/phoenix_gazebo/launch/include/webots/webots.launch.py b/phoenix_gazebo/launch/include/webots/webots.launch.py index 902a3a3..65e2f79 100644 --- a/phoenix_gazebo/launch/include/webots/webots.launch.py +++ b/phoenix_gazebo/launch/include/webots/webots.launch.py @@ -25,7 +25,11 @@ def generate_launch_description(): {'robot_description': urdf_path, 'use_sim_time': True} ], - respawn=True + respawn=True, + remappings=[ + ('/vectornav/imu', '/phoenix/imu'), + ('/vectornav/gnss', '/phoenix/navsat'), + ] ) # The WebotsLauncher is a Webots custom action that allows you to start a Webots simulation instance. diff --git a/phoenix_gazebo/launch/sim.launch.py b/phoenix_gazebo/launch/sim.launch.py index 18b3c79..a2bceda 100644 --- a/phoenix_gazebo/launch/sim.launch.py +++ b/phoenix_gazebo/launch/sim.launch.py @@ -15,6 +15,7 @@ def generate_launch_description(): # ROS packages pkg_phoenix_gazebo = get_package_share_directory('phoenix_gazebo') + pkg_phoenix_robot = get_package_share_directory('phoenix_robot') # Launch arguments use_sim_time = LaunchConfiguration('use_sim_time', default='true') @@ -91,6 +92,34 @@ def generate_launch_description(): condition=UnlessCondition(use_ai) ) + vectornav = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + os.path.join(pkg_phoenix_robot, 'launch'), + '/include/vectornav/vectornav.launch.py' + ]), + launch_arguments={ + 'use_sim_time': use_sim_time, + }.items(), + ) + + robot_localization = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + os.path.join(pkg_phoenix_robot, 'launch') + , '/include/robot_localization/robot_localization.launch.py' + ]), + launch_arguments={'use_sim_time': use_sim_time}.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(), + ) + pp = IncludeLaunchDescription( PythonLaunchDescriptionSource([ os.path.join(pkg_phoenix_gazebo, 'launch'), @@ -121,11 +150,13 @@ def generate_launch_description(): # Nodes state_publishers, - ekf, + # ekf, # old odom stack webots, - obj_detector_ai, - obj_detector_cv, + # obj_detector_ai, + # obj_detector_cv, pp, - poly_plan, - poly_plan_ai + # poly_plan, + # poly_plan_ai + robot_localization, # new locationization! + GPS_waypoints # auton now ]) \ No newline at end of file diff --git a/phoenix_robot/config/robot_localization/robot_localization_dual_ekf.yaml b/phoenix_robot/config/robot_localization/robot_localization_dual_ekf.yaml index e32602e..3dfce00 100644 --- a/phoenix_robot/config/robot_localization/robot_localization_dual_ekf.yaml +++ b/phoenix_robot/config/robot_localization/robot_localization_dual_ekf.yaml @@ -14,7 +14,7 @@ ekf_filter_node_odom: odom0: /odom_can # odom0_config: [false, false, false, false, false, false, - true, true, false, + true, false, false, false, false, false, false, false, false] # x y z @@ -22,8 +22,13 @@ ekf_filter_node_odom: # vx vy vz # vr vp vy # ax ay az + # [x_pos , y_pos , z_pos, + # roll , pitch , yaw, + # x_vel , y_vel , z_vel, + # roll_vel, pitch_vel, yaw_vel, + # x_accel , y_accel , z_accel] - imu0: /phoenix/imu + imu0: /phoenix/imu # this is the vector_nav imu imu0_config: [false, false, false, false, false, false, false, false, false, @@ -36,6 +41,8 @@ ekf_filter_node_map: frequency: 30.0 two_d_mode: true publish_tf: true + print_diagnostics: true + publish_tf: true # Frame configuration map_frame: map @@ -47,20 +54,27 @@ ekf_filter_node_map: odom0: /odom_can odom0_config: [false, false, false, # x y z false, false, false, # x y z - true, true, false, # vx vy vz + true, false, false, # vx vy vz false, false, false, # vr vp vy false, false, false] # ax ay az + # [x_pos , y_pos , z_pos, + # roll , pitch , yaw, + # x_vel , y_vel , z_vel, + # roll_vel, pitch_vel, yaw_vel, + # x_accel , y_accel , z_accel] + + imu0: /phoenix/gps/imu imu0_config: [false, false, false, - true, true, true, + false, false, true, false, false, false, - true, true, true, + false, false, true, true, true, true] imu0_relative: false # Use absolute IMU data for global EKF # GPS Odometry produced by NavSat Transform - odom1: /odometry/gps + odom1: /odometry/navsat_gps odom1_config: [true, true, false, # X, Y (Global position) false, false, false, false, false, false, diff --git a/phoenix_robot/launch/common.launch.py b/phoenix_robot/launch/common.launch.py index fb2751f..36e074b 100644 --- a/phoenix_robot/launch/common.launch.py +++ b/phoenix_robot/launch/common.launch.py @@ -155,6 +155,7 @@ def generate_launch_description(): 'use_sim_time': use_sim_time, }.items(), ) + teleop_ack_rc = IncludeLaunchDescription( PythonLaunchDescriptionSource( # /home/isc-learning2/Documents/dev/testing-2026-ws/src/Phoenix/phoenix_robot/launch/include/teleop_ack_rc/teleop_ack_rc.launch.py From 9ba1b32c4be29f8489f41617d5ab1faa4ac4831a Mon Sep 17 00:00:00 2001 From: redto0 Date: Mon, 11 May 2026 23:30:01 -0400 Subject: [PATCH 44/45] update file path --- .../gps_waypoint_publisher/gps_waypoint_publisher.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/phoenix_gazebo/launch/include/gps_waypoint_publisher/gps_waypoint_publisher.launch.py b/phoenix_gazebo/launch/include/gps_waypoint_publisher/gps_waypoint_publisher.launch.py index 590ee53..87613a7 100644 --- a/phoenix_gazebo/launch/include/gps_waypoint_publisher/gps_waypoint_publisher.launch.py +++ b/phoenix_gazebo/launch/include/gps_waypoint_publisher/gps_waypoint_publisher.launch.py @@ -35,7 +35,7 @@ def generate_launch_description(): # 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/src/gps_waypoints_purdue_sim_mk1.txt' - real_waypoints = '/home/isc/Documents/dev/phnx_ws_2026/src/gps_publisher/src/gps_waypoints_parking_lot_mk1.txt' + real_waypoints = '/home/isc/Documents/dev/phnx_ws_2026/data/gps_publisher/src/gps_waypoints_parking_lot_mk1.txt' # This expression picks the real path if use_sim_time is 'false', else sim path chosen_file = PythonExpression([ From c7d6272bcda7d23d1b400b3eb78a33eb79e557f4 Mon Sep 17 00:00:00 2001 From: redto0 Date: Tue, 12 May 2026 01:41:09 -0400 Subject: [PATCH 45/45] sim improvemets --- .../gps_waypoint_publisher/gps_waypoint_publisher.launch.py | 4 ++-- .../robot_localization/robot_localization_dual_ekf.yaml | 6 +++--- .../include/robot_localization/robot_localization.launch.py | 4 ++-- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/phoenix_gazebo/launch/include/gps_waypoint_publisher/gps_waypoint_publisher.launch.py b/phoenix_gazebo/launch/include/gps_waypoint_publisher/gps_waypoint_publisher.launch.py index 87613a7..1326c92 100644 --- a/phoenix_gazebo/launch/include/gps_waypoint_publisher/gps_waypoint_publisher.launch.py +++ b/phoenix_gazebo/launch/include/gps_waypoint_publisher/gps_waypoint_publisher.launch.py @@ -34,8 +34,8 @@ def generate_launch_description(): 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/src/gps_waypoints_purdue_sim_mk1.txt' - real_waypoints = '/home/isc/Documents/dev/phnx_ws_2026/data/gps_publisher/src/gps_waypoints_parking_lot_mk1.txt' + 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([ diff --git a/phoenix_robot/config/robot_localization/robot_localization_dual_ekf.yaml b/phoenix_robot/config/robot_localization/robot_localization_dual_ekf.yaml index 3dfce00..0df0829 100644 --- a/phoenix_robot/config/robot_localization/robot_localization_dual_ekf.yaml +++ b/phoenix_robot/config/robot_localization/robot_localization_dual_ekf.yaml @@ -65,13 +65,13 @@ ekf_filter_node_map: # x_accel , y_accel , z_accel] - imu0: /phoenix/gps/imu + imu0: /phoenix/imu imu0_config: [false, false, false, false, false, true, false, false, false, false, false, true, - true, true, true] - imu0_relative: false # Use absolute IMU data for global EKF + false, false, true] + imu0_relative: true # Use absolute IMU data for global EKF # GPS Odometry produced by NavSat Transform odom1: /odometry/navsat_gps diff --git a/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py b/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py index 423d551..bfe5c0b 100644 --- a/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py +++ b/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py @@ -76,7 +76,7 @@ def generate_launch_description(): remappings=[ ('imu', '/phoenix/imu'), # Data from VectorNav ('gps/fix', '/phoenix/navsat'), # Data from VectorNav - ('odometry/filtered', '/odom'), # Input from LOCAL EKF + ('odometry/filtered', '/odometry/gps/filter'), # Input from global EKF ('odometry/gps', '/odometry/navsat_gps'), # Output to GLOBAL EKF ('gps/filtered', 'gps/filtered'), # Extra gps/filtered sensor_msgs/NavSatFix ] @@ -87,7 +87,7 @@ def generate_launch_description(): default_value='true', description='Use simulation clock if true'), DeclareLaunchArgument('yaw_offset', - default_value='-0.105', # flaot for dearborn, needto change for purdue + default_value='-0.455', # flaot for dearborn, needto change for purdue description='magnetic_declination_radians paramter'), # Nodes