diff --git a/phoenix.repos b/phoenix.repos index de83e8f..593d2d0 100644 --- a/phoenix.repos +++ b/phoenix.repos @@ -39,14 +39,14 @@ repositories: type: git url: git@github.com:luxonis/depthai-ros.git version: 82ce67c - polynomial_planner: + vectornav: type: git - url: git@github.com:ISC-Project-Phoenix/Polynomial_planner.git - version: master - road_detectors: + url: https://github.com/dawonn/vectornav.git + version: ros2 + gps_publisher: type: git - url: git@github.com:ISC-Project-Phoenix/road_detectors.git - version: master + url: git@github.com:ISC-Project-Phoenix/GPS-publisher.git + version: redtoo_dev_experiements hybrid_pp: type: git url: git@github.com:ISC-Project-Phoenix/hybrid_pp.git @@ -62,4 +62,12 @@ repositories: teleop_ack_joy: type: git url: git@github.com:iscumd/teleop_ack_joy.git - version: master \ No newline at end of file + version: master + design: + type: git + url: git@github.com:ISC-Project-Phoenix/design.git + version: main + teleop_ack_rc: + type: git + url: git@github.com:ISC-Project-Phoenix/teleop_ack_rc.git + verison: main \ No newline at end of file diff --git a/phoenix_description/urdf/phoenix.urdf b/phoenix_description/urdf/phoenix.urdf index 4932605..52d25b9 100644 --- a/phoenix_description/urdf/phoenix.urdf +++ b/phoenix_description/urdf/phoenix.urdf @@ -377,6 +377,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -530,6 +565,27 @@ + + + /vectornav/imu + vectornav + true + + vectornav_inert + vectornav_gyro + vectornav_accel + + + + + vectornav + /vectornav/gnss + 10 + true + 40.0 -86.9 0 + + + 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..60e8efa --- /dev/null +++ b/phoenix_gazebo/config/navsat_transform/navsat_transform.yaml @@ -0,0 +1,66 @@ +navsat_transform_node: + ros__parameters: + # Match the base frame to what your EKF uses + base_link_frame: base_footprint + + # Force the subscriber to accept Webots' Best Effort QoS + # this is a quick fix to allow GPS to work in the EKF and NAVSAT. + # ideally Reliable is better. TODO + qos_overrides: + /vectornav/imu: + subscription: + reliability: best_effort + /vectornav/gnss: + subscription: + reliability: best_effort + +# Frequency of the main run loop + frequency: 30.0 + +# Delay time, in seconds, before we calculate the transform from the UTM frame to your world frame. This is especially +# important if you have use_odometry_yaw set to true. Defaults to 0. + delay: 3.0 + +# PLEASE READ: Like all nodes in robot_localization, this node assumes that your IMU data is reported in the ENU frame. +# Many IMUs report data in the NED frame, so you'll want to verify that your data is in the correct frame before using +# it. + +# If your IMU does not account for magnetic declination, enter the value for your location here. If you don't know it, +# see http://www.ngdc.noaa.gov/geomag-web/ (make sure to convert the value to radians). This parameter is mandatory. + magnetic_declination_radians: 0.0 + +# Your IMU's yaw, once the magentic_declination_radians value is added to it, should report 0 when facing east. If it +# doesn't, enter the offset here. Defaults to 0. + yaw_offset: 0.0 + +# If this is true, the altitude is set to 0 in the output odometry message. Defaults to false. + zero_altitude: true + +# If this is true, the transform world_frame->utm transform is broadcast for use by other nodes. Defaults to false. + broadcast_utm_transform: false + +# If this is true, the utm->world_frame transform will be published instead of the world_frame->utm transform. +# Note that broadcast_utm_transform still has to be enabled. Defaults to false. + broadcast_utm_transform_as_parent_frame: false + +# If this is true, all received odometry data is converted back to a lat/lon and published as a NavSatFix message as +# /gps/filtered. Defaults to false. + publish_filtered_gps: true + +# If this is true, the node ignores the IMU data and gets its heading from the odometry source (typically the +# /odometry/filtered topic coming from one of robot_localization's state estimation nodes). BE CAREFUL when using this! +# The yaw value in your odometry source *must* be world-referenced, e.g., you cannot use your odometry source for yaw +# if your yaw data is based purely on integrated velocities. Defaults to false. + use_odometry_yaw: false + +# If true, will retrieve the datum from the 'datum' parameter below, if available. If no 'datum' parameter exists, +# navsat_transform_node will wait until the user calls the 'datum' service with the SetDatum service message. + wait_for_datum: true + +# Instead of using the first GPS location and IMU-based heading for the local-frame origin, users can specify the +# origin (datum) using this parameter. The fields in the parameter represent latitude and longitude in decimal degrees, +# and heading in radians. As navsat_transform_node assumes an ENU standard, a 0 heading corresponds to east. +# wait this might actually be usefully for translating over maps lol -AFB +# keep in mind this needs to be updated for the actual location of where every the kart is in. + datum: [55.944904, -3.186693, 0.0] + diff --git a/phoenix_gazebo/config/robot_localization/robot_localization.yaml b/phoenix_gazebo/config/robot_localization/robot_localization.yaml index c0888cf..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 @@ -169,18 +169,31 @@ ekf_filter_node: # offset from base_link. # odom0_pose_use_child_frame: false - imu0: /camera/mid/imu + imu0: /vectornav/imu imu0_config: [false, false, false, true, true, true, false, false, false, true, true, true, true, true, true] + # MUST BE FALSE so the EKF treats the yaw as an absolute compass heading imu0_differential: false + + # MUST BE FALSE so 0 radians is locked to East (standard ENU) imu0_relative: false + + imu0_queue_size: 10 + imu0_pose_use_child_frame: true #imu0_pose_rejection_threshold: 0.8 #imu0_twist_rejection_threshold: 0.8 #imu0_linear_acceleration_rejection_threshold: 0.8 + + # need for the webots issue. tbh webots is probably the root cause. + # Force the EKF to accept the Best Effort IMU data + qos_overrides: + /vectornav/imu: + subscription: + reliability: best_effort # [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set # this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame. diff --git a/phoenix_gazebo/launch/common.launch.py b/phoenix_gazebo/launch/common.launch.py index 0900949..e710dfa 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( @@ -133,5 +146,6 @@ def generate_launch_description(): teleop_ack_joy, logi_g29, rviz, - robot_state_controller + robot_state_controller, + # 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 new file mode 100644 index 0000000..1326c92 --- /dev/null +++ b/phoenix_gazebo/launch/include/gps_waypoint_publisher/gps_waypoint_publisher.launch.py @@ -0,0 +1,63 @@ +# MIT License +# +# Copyright (c) 2021 Intelligent Systems Club +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + +from launch_ros.actions import Node +from launch.substitutions import PythonExpression + + + +def generate_launch_description(): + # Launch arguments + use_sim_time = LaunchConfiguration('use_sim_time', default='true') + + # Hardcoded paths, so you should update these to use path join at some point + sim_waypoints = '/home/redtoo/Documents/ws-gps-phoenix/src/gps_publisher/data/gps_waypoints_purdue_sim_mk2.txt' + real_waypoints = '/home/isc/Documents/dev/phnx_ws_2026/src/gps_publisher/data/gps_waypoints_parking_lot_mk1.txt' + + # This expression picks the real path if use_sim_time is 'false', else sim path + chosen_file = PythonExpression([ + "'", real_waypoints, "' if '", use_sim_time, "' == 'false' else '", sim_waypoints, "'" + ]) + + plan = Node( + package='yet_another_gps_publisher', + executable='yet_another_gps_publisher', + name='yet_another_gps_publisher', + output='screen', + parameters=[{ + 'use_sim_time': use_sim_time, + 'waypoint_file_path': chosen_file + }], ) + + return LaunchDescription([ + # Launch Arguments + DeclareLaunchArgument('use_sim_time', + default_value='true', + description='Use simulation clock if true'), + # Nodes + plan, + + ]) 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..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,30 +31,59 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution def generate_launch_description(): - # Launch arguments - use_sim_time = LaunchConfiguration('use_sim_time', default='true') - rl_p = get_package_share_directory('robot_localization') + pkg_share = get_package_share_directory('phoenix_robot') + + rl_config = PathJoinSubstitution([pkg_share, 'config', 'robot_localization.yaml']) + navsat_config = PathJoinSubstitution([pkg_share, 'config', 'navsat_transform.yaml']) - rl = Node( + # LOCAL EKF (odom -> base_link) + # No gps instead a smooth transtion. + local_ekf = Node( package='robot_localization', executable='ekf_node', - name='ekf_filter_node', + name='ekf_filter_node_odom', output='screen', - parameters=[os.path.join(get_package_share_directory("phoenix_gazebo"), 'config', 'robot_localization', - 'robot_localization.yaml')], - remappings=[ - ('/odometry/filtered', '/odom'), - ], + parameters=[rl_config, {'use_sim_time': LaunchConfiguration('use_sim_time')}], + remappings=[('/odometry/filtered', '/odom')] + ) + + # GLOBAL EKF (map -> odom) + # This node provides the 'map' frame. It consumes the GPS odometry. + global_ekf = Node( + package='robot_localization', + executable='ekf_node', + name='ekf_filter_node_map', + output='screen', + parameters=[rl_config, {'use_sim_time': LaunchConfiguration('use_sim_time')}], + remappings=[('/odometry/filtered', '/odometry/global')] ) + # 3. NAVSAT TRANSFORM NODE + # Converts Lat/Lon to X/Y and bridges the GPS into the Global EKF + navsat_transform = Node( + package='robot_localization', + executable='navsat_transform_node', + name='navsat_transform', + output='screen', + parameters=[navsat_config, {'use_sim_time': LaunchConfiguration('use_sim_time')}], + remappings=[ + ('imu', '/phoenix/imu'), # Data from VectorNav + ('gps/fix', '/phoenix/navsat'), # Data from VectorNav + ('odometry/filtered', '/odom'), # Input from LOCAL EKF + ('odometry/gps', '/odometry/gps') # Output to GLOBAL EKF + ] + ) return LaunchDescription([ # Launch Arguments DeclareLaunchArgument('use_sim_time', default_value='true', description='Use simulation clock if true'), # Nodes - rl, + local_ekf, + global_ekf, + navsat_transform, ]) 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_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_gazebo/webots_project/protos/Phoenix2.proto b/phoenix_gazebo/webots_project/protos/Phoenix2.proto index 859a86c..e692008 100755 --- a/phoenix_gazebo/webots_project/protos/Phoenix2.proto +++ b/phoenix_gazebo/webots_project/protos/Phoenix2.proto @@ -151,6 +151,29 @@ 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" + + boundingObject Box { size 0.1 0.1 0.1 } + physics Physics { mass 0.5 } + } + # Main body visual Pose { translation 0.65 3.64684e-06 -0.13 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/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..4a158af --- /dev/null +++ b/phoenix_robot/config/navsat_transform/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: 1.5707963 + +# 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: 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: 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: 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: 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. + 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 +# NOTE WE + datum: [55.944904, -3.186693, 0.0] + 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..89d003a --- /dev/null +++ b/phoenix_robot/config/phnx_io_ros/phnx_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/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..0df0829 --- /dev/null +++ b/phoenix_robot/config/robot_localization/robot_localization_dual_ekf.yaml @@ -0,0 +1,83 @@ +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, false, false, + false, false, false, + false, false, false] + # x y z + # r p y + # 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 # this is the vector_nav imu + imu0_config: [false, false, false, + false, false, false, + 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 + print_diagnostics: 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, 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/imu + imu0_config: [false, false, false, + false, false, true, + false, false, false, + false, false, true, + false, false, true] + imu0_relative: true # Use absolute IMU data for global EKF + + # GPS Odometry produced by NavSat Transform + odom1: /odometry/navsat_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 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/vectornav/vectornav.yaml b/phoenix_robot/config/vectornav/vectornav.yaml new file mode 100644 index 0000000..f69900a --- /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-port0" + 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/common.launch.py b/phoenix_robot/launch/common.launch.py index 1bd3025..36e074b 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([ @@ -32,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' @@ -86,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(), ) @@ -135,6 +136,45 @@ 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(), + ) + + 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(), + ) + + 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, + 'max_speed': "4.0" + }.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( @@ -154,12 +194,15 @@ def generate_launch_description(): # Nodes robot_state_controller, state_publishers, - ekf, + robot_localization, camera, pir, pp, - obj_detector_ai, - obj_detector_cv, - poly_plan, - poly_plan_ai - ]) \ No newline at end of file + # obj_detector_ai, + # obj_detector_cv, + # poly_plan, + # poly_plan_ai, + vectornav, + GPS_waypoints, + teleop_ack_rc, + ]) 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']) + ], + ), ]) 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 14644f8..bfe5c0b 100644 --- a/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py +++ b/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py @@ -25,41 +25,73 @@ 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 launch.substitutions import LaunchConfiguration, PathJoinSubstitution 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')] ) + # 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/gps/filter')] # TODO make this '/odom_global/filtered' and change in GPS + ) + + # 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', + output='screen', + 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', '/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 + ] + ) 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'), + DeclareLaunchArgument('yaw_offset', + default_value='-0.455', # flaot for dearborn, needto change for purdue + description='magnetic_declination_radians paramter'), + # Nodes - rl, + local_ekf, + global_ekf, + navsat_transform, ]) 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..ad621b6 --- /dev/null +++ b/phoenix_robot/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/include/vectornav/vectornav.launch.py b/phoenix_robot/launch/include/vectornav/vectornav.launch.py new file mode 100644 index 0000000..dcb83ec --- /dev/null +++ b/phoenix_robot/launch/include/vectornav/vectornav.launch.py @@ -0,0 +1,82 @@ +# 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(): + # Launch arguments + use_sim_time = LaunchConfiguration('use_sim_time', default='false') + config_file_name = LaunchConfiguration('config_file_name') + + + # 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', + # confirm remappings for the vectornav. + 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])], + ) + + return LaunchDescription([ + # Launch Arguments + DeclareLaunchArgument('use_sim_time', + default_value='false', + description='Use simulation clock if true'), + DeclareLaunchArgument('config_file_name', + default_value='vectornav.yaml', + description='Name of the config file to load'), + # Nodes + start_vectornav_cmd, + start_vectornav_sensor_msgs_cmd, + ]) 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', 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