diff --git a/CMakeLists.txt b/CMakeLists.txt index 189cd35..d212984 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,27 +15,30 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp rclcpp_lifecycle ublox_dgnss + sensor_msgs ) # find dependencies +find_package(ublox_ubx_msgs REQUIRED) find_package(ament_cmake REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -add_executable(${PROJECT_NAME} +add_executable(gps_node src/heading.cpp src/heading_node.cpp ) -target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) -target_include_directories(${PROJECT_NAME} PUBLIC +target_compile_features(gps_node PUBLIC cxx_std_17) +target_include_directories(gps_node PUBLIC $ $ ) -ament_target_dependencies( - ${PROJECT_NAME} PUBLIC - ${THIS_PACKAGE_INCLUDE_DEPENDS} +ament_target_dependencies(gps_node + rclcpp + sensor_msgs + ublox_ubx_msgs ) # Parse version number from the file @@ -51,7 +54,7 @@ configure_file( # INSTALL install(TARGETS - ${PROJECT_NAME} + gps_node DESTINATION lib/${PROJECT_NAME} ) diff --git a/README.md b/README.md index 2da0cd1..deaf7be 100644 --- a/README.md +++ b/README.md @@ -1,25 +1,78 @@ -# UMRT ROS Template -ROS project repository template for the University of Manitoba Robotics Team. - -New projects should be **forked** from this repo (not using this as a template, as that prevents template changes from -trickling down). Each new project must: -1. Fill in missing fields in package.in.xml -2. Fill in project name in CMakeLists.txt and Doxyfile -3. Go into `umrt-build` package settings and give the new repo read permission -4. Go into `umrt-apt-image` package settings and give the new repo read permission -5. Go into `UMRoboticsTeam` organisation secrets and add the new repo to: - - `APT_DEPLOY_KEY` - - `APT_SIGNING_KEY` -6. Copy the rulesets (branch protection rules) from a mature repository like - [umrt-arm-firmware-lib](https://github.com/UMRoboticsTeam/umrt-arm-firmware-lib/) -7. Remove this notice and fill in below README template -8. Write something in mainpage.dox -9. Replace example files with real code, add source files to CMake targets, document it with Doxygen, and proceed - ---- -# Project Name - -This library/executable/project implements XYZ functionality for the University of Manitoba Robotics Team's -rover/robotic arm. - -[See the documentation](https://umroboticsteam.github.io/********** project-name **********/) \ No newline at end of file +# Heading + +This project implements heading for the University of Manitoba Robotics Team's +rover. + +**Overview** +Dual GPS localization and heading calculation package for the University of Manitoba. +This package uses 2 SparkFun GNSS Combo Breakout to: + 1. Get rover position + 2. Compute rover heading + 3. Publish a combined GPS topic. +This combo breakout pairs the u-blox ZED-F9P multi-band high-precision GNSS module with the NEO-D9S L-band GNSS correction data receiver. In this setup, only ZED-F9P is used for position and heading estimation. Website to find the details about this board: datasheet. + +**Hardware Setup** +Both GPS receivers are connected to Jetson through the USB-A Ports. Each receiver appears as its own serial USB device inside Linux. +Each GPS receiver is connected to its own external GNSS antenna. The antennas receive satellite signals while the receivers process the GNSS data and publish GPS coordinates. +The antennas are mounted left and right on the rover, connected to their respective GPS receivers. This creates a horizontal baseline and the package uses this setup to estimate heading. + +**Header Files and Heading calculation** +The header file, gps_node.hpp defines: +* Publishers: Sends messages to a topic. Other nodes can subscribe to it. +* Subscribers: Listens to a topic. Receives messages when published. +* Some variables and functions: Store and process the GPS data. + +The heading_node.cpp file uses the header file to: +* Initialize publisher, subscriber, variables and functions +* Wait until the GPS actually receives data to start processing it. +* Use the latitude and longitude positions of both GPS to calculate the heading. + +**Heading Estimation** +First, use the latitude and longitude coordinates to calculate the difference between left and right GPS in meters. Since the gps is mounted on the left and right of the rover, its forward is perpendicular to the baseline. Thus, we rotate the vector 90° to make the rover point forward: +double dx = (gpsRight_msg.longitude - gpsLeft_msg.longitude) * cos(((gpsRight_msg.latitude + gpsLeft_msg.latitude) / 2.0) * M_PI / 180.0) * 111320.0; +double dy = (gpsRight_msg.latitude - gpsLeft_msg.latitude) * 111320.0; +double forward_x = -dy; +double forward_y = dx; + +Now, to get the heading angle, just use atan2(x , y) to convert the forward vector into an angle. +heading_angle = atan2(forward_x, forward_y) * 180.0 / M_PI; + +Then, compare this angle to the angle from a compass to get the heading direction. Finally, publish all this data to the topic. + +**Node** +The *gps_node* created in the heading_node.cpp file, is the only node created in this package to take in the coordinates from the 2 GPS receivers and publish an output. + +**Topics** +There are 3 main topics: +* /gps_left/fix : This collects and processes the GPS coordinates from the left GPS. +* /gps_right/fix : This collects and processes the GPS coordinates from the right GPS. +* gps/fix : This publishes the combined GPS data. + +Steps to run the package: +| Terminal Steps | Details | Example Command | +|---|---|---| +| | Go inside the directory | `cd Documents/GPS` | +| | Build the Image file (You need to do this every time you make changes to your files) | `./gpsImage.sh` | +| | Run the container with the name ‘gps’ | `./gpsContainer.sh gpsImage` | +| | (Every time) source the file | `source install/setup.bash` | +| | Launch the rosbridge server | `ros2 launch rosbridge_server rosbridge_websocket_launch.xml` | +| | (New terminal) Connect the container to this new terminal using: | `docker exec -it gpsImage bash` | +| | Run the launch file | `ros2 launch umrt-localization-ros gps.launch.py` | +| | (New terminal) Connect the container to this new terminal using: | `docker exec -it gpsImage bash` | +| | Run the node | `ros2 run umrt-localization-ros gps_node` | +| | (New Terminal) Connect one terminal’s container to another terminal | `ros2 topic echo /gps/fix` | +| | Run the topic | `ros2 topic echo /gps/fix` | +| **In Foxglove** | Go to Dashboard | | +| | Click on open connection | | +| | On the pop-up window, select rosbridge | | +| | Click on open | | +| | Click on the dot, and then on the taskbar on the left select topic, and then choose /fix | | + +**Overview on Launch File:** +The launch file distinguishes the 2 gps via their serial string, which we were able to modify with ublox’s software U-center. + +Once the launch file is ran, our 2 ublox dgnss nodes are created, and they will then output a nav-sat fix, which the heading node (gps_node) will use to calculate a bearing. Below is a diagram to illustrate the process. +finalfinalfinal drawio + + +[See the documentation](https://umroboticsteam.github.io/********** project-name **********/) diff --git a/include/project-name/example_node.hpp b/include/umrt-localization-ros/example_node.hpp similarity index 100% rename from include/project-name/example_node.hpp rename to include/umrt-localization-ros/example_node.hpp diff --git a/include/umrt-localization-ros/gps_node.hpp b/include/umrt-localization-ros/gps_node.hpp new file mode 100644 index 0000000..584bfa9 --- /dev/null +++ b/include/umrt-localization-ros/gps_node.hpp @@ -0,0 +1,21 @@ +#ifndef GPS_NODE_HPP +#define GPS_NODE_HPP + +#include +#include + +class GpsNode : public rclcpp::Node { +public: + GpsNode(); + +private: + rclcpp::Subscription::SharedPtr relpos_sub; + + void relposCallback( + const ublox_ubx_msgs::msg::UBXNavRelPosNED::SharedPtr msg + ); + + double normalizeHeading(double heading_deg) const; +}; + +#endif // GPS_NODE_HPP \ No newline at end of file diff --git a/launch/example.launch.py b/launch/example.launch.py deleted file mode 100644 index 19de1d8..0000000 --- a/launch/example.launch.py +++ /dev/null @@ -1,21 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node - -import os -from ament_index_python.packages import get_package_share_directory - -def generate_launch_description(): - - config = os.path.join(get_package_share_directory('project-name'),'launch', 'example.yaml') - - example_node = Node( - package='project-name', - executable='project-name', - name = 'umrt_example_node', - parameters=[config] - ) - - return LaunchDescription([ - example_node - ]) - diff --git a/launch/example.yaml b/launch/example.yaml index 116ea5c..a0a60ee 100644 --- a/launch/example.yaml +++ b/launch/example.yaml @@ -1,3 +1,14 @@ umrt_example_node: ros__parameters: - example_param: 0 + gps1_topic: "/gps1/fix" + gps2_topic: "/gps2/fix" + + # Topic where the combined GPS data will be published + combined_topic: "/combined/fix" + + # Frame ID for the NavSatFix messages + frame_id: "map" + + # Optional parameters + example_param: 0 # Your previous example param + publish_rate: 10 # Hz, how often to publish combined GPS data \ No newline at end of file diff --git a/launch/gps.launch.py b/launch/gps.launch.py new file mode 100644 index 0000000..7da1912 --- /dev/null +++ b/launch/gps.launch.py @@ -0,0 +1,62 @@ +""" +UMRT Moving Base RTK GPS launch file +""" + +""" +Imports +""" +from launch import LaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import IncludeLaunchDescription, TimerAction, LogInfo +from launch.substitutions import LaunchConfiguration +import os +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + + launch_dir = get_package_share_directory('umrt-localization-ros') + + base_launch_path = os.path.join( + launch_dir, + 'launch', + 'ublox_mb+r_base.launch.py' + ) + rover_launch_path = os.path.join( + launch_dir, + 'launch', + 'ublox_mb+r_rover.launch.py' + ) + + gps_base = IncludeLaunchDescription( + PythonLaunchDescriptionSource(base_launch_path), + launch_arguments={ + 'namespace': 'gps_left', + 'device_family': 'F9P', + 'device_serial_string': 'GPSF', # left/base receiver serial + 'frame_id': 'gps_left_link', + }.items() + ) + + gps_rover = IncludeLaunchDescription( + PythonLaunchDescriptionSource(rover_launch_path), + launch_arguments={ + 'namespace': 'gps_right', + 'device_family': 'F9P', + 'device_serial_string': 'GPSB', # right/rover receiver serial + 'frame_id': 'gps_right_link', + }.items() + ) + + gps_rover_delayed = TimerAction( + period=5.0, + actions=[ + LogInfo(msg='Moving base started. Waiting 5s before starting rover...'), + gps_rover + ] + ) + + return LaunchDescription([ + gps_base, + gps_rover_delayed, + ]) \ No newline at end of file diff --git a/launch/oldgps.launch.py b/launch/oldgps.launch.py new file mode 100644 index 0000000..9f82e8f --- /dev/null +++ b/launch/oldgps.launch.py @@ -0,0 +1,59 @@ +""" +UMRT Gps launch file +""" + +""" +Imports +""" +from launch import LaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.substitutions import LaunchConfiguration +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy +from launch_ros.actions import Node +import os +from ament_index_python.packages import get_package_share_directory +from launch.actions import RegisterEventHandler, TimerAction, LogInfo +from launch.event_handlers import OnProcessStart + +""" +Generate Launch Description +""" +def generate_launch_description(): + + # Path to this package's launch dir + launch_dir = get_package_share_directory('umrt-localization-ros') + launch_path = os.path.join(launch_dir, 'launch','ublox.launch.py') + + gps_1 = IncludeLaunchDescription( + PythonLaunchDescriptionSource(launch_path), + launch_arguments={ + 'namespace': 'gps_left', + 'device_family': 'F9P', + 'device_serial_string': 'GPSF', # Replace with actual serial + 'frame_id': 'gps_left_link', + }.items() + ) + + gps_2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource(launch_path), + launch_arguments={ + 'namespace': 'gps_right', + 'device_family': 'F9P', + 'device_serial_string': 'GPSB', # Replace with actual serial + 'frame_id': 'gps_right_link', + }.items() + ) + + gps_2_delayed = TimerAction( + period = 5.0, + actions = [ + LogInfo(msg='GPS_LEFT started. Waiting 5s for USB to claim before starting GPS_RIGHT...'), + gps_2 + ] + ) + + return LaunchDescription([ + gps_1, + gps_2_delayed, + ]) diff --git a/launch/ublox.launch.py b/launch/ublox.launch.py new file mode 100644 index 0000000..0d7926f --- /dev/null +++ b/launch/ublox.launch.py @@ -0,0 +1,89 @@ +""" Launch ublox_dgnss_node publishing high precision Lon/Lat messages""" +import launch +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, TextSubstitution +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +def generate_launch_description(): + """Generate launch description for ublox_dgnss components.""" + + namespace = LaunchConfiguration('namespace') + device_family = LaunchConfiguration("device_family") + device_serial_string = LaunchConfiguration('device_serial_string') + frame_id = LaunchConfiguration('frame_id') + + + log_level_arg = DeclareLaunchArgument( + "log_level", default_value=TextSubstitution(text="INFO") + ) + namespace_arg = DeclareLaunchArgument( + "namespace", default_value="" + ) + device_family_arg = DeclareLaunchArgument( + "device_family", default_value=TextSubstitution(text="F9P") + ) + device_serial_string_arg = DeclareLaunchArgument( + "device_serial_string", + default_value="", + description="Serial string of the device to use" + ) + frame_id_arg = DeclareLaunchArgument( + "frame_id", + default_value="ubx", + description="The frame_id to use in header of published messages" + ) + + params = [{"DEVICE_FAMILY": device_family}, + {'DEVICE_SERIAL_STRING': device_serial_string}, + {'FRAME_ID': frame_id}, + {'CFG_USBOUTPROT_NMEA': False}, + {'CFG_RATE_MEAS': 10}, + {'CFG_RATE_NAV': 100}, + {'CFG_MSGOUT_UBX_NAV_HPPOSLLH_USB': 1}, + {'CFG_MSGOUT_UBX_NAV_STATUS_USB': 5}, + {'CFG_MSGOUT_UBX_NAV_COV_USB': 1}, + {'CFG_MSGOUT_UBX_RXM_RTCM_USB': 1}] + + container1 = ComposableNodeContainer( + name='ublox_dgnss_container', + namespace=namespace, + package='rclcpp_components', + executable='component_container_mt', + arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')], + composable_node_descriptions=[ + ComposableNode( + package='ublox_dgnss_node', + plugin='ublox_dgnss::UbloxDGNSSNode', + name='ublox_dgnss', + namespace=namespace, + parameters=params + ) + ] + ) + + container2 = ComposableNodeContainer( + name='ublox_nav_sat_fix_hp_container', + namespace=namespace, + package='rclcpp_components', + executable='component_container_mt', + arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')], + composable_node_descriptions=[ + ComposableNode( + package='ublox_nav_sat_fix_hp_node', + plugin='ublox_nav_sat_fix_hp::UbloxNavSatHpFixNode', + name='ublox_nav_sat_fix_hp', + namespace=namespace + ) + ] + ) + + return launch.LaunchDescription([ + log_level_arg, + device_family_arg, + namespace_arg, + device_serial_string_arg, + frame_id_arg, + container1, + container2, + ]) \ No newline at end of file diff --git a/launch/ublox_mb+r_base.launch.py b/launch/ublox_mb+r_base.launch.py new file mode 100644 index 0000000..9f9ca99 --- /dev/null +++ b/launch/ublox_mb+r_base.launch.py @@ -0,0 +1,126 @@ +""" Launch ublox_dgnss_node publishing high precision Lon/Lat messages""" +import launch +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import TextSubstitution +from launch.substitutions import LaunchConfiguration + +def generate_launch_description(): + """Generate launch description for ublox_dgnss components.""" + + namespace = LaunchConfiguration('namespace') + device_serial_string = LaunchConfiguration('device_serial_string') + device_family = LaunchConfiguration("device_family") + frame_id = LaunchConfiguration('frame_id') + + log_level_arg = DeclareLaunchArgument( + "log_level", default_value=TextSubstitution(text="INFO") + ) + device_family_arg = DeclareLaunchArgument( + "device_family", default_value=TextSubstitution(text="F9P") + ) + device_serial_string_arg = DeclareLaunchArgument( + "device_serial_string", + default_value="", + description="Serial string of the device to use" + ) + frame_id_arg = DeclareLaunchArgument( + "frame_id", + default_value="ubx", + description="The frame_id to use in header of published messages" + ) + + params_base= [ + {"DEVICE_FAMILY": device_family}, + {'DEVICE_SERIAL_STRING': device_serial_string}, + {'FRAME_ID': frame_id}, + + # config measurement interval to 200 ms (ie 5 Hz) and nav update rate to once per measurement + {'CFG_RATE_MEAS': 0xc8}, + {'CFG_RATE_NAV': 0x1}, + + # disable all messages on UART1 + {'CFG_UART1INPROT_NMEA': False}, + {'CFG_UART1INPROT_RTCM3X': False}, + {'CFG_UART1INPROT_UBX': False}, + {'CFG_UART1OUTPROT_NMEA': False}, + {'CFG_UART1OUTPROT_RTCM3X': False}, + {'CFG_UART1OUTPROT_UBX': False}, + + # set UART2 baud rate to 460800 + {'CFG_UART2_BAUDRATE': 0x70800}, + + + # send RTCM messages only (to rover) on UART2 + {'CFG_UART2INPROT_NMEA': False}, + {'CFG_UART2INPROT_RTCM3X': False}, + {'CFG_UART2INPROT_UBX': False}, + {'CFG_UART2OUTPROT_NMEA': False}, + {'CFG_UART2OUTPROT_RTCM3X': True}, + {'CFG_UART2OUTPROT_UBX': False}, + + # RTCM and UBX messages as required on USB + {'CFG_USBINPROT_NMEA': False}, + {'CFG_USBINPROT_RTCM3X': True}, + {'CFG_USBINPROT_UBX': True}, + {'CFG_USBOUTPROT_NMEA': False}, + {'CFG_USBOUTPROT_RTCM3X': False},#set this back to false + {'CFG_USBOUTPROT_UBX': True}, + + # output RTCM messages required for moving base+rover mode on UART2 + {'CFG_MSGOUT_RTCM_3X_TYPE4072_0_UART2': 0x1}, + {'CFG_MSGOUT_RTCM_3X_TYPE1074_UART2': 0x1}, + {'CFG_MSGOUT_RTCM_3X_TYPE1084_UART2': 0x1}, + {'CFG_MSGOUT_RTCM_3X_TYPE1094_UART2': 0x1}, + {'CFG_MSGOUT_RTCM_3X_TYPE1124_UART2': 0x1}, + {'CFG_MSGOUT_RTCM_3X_TYPE1230_UART2': 0x1}, + + # messages required for navsatfix calcs by ROS node + {'CFG_MSGOUT_UBX_NAV_HPPOSLLH_USB': 0x1}, + {'CFG_MSGOUT_UBX_NAV_COV_USB': 0x1}, + {'CFG_MSGOUT_UBX_NAV_STATUS_USB': 0x1}, + {'CFG_MSGOUT_UBX_NAV_PVT_USB': 0x1}, + + ] + + container_base = ComposableNodeContainer( + name='ublox_dgnss_moving_base', + namespace='', + package='rclcpp_components', + executable='component_container_mt', + arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')], + composable_node_descriptions=[ + ComposableNode( + package='ublox_dgnss_node', + plugin='ublox_dgnss::UbloxDGNSSNode', + name='ublox_dgnss', + namespace='base', + parameters=params_base + ) + ] + ) + + container_navsatfix = ComposableNodeContainer( + name='ublox_nav_sat_fix_hp_container', + namespace='', + package='rclcpp_components', + executable='component_container_mt', + arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')], + composable_node_descriptions=[ + ComposableNode( + package='ublox_nav_sat_fix_hp_node', + plugin='ublox_nav_sat_fix_hp::UbloxNavSatHpFixNode', + namespace='base', + name='ublox_nav_sat_fix_hp' + ) + ] + ) + + return launch.LaunchDescription([ + log_level_arg, + device_family_arg, + device_serial_string_arg, + container_base, + container_navsatfix, + ]) \ No newline at end of file diff --git a/launch/ublox_mb+r_rover.launch.py b/launch/ublox_mb+r_rover.launch.py new file mode 100644 index 0000000..35b8f18 --- /dev/null +++ b/launch/ublox_mb+r_rover.launch.py @@ -0,0 +1,121 @@ +""" Launch ublox_dgnss_node publishing high precision Lon/Lat messages""" +import launch +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import TextSubstitution +from launch.substitutions import LaunchConfiguration + +def generate_launch_description(): + """Generate launch description for ublox_dgnss components.""" + + namespace = LaunchConfiguration('namespace') + device_family = LaunchConfiguration("device_family") + device_serial_string = LaunchConfiguration('device_serial_string') + frame_id = LaunchConfiguration('frame_id') + + log_level_arg = DeclareLaunchArgument( + "log_level", default_value=TextSubstitution(text="INFO") + ) + device_family_arg = DeclareLaunchArgument( + "device_family", default_value=TextSubstitution(text="F9P") + ) + device_serial_string_arg = DeclareLaunchArgument( + "device_serial_string", + default_value="", + description="Serial string of the device to use" + ) + frame_id_arg = DeclareLaunchArgument( + "frame_id", + default_value="ubx", + description="The frame_id to use in header of published messages" + ) + + params_rover = [ + {"DEVICE_FAMILY": device_family}, + {'DEVICE_SERIAL_STRING': device_serial_string}, + {'FRAME_ID': frame_id}, + + # config measurement interval to 200 ms (ie 5 Hz) and nav update rate to once per measurement + {'CFG_RATE_MEAS': 0xc8}, + {'CFG_RATE_NAV': 0x1}, + + # disable all messages on UART1 + {'CFG_UART1INPROT_NMEA': False}, + {'CFG_UART1INPROT_RTCM3X': True}, + {'CFG_UART1INPROT_UBX': False}, + {'CFG_UART1OUTPROT_NMEA': False}, + {'CFG_UART1OUTPROT_RTCM3X': False}, + {'CFG_UART1OUTPROT_UBX': False}, + + # set UART2 baud rate to 460800 + {'CFG_UART1_BAUDRATE': 0x70800}, + {'CFG_UART2_BAUDRATE': 0x70800}, + + # receive RTCM messages only (from base) on UART2 + {'CFG_UART2INPROT_NMEA': False}, + {'CFG_UART2INPROT_RTCM3X': True}, + {'CFG_UART2INPROT_UBX': False}, + {'CFG_UART2OUTPROT_NMEA': False}, + {'CFG_UART2OUTPROT_RTCM3X': False}, + {'CFG_UART2OUTPROT_UBX': False}, + + # send/receive UBX messages only on USB + {'CFG_USBINPROT_NMEA': False}, + {'CFG_USBINPROT_RTCM3X': False}, + {'CFG_USBINPROT_UBX': True}, + {'CFG_USBOUTPROT_NMEA': False}, + {'CFG_USBOUTPROT_RTCM3X': False}, + {'CFG_USBOUTPROT_UBX': True}, + + # messages required for navsatfix calcs by ROS node + {'CFG_MSGOUT_UBX_NAV_HPPOSLLH_USB': 0x1}, + {'CFG_MSGOUT_UBX_NAV_COV_USB': 0x1}, + {'CFG_MSGOUT_UBX_NAV_STATUS_USB': 0x1}, + {'CFG_MSGOUT_UBX_NAV_PVT_USB': 0x1}, + + # output relative position messages + {'CFG_MSGOUT_UBX_NAV_RELPOSNED_USB': 0x1}, + {'CFG_MSGOUT_UBX_RXM_RTCM_USB': 0x1}, + ] + + container_rover = ComposableNodeContainer( + name='ublox_dgnss_rover', + namespace='', + package='rclcpp_components', + executable='component_container_mt', + arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')], + composable_node_descriptions=[ + ComposableNode( + package='ublox_dgnss_node', + plugin='ublox_dgnss::UbloxDGNSSNode', + name='ublox_dgnss', + namespace='rover', + parameters=params_rover + ) + ] + ) + + container_navsatfix = ComposableNodeContainer( + name='ublox_nav_sat_fix_hp_container', + namespace='', + package='rclcpp_components', + executable='component_container_mt', + arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')], + composable_node_descriptions=[ + ComposableNode( + package='ublox_nav_sat_fix_hp_node', + plugin='ublox_nav_sat_fix_hp::UbloxNavSatHpFixNode', + namespace='rover', + name='ublox_nav_sat_fix_hp' + ) + ] + ) + + return launch.LaunchDescription([ + log_level_arg, + device_family_arg, + device_serial_string_arg, + container_rover, + container_navsatfix, + ]) \ No newline at end of file diff --git a/src/heading.cpp b/src/heading.cpp index c1599cb..38c043e 100644 --- a/src/heading.cpp +++ b/src/heading.cpp @@ -3,12 +3,11 @@ // #include - -#include "project-name/example_node.hpp" +#include "umrt-localization-ros/gps_node.hpp" int main(int argc, char* argv[]){ rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } \ No newline at end of file diff --git a/src/heading_node.cpp b/src/heading_node.cpp index 0e2066b..5e02ebf 100644 --- a/src/heading_node.cpp +++ b/src/heading_node.cpp @@ -1,9 +1,143 @@ -#include "project-name/example_node.hpp" +#include "umrt-localization-ros/gps_node.hpp" -#include +#include -ExampleNode::ExampleNode() : Node("example") {} +#include +#include -void ExampleNode::example() { - std::cout << "Hello World" << std::endl; +GpsNode::GpsNode() : Node("gps_node") +{ + rclcpp::QoS qos(10); + qos.reliable(); + qos.transient_local(); + + relpos_sub = this->create_subscription( + "/rover/ubx_nav_rel_pos_ned", + qos, + std::bind(&GpsNode::relposCallback, this, std::placeholders::_1) + ); + + RCLCPP_INFO(this->get_logger(), "Moving-base RTK heading node initialized."); +} + +double GpsNode::normalizeHeading(double heading_deg) const +{ + while (heading_deg < 0.0) { + heading_deg += 360.0; + } + while (heading_deg >= 360.0) { + heading_deg -= 360.0; + } + return heading_deg; } + +void GpsNode::relposCallback( + const ublox_ubx_msgs::msg::UBXNavRelPosNED::SharedPtr msg +) +{ + /* + * RELPOSNED gives the vector from the moving base antenna + * to the rover antenna. + * + * Antenna mounting on this robot: + * left antenna = base + * right antenna = rover + * + * So the baseline vector points left -> right (sideways). + * Robot forward heading = baseline heading - 90 degrees. + */ + + // --- Gate on solution validity before trusting anything --- + // + // Field layout per ublox_ubx_msgs: validity flags are flat bools on the + // message, and carr_soln is a CarrSoln sub-message with a .status field + // plus named constants. Confirm with: + // ros2 interface show ublox_ubx_msgs/msg/UBXNavRelPosNED + + if (!msg->gnss_fix_ok || !msg->rel_pos_valid) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, + "RELPOSNED not valid yet (gnss_fix_ok=%d, rel_pos_valid=%d); skipping.", + static_cast(msg->gnss_fix_ok), + static_cast(msg->rel_pos_valid) + ); + return; + } + + const uint8_t carr_soln = msg->carr_soln.status; + if (carr_soln != + ublox_ubx_msgs::msg::CarrSoln::CARRIER_SOLUTION_PHASE_WITH_FIXED_AMBIGUITIES) + { + // 0 = no carrier solution, 1 = RTK float, 2 = RTK fixed. + // Float headings can be off by several degrees; log but don't trust. + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, + "RTK not fixed (carr_soln=%d); heading unreliable, skipping.", + static_cast(carr_soln) + ); + return; + } + + // --- Build the baseline vector in meters, including HP components --- + // rel_pos_n/e are in cm; rel_pos_hp_n/hp_e are in 0.1 mm. + // Full HP value in cm = rel_pos_n + rel_pos_hp_n * 1e-2, then * 1e-2 for m. + const double rel_n = + static_cast(msg->rel_pos_n) * 1e-2 + + static_cast(msg->rel_pos_hp_n) * 1e-4; + const double rel_e = + static_cast(msg->rel_pos_e) * 1e-2 + + static_cast(msg->rel_pos_hp_e) * 1e-4; + + if (rel_n == 0.0 && rel_e == 0.0) { + RCLCPP_WARN(this->get_logger(), + "RELPOSNED baseline is zero; cannot compute heading."); + return; + } + + // --- Sanity check: baseline length vs. known antenna separation --- + // Set this to your measured antenna-to-antenna distance in meters. + // If the RTK length disagrees, the ambiguities fixed wrongly and the + // heading is garbage (see u-blox app note UBX-19009093, section 2.4). + constexpr double kExpectedBaselineM = 1.0; // <-- EDIT to your robot + constexpr double kBaselineToleranceM = 0.05; // +/- 5 cm + + const double baseline_len = std::sqrt(rel_n * rel_n + rel_e * rel_e); + if (std::fabs(baseline_len - kExpectedBaselineM) > kBaselineToleranceM) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, + "Baseline length %.3f m differs from expected %.2f m; " + "possible wrong ambiguity fix, skipping.", + baseline_len, kExpectedBaselineM + ); + return; + } + + // --- Compute heading --- + double baseline_heading = + std::atan2(rel_e, rel_n) * 180.0 / M_PI; + baseline_heading = normalizeHeading(baseline_heading); + + const double heading_angle = normalizeHeading(baseline_heading - 90.0); + + std::string heading_dir = "N/A"; + if (heading_angle >= 337.5 || heading_angle < 22.5) heading_dir = "N"; + else if (heading_angle < 67.5) heading_dir = "NE"; + else if (heading_angle < 112.5) heading_dir = "E"; + else if (heading_angle < 157.5) heading_dir = "SE"; + else if (heading_angle < 202.5) heading_dir = "S"; + else if (heading_angle < 247.5) heading_dir = "SW"; + else if (heading_angle < 292.5) heading_dir = "W"; + else if (heading_angle < 337.5) heading_dir = "NW"; + + RCLCPP_INFO( + this->get_logger(), + "Heading: %s, angle: %.2f deg, baseline heading: %.2f deg, " + "rel_n: %.3f m, rel_e: %.3f m, len: %.3f m", + heading_dir.c_str(), + heading_angle, + baseline_heading, + rel_n, + rel_e, + baseline_len + ); +} \ No newline at end of file diff --git a/version b/version index 7bcd0e3..6812f81 100644 --- a/version +++ b/version @@ -1 +1 @@ -0.0.2 \ No newline at end of file +0.0.3 \ No newline at end of file