diff --git a/phoenix.repos b/phoenix.repos index 9fb6207..53290c8 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,7 @@ repositories: type: git url: git@github.com:iscumd/teleop_ack_joy.git version: master - vectornav: - type: git - url: https://github.com/dawonn/vectornav.git - version: ros2 - gps_publisher: + design: type: git - url: git@github.com:Redderpears/gps-publisher.git - version: master + url: git@github.com:ISC-Project-Phoenix/design.git + version: main \ No newline at end of file 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/yaml b/phoenix_robot/config/navsat_transform/navsat_transform/navsat_transform.yaml similarity index 100% rename from phoenix_robot/config/navsat_transform/navsat_transform/yaml rename to phoenix_robot/config/navsat_transform/navsat_transform/navsat_transform.yaml diff --git a/phoenix_robot/config/navsat_transform/navsat_transform/not_Sure.yaml b/phoenix_robot/config/navsat_transform/navsat_transform/not_Sure.yaml new file mode 100644 index 0000000..c7a6ac6 --- /dev/null +++ b/phoenix_robot/config/navsat_transform/navsat_transform/not_Sure.yaml @@ -0,0 +1,51 @@ +navsat_transform_node: + ros__parameters: +# Frequency of the main run loop + frequency: 30.0 + +# Delay time, in seconds, before we calculate the transform from the UTM frame to your world frame. This is especially +# important if you have use_odometry_yaw set to true. Defaults to 0. + delay: 3.0 + +# PLEASE READ: Like all nodes in robot_localization, this node assumes that your IMU data is reported in the ENU frame. +# Many IMUs report data in the NED frame, so you'll want to verify that your data is in the correct frame before using +# it. + +# If your IMU does not account for magnetic declination, enter the value for your location here. If you don't know it, +# see http://www.ngdc.noaa.gov/geomag-web/ (make sure to convert the value to radians). This parameter is mandatory. + magnetic_declination_radians: 0.0 + +# Your IMU's yaw, once the magentic_declination_radians value is added to it, should report 0 when facing east. If it +# doesn't, enter the offset here. Defaults to 0. + yaw_offset: 0.0 + +# If this is true, the altitude is set to 0 in the output odometry message. Defaults to false. + zero_altitude: false + +# If this is true, the transform world_frame->utm transform is broadcast for use by other nodes. Defaults to false. + broadcast_utm_transform: false + +# If this is true, the utm->world_frame transform will be published instead of the world_frame->utm transform. +# Note that broadcast_utm_transform still has to be enabled. Defaults to false. + broadcast_utm_transform_as_parent_frame: false + +# If this is true, all received odometry data is converted back to a lat/lon and published as a NavSatFix message as +# /gps/filtered. Defaults to false. + publish_filtered_gps: true + +# If this is true, the node ignores the IMU data and gets its heading from the odometry source (typically the +# /odometry/filtered topic coming from one of robot_localization's state estimation nodes). BE CAREFUL when using this! +# The yaw value in your odometry source *must* be world-referenced, e.g., you cannot use your odometry source for yaw +# if your yaw data is based purely on integrated velocities. Defaults to false. + use_odometry_yaw: false + +# If true, will retrieve the datum from the 'datum' parameter below, if available. If no 'datum' parameter exists, +# navsat_transform_node will wait until the user calls the 'datum' service with the SetDatum service message. + wait_for_datum: false + +# Instead of using the first GPS location and IMU-based heading for the local-frame origin, users can specify the +# origin (datum) using this parameter. The fields in the parameter represent latitude and longitude in decimal degrees, +# and heading in radians. As navsat_transform_node assumes an ENU standard, a 0 heading corresponds to east. +# wait this might actually be usefully for translating over maps lol -AFB + datum: [55.944904, -3.186693, 0.0] + 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 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 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'),