Skip to content
Permalink

Comparing changes

Choose two branches to see what’s changed or to start a new pull request. If you need to, you can also or learn more about diff comparisons.

Open a pull request

Create a new pull request by comparing changes across two branches. If you need to, you can also . Learn more about diff comparisons here.
base repository: septentrio-gnss/septentrio_gnss_driver
Failed to load repositories. Confirm that selected base ref is valid, then try again.
Loading
base: master
Choose a base ref
...
head repository: Greenroom-Robotics/septentrio_gnss_driver
Failed to load repositories. Confirm that selected head ref is valid, then try again.
Loading
compare: master
Choose a head ref
Able to merge. These branches can be automatically merged.
  • 5 commits
  • 12 files changed
  • 4 contributors

Commits on Oct 20, 2024

  1. feat: add twist and geopose publishers in FLU (#5)

    * feat: add some publisher
    
    * chore: fix cmake issues
    
    * chore: fixes
    
    * chore: woops
    
    * chore: tidy launch
    
    * feat: add frame_id
    
    * chore: add pycache
    
    * chore: woop
    
    * chore: more woops
    MrBlenny authored Oct 20, 2024

    Verified

    This commit was created on GitHub.com and signed with GitHub’s verified signature.
    Copy the full SHA
    da1ef54 View commit details
  2. Copy the full SHA
    f188cfc View commit details

Commits on Oct 22, 2024

  1. Copy the full SHA
    7196f38 View commit details
  2. fix: check nans in geopose

    KyleJewiss committed Oct 22, 2024
    Copy the full SHA
    1b34388 View commit details

Commits on Oct 30, 2024

  1. fix: reduce log spam (#6)

    * feat: added throttle logging option
    
    * fix: throttled error message so it doesn't spam
    KyleJewiss authored Oct 30, 2024

    Verified

    This commit was created on GitHub.com and signed with GitHub’s verified signature.
    Copy the full SHA
    5b7a5e9 View commit details
52 changes: 52 additions & 0 deletions .github/workflows/release.yml
Original file line number Diff line number Diff line change
@@ -0,0 +1,52 @@
name: Tag & Release

on:
workflow_dispatch:
inputs:
package:
type: choice
description: 'If not specified, all packages will be released.'
options:
- ""
- septentrio_gnss_driver

jobs:
release:
strategy:
fail-fast: true
matrix:
job:
- runner: buildjet-8vcpu-ubuntu-2204
arch: amd64
ros_distro: iron
github_release: true
- runner: buildjet-16vcpu-ubuntu-2204-arm
arch: arm64
ros_distro: iron
github_release: false
- runner: buildjet-8vcpu-ubuntu-2204
arch: amd64
ros_distro: jazzy
github_release: false
- runner: buildjet-16vcpu-ubuntu-2204-arm
arch: arm64
ros_distro: jazzy
github_release: false

name: Release - ${{ matrix.job.arch }} - ${{ matrix.job.ros_distro }}
runs-on: ${{ matrix.job.runner }}

steps:
- name: Checkout this repository
uses: actions/checkout@v3

- name: Semantic release
uses: Greenroom-Robotics/ros_semantic_release_action@main
with:
token: ${{ secrets.API_TOKEN_GITHUB }}
package: ${{ github.event.inputs.package }}
arch: ${{ matrix.job.arch }}
ros_distro: ${{ matrix.job.ros_distro }}
github_release: ${{ matrix.job.github_release }}
public: true
changelog: false
5 changes: 4 additions & 1 deletion .gitignore
Original file line number Diff line number Diff line change
@@ -1,4 +1,7 @@
#config/rover.yaml
.vscode
build
msg/BlockHeader.msg
install
log
msg/BlockHeader.msg
__pycache__
8 changes: 5 additions & 3 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -1,8 +1,8 @@
cmake_minimum_required(VERSION 3.5)
project(septentrio_gnss_driver)

## Compile as C++17
add_compile_options(-std=c++17)
## Compile as C++20
add_compile_options(-std=c++20)

if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES)
message(STATUS "Setting build type to Release as none was specified.")
@@ -166,7 +166,8 @@ elseif(ament_cmake_FOUND)
find_package(tf2_eigen REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(tf2_ros REQUIRED)

find_package(geographic_msgs REQUIRED)

## Declare messages
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/AIMPlusStatus.msg"
@@ -214,6 +215,7 @@ elseif(ament_cmake_FOUND)
tf2_eigen
tf2_geometry_msgs
tf2_ros
geographic_msgs
)

## shared library
30 changes: 28 additions & 2 deletions include/septentrio_gnss_driver/abstraction/typedefs.hpp
Original file line number Diff line number Diff line change
@@ -52,8 +52,11 @@
#include <diagnostic_msgs/msg/diagnostic_status.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <geometry_msgs/msg/quaternion.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <geometry_msgs/msg/twist_with_covariance.hpp>
#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
#include <geographic_msgs/msg/geo_pose_with_covariance_stamped.hpp>
#include <geographic_msgs/msg/geo_pose_stamped.hpp>
#include <gps_msgs/msg/gps_fix.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <sensor_msgs/msg/imu.hpp>
@@ -108,7 +111,10 @@ typedef diagnostic_msgs::msg::DiagnosticStatus DiagnosticStatusMsg;
typedef geometry_msgs::msg::Quaternion QuaternionMsg;
typedef geometry_msgs::msg::PoseWithCovarianceStamped PoseWithCovarianceStampedMsg;
typedef geometry_msgs::msg::TwistWithCovarianceStamped TwistWithCovarianceStampedMsg;
typedef geometry_msgs::msg::TwistStamped TwistStampedMsg;
typedef geometry_msgs::msg::TransformStamped TransformStampedMsg;
typedef geographic_msgs::msg::GeoPoseStamped GeoPoseStampedMsg;
typedef geographic_msgs::msg::GeoPoseWithCovarianceStamped GeoPoseWithCovarianceStampedMsg;
typedef gps_msgs::msg::GPSFix GpsFixMsg;
typedef gps_msgs::msg::GPSStatus GpsStatusMsg;
typedef sensor_msgs::msg::NavSatFix NavSatFixMsg;
@@ -177,10 +183,15 @@ namespace log_level {
enum LogLevel
{
DEBUG,
DEBUG_THROTTLE,
INFO,
INFO_THROTTLE,
WARN,
WARN_THROTTLE,
ERROR,
FATAL
ERROR_THROTTLE,
FATAL,
FATAL_THROTTLE
};
} // namespace log_level

@@ -282,25 +293,40 @@ class ROSaicNodeBase : public rclcpp::Node
* @param[in] logLevel Log level
* @param[in] s String to log
*/
void log(log_level::LogLevel logLevel, const std::string& s) const
void log(log_level::LogLevel logLevel, const std::string& s, std::chrono::milliseconds duration = std::chrono::milliseconds(0)) const
{
switch (logLevel)
{
case log_level::DEBUG:
RCLCPP_DEBUG_STREAM(this->get_logger(), s);
break;
case log_level::DEBUG_THROTTLE:
RCLCPP_DEBUG_THROTTLE(this->get_logger(), *this->get_clock(), duration.count(), "%s", s.c_str());
break;
case log_level::INFO:
RCLCPP_INFO_STREAM(this->get_logger(), s);
break;
case log_level::INFO_THROTTLE:
RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), duration.count(), "%s", s.c_str());
break;
case log_level::WARN:
RCLCPP_WARN_STREAM(this->get_logger(), s);
break;
case log_level::WARN_THROTTLE:
RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), duration.count(), "%s", s.c_str());
break;
case log_level::ERROR:
RCLCPP_ERROR_STREAM(this->get_logger(), s);
break;
case log_level::ERROR_THROTTLE:
RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), duration.count(), "%s", s.c_str());
break;
case log_level::FATAL:
RCLCPP_FATAL_STREAM(this->get_logger(), s);
break;
case log_level::FATAL_THROTTLE:
RCLCPP_FATAL_THROTTLE(this->get_logger(), *this->get_clock(), duration.count(), "%s", s.c_str());
break;
default:
break;
}
5 changes: 3 additions & 2 deletions include/septentrio_gnss_driver/communication/io.hpp
Original file line number Diff line number Diff line change
@@ -295,12 +295,13 @@ namespace io {
while (node_->ok() && ec)
{
node_->log(
log_level::ERROR,
log_level::ERROR_THROTTLE,
"TCP connection to " +
endpointIterator->endpoint().address().to_string() +
" on port " +
std::to_string(endpointIterator->endpoint().port()) +
" failed: " + ec.message() + ". Retrying ...");
" failed: " + ec.message() + ". Retrying ...",
std::chrono::milliseconds(5000));
using namespace std::chrono_literals;
std::this_thread::sleep_for(1s);
ec = connectInternal(endpointIterator);
6 changes: 6 additions & 0 deletions include/septentrio_gnss_driver/communication/settings.hpp
Original file line number Diff line number Diff line change
@@ -318,6 +318,12 @@ struct Settings
bool publish_gpsfix;
//! Whether or not to publish the PoseWithCovarianceStampedMsg message
bool publish_pose;
//! Whether or not to publish the GeoposeMsg message
bool publish_geopose_stamped;
//! Whether or not to publish the GeoposeWithCovarianceStampedMsg message
bool publish_geopose_covariance_stamped;
//! Whether or not to publish the TwistMsg message
bool publish_twist_flu_stamped;
//! Whether or not to publish the DiagnosticArrayMsg message
bool publish_diagnostics;
//! Whether or not to publish the ImuMsg message
Original file line number Diff line number Diff line change
@@ -175,6 +175,8 @@ namespace settings {
settings.publish_navsatfix = true;
settings.publish_gpsfix = true;
settings.publish_pose = true;
settings.publish_geopose_stamped = true;
settings.publish_geopose_covariance_stamped = true;
settings.publish_diagnostics = true;
settings.publish_aimplusstatus = true;
settings.publish_galauthstatus = true;
@@ -204,6 +206,7 @@ namespace settings {
settings.publish_localization = true;
settings.publish_localization_ecef = true;
settings.publish_twist = true;
settings.publish_twist_flu_stamped = true;
if (!settings.publish_tf_ecef)
settings.publish_tf = true;
} else if (settings.auto_publish && settings.configure_rx)
134 changes: 134 additions & 0 deletions launch/greenroom.launch.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,134 @@
import os
import launch
from launch_ros.actions import Node

os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{time}: [{name}] [{severity}]\t{message}'

def generate_launch_description():

tf_imu = Node(
package="tf2_ros",
executable="static_transform_publisher",
arguments = "0 0 0 0 0 0 base_link imu".split(' ')
)

tf_gnss = Node(
package="tf2_ros",
executable="static_transform_publisher",
arguments = "0 0 0 0 0 0 imu gnss".split(' ')
)

tf_vsm = Node(
package="tf2_ros",
executable="static_transform_publisher",
arguments = "0 0 0 0 0 0 imu vsm".split(' ')
)

tf_aux1 = Node(
package="tf2_ros",
executable="static_transform_publisher",
arguments = "0 0 0 0 0 0 imu aux1".split(' ')
)

parameters_list = [
{
"covariance_threshold": 50.0,
"device": "tcp://10.27.1.102:28784",
"configure_rx": True,
# "custom_commands_file": "",
# "login": {"user": "", "password": ""},
# "osnma": {"mode": "off"},
# "ntp_server": True,
"keep_open": True,
"frame_id": "septentrio_gnss",
"imu_frame_id": "imu",
"poi_frame_id": "base_link",
"vsm_frame_id": "vsm",
"aux1_frame_id": "aux1",
"vehicle_frame_id": "base_link",
"local_frame_id": "odom",
"insert_local_frame": True,
# "get_spatial_config_from_tf": False,
"lock_utm_zone": True,
"use_ros_axis_orientation": True,
"receiver_type": "ins",
"multi_antenna": True,
"datum": "Default",
# "poi_to_arp": {"delta_e": 0.0, "delta_n": 0.0, "delta_u": 0.0},
# "att_offset": {"heading": 90.0, "pitch": 0.0},
# "ant_type": "Unknown",
# "ant_serial_nr": "Unknown",
# "ant_aux1_type": "Unknown",
# "ant_aux1_serial_nr": "Unknown",
# "polling_period": {"pvt": 100, "rest": 500},
# "use_gnss_time": False,
# "ptp_server_clock": False,
# "latency_compensation": True,
# "rtk_settings": {
# "ntrip_1": {
# "id": "",
# "caster": "",
# "caster_port": 2101,
# "username": "",
# "password": "",
# "mountpoint": "",
# "version": "v2",
# "tls": False,
# "fingerprint": "",
# "rtk_standard": "auto",
# "send_gga": "auto",
# "keep_open": True,
# },
# "ip_server_1": {
# "id": "",
# "port": 0,
# "rtk_standard": "auto",
# "send_gga": "auto",
# "keep_open": True,
# },
# "serial_1": {
# "port": "",
# "baud_rate": 115200,
# "rtk_standard": "auto",
# "send_gga": "auto",
# "keep_open": True,
# },
# },
"publish": {
"geopose_stamped": True,
"twist_flu_stamped": True,
"diagnostics": True,
},
"ins_spatial_config": {
"imu_orientation": {"theta_x": 0.0, "theta_y": 0.0, "theta_z": 0.0},
"poi_lever_arm": {"delta_x": 0.5, "delta_y": 0.0, "delta_z": 0.0},
"ant_lever_arm": {"x": -0.5, "y": 0.0, "z": 0.0},
"vsm_lever_arm": {"vsm_x": 0.0, "vsm_y": 0.0, "vsm_z": 0.0},
},
"ins_initial_heading": "auto",
"ins_std_dev_mask": {"att_std_dev": 5.0, "pos_std_dev": 10.0},
# "ins_use_poi": False,
"ins_vsm": {
"ros": {
"source": "",
"config": [False, False, False],
"variances_by_parameter": False,
"variances": [0.0, 0.0, 0.0],
},
"ip_server": {"id": "", "port": 0, "keep_open": True},
"serial": {"port": "", "baud_rate": 115200, "keep_open": True},
},
"activate_debug_log": False,
}
]

node = Node(
package='septentrio_gnss_driver',
executable='septentrio_gnss_driver_node',
name='septentrio_gnss_driver',
emulate_tty=True,
sigterm_timeout = '20',
parameters=parameters_list
)

return launch.LaunchDescription([node, tf_imu, tf_gnss, tf_vsm, tf_aux1])
2 changes: 2 additions & 0 deletions package.xml
Original file line number Diff line number Diff line change
@@ -39,6 +39,8 @@
<depend>nmea_msgs</depend>
<depend>sensor_msgs</depend>
<depend>geometry_msgs</depend>
<depend>geographic_msgs</depend>
<depend>gps_msgs</depend>
<depend condition="$ROS_VERSION == 2">gps_msgs</depend>
<depend condition="$ROS_VERSION == 1">gps_common</depend>
<depend>nav_msgs</depend>
Loading