diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml new file mode 100644 index 0000000..7312fff --- /dev/null +++ b/.github/workflows/main.yml @@ -0,0 +1,16 @@ +name: CI + +on: [push, pull_request] + +jobs: + industrial_ci: + strategy: + matrix: + env: + - {ROS_DISTRO: noetic, ROS_REPO: testing} + - {ROS_DISTRO: noetic, ROS_REPO: main} + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - uses: 'ros-industrial/industrial_ci@master' + env: ${{matrix.env}} diff --git a/.travis.yml b/.travis.yml deleted file mode 100644 index 0c32515..0000000 --- a/.travis.yml +++ /dev/null @@ -1,25 +0,0 @@ -sudo: required -dist: trusty -language: generic -compiler: - - gcc -notifications: - email: - on_success: always - on_failure: always -env: - matrix: - - USE_DEB=true ROS_DISTRO="indigo" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu - - USE_DEB=true ROS_DISTRO="indigo" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu - - USE_DEB=true ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu - - USE_DEB=true ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu - - USE_DEB=true ROS_DISTRO="lunar" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu - - USE_DEB=true ROS_DISTRO="lunar" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu -install: - - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config -matrix: - allow_failures: - - env: ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu -script: - - source .ci_config/travis.sh -# - source ./travis.sh # Enable this when you have a package-local script diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..b8c0cdb --- /dev/null +++ b/LICENSE @@ -0,0 +1,11 @@ +Copyright 2023, Ken Tossell + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS “AS IS” AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/gps_common/CHANGELOG.rst b/gps_common/CHANGELOG.rst index 2cf0c68..03ba1dc 100644 --- a/gps_common/CHANGELOG.rst +++ b/gps_common/CHANGELOG.rst @@ -2,6 +2,20 @@ Changelog for package gps_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.3.4 (2023-06-14) +------------------ + +0.3.3 (2022-11-28) +------------------ +* Fix truncation warning for UTM zone snprintf() (`#44 `_) +* Contributors: Kevin Hallenbeck + +0.3.2 (2020-05-25) +------------------ + +0.3.1 (2020-03-05) +------------------ + 0.3.0 (2019-10-03) ------------------ * Switching order of commands to make catkin happy diff --git a/gps_common/CMakeLists.txt b/gps_common/CMakeLists.txt index 44567df..3d7b13f 100644 --- a/gps_common/CMakeLists.txt +++ b/gps_common/CMakeLists.txt @@ -28,6 +28,7 @@ catkin_python_setup() add_message_files( FILES GPSStatus.msg + GPSExtendedStatus.msg GPSFix.msg ) diff --git a/gps_common/include/gps_common/conversions.h b/gps_common/include/gps_common/conversions.h index 4b9869e..f73bf2d 100644 --- a/gps_common/include/gps_common/conversions.h +++ b/gps_common/include/gps_common/conversions.h @@ -183,7 +183,7 @@ static inline void LLtoUTM(const double Lat, const double Long, LongOriginRad = LongOrigin * RADIANS_PER_DEGREE; //compute the UTM Zone from the latitude and longitude - snprintf(UTMZone, 4, "%d%c", ZoneNumber, UTMLetterDesignator(Lat)); + snprintf(UTMZone, 13, "%d%c", ZoneNumber, UTMLetterDesignator(Lat)); eccPrimeSquared = (eccSquared)/(1-eccSquared); @@ -210,7 +210,7 @@ static inline void LLtoUTM(const double Lat, const double Long, static inline void LLtoUTM(const double Lat, const double Long, double &UTMNorthing, double &UTMEasting, std::string &UTMZone) { - char zone_buf[] = {0, 0, 0, 0}; + char zone_buf[13] = {0}; LLtoUTM(Lat, Long, UTMNorthing, UTMEasting, zone_buf); diff --git a/gps_common/msg/GPSExtendedStatus.msg b/gps_common/msg/GPSExtendedStatus.msg new file mode 100644 index 0000000..de5816e --- /dev/null +++ b/gps_common/msg/GPSExtendedStatus.msg @@ -0,0 +1,9 @@ +# Extended Measurement status to use for GPSStatus.msg +int16 STATUS_NO_FIX=-1 # Unable to fix position +int16 STATUS_FIX=0 # Normal fix +int16 STATUS_SBAS_FIX=1 # Fixed using a satellite-based augmentation system +int16 STATUS_GBAS_FIX=2 # or a ground-based augmentation system +int16 STATUS_DGPS_FIX=18 # Fixed with DGPS +int16 STATUS_RTK_FIX=19 # Real-Time Kinematic, fixed integers +int16 STATUS_RTK_FLOAT=20 # Real-Time Kinematic, float integers +int16 STATUS_WAAS_FIX=33 # Fixed with WAAS diff --git a/gps_common/package.xml b/gps_common/package.xml index 206e566..b5d868a 100644 --- a/gps_common/package.xml +++ b/gps_common/package.xml @@ -1,11 +1,12 @@ gps_common - 0.3.0 + 0.3.4 GPS messages and common routines for use in GPS drivers - Timo Roehling - P. J. Reed + Timo Roehling + P. J. Reed + Southwest Research Institute BSD http://ros.org/wiki/gps_common diff --git a/gps_umd/CHANGELOG.rst b/gps_umd/CHANGELOG.rst index 24ecd43..c59ef12 100644 --- a/gps_umd/CHANGELOG.rst +++ b/gps_umd/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package gps_umd ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.3.4 (2023-06-14) +------------------ + +0.3.3 (2022-11-28) +------------------ + +0.3.2 (2020-05-25) +------------------ + +0.3.1 (2020-03-05) +------------------ + 0.3.0 (2019-10-03) ------------------ diff --git a/gps_umd/package.xml b/gps_umd/package.xml index 856ee82..50aa502 100644 --- a/gps_umd/package.xml +++ b/gps_umd/package.xml @@ -1,11 +1,12 @@ gps_umd - 0.3.0 + 0.3.4 gps_umd metapackage - Ken Tossell - P. J. Reed + Ken Tossell + P. J. Reed + Southwest Research Institute BSD diff --git a/gpsd_client/CHANGELOG.rst b/gpsd_client/CHANGELOG.rst index d20374f..29ed45d 100644 --- a/gpsd_client/CHANGELOG.rst +++ b/gpsd_client/CHANGELOG.rst @@ -2,6 +2,33 @@ Changelog for package gpsd_client ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.3.4 (2023-06-14) +------------------ +* GPSD API v12 Compatibility (`#78 `_) +* Contributors: MariuszSzczepanikSpyrosoft + +0.3.3 (2022-11-28) +------------------ +* Adding better debugging output to help diagnose corner case (`#59 `_) + * Adding better debugging output to help diagnose corner case + DISTRIBUTION A. Approved for public release; distribution unlimited. OPSEC `#4584 `_ +* Merge pull request `#39 `_ from shr-project/jansa/gpsd + Fix build with gpsd-3.21 +* Fix build with gpsd-3.21 + Adapt to changes from this commit: + https://gitlab.com/gpsd/gpsd/-/commit/29991d6ffeb41ecfc8297db68bb68be0128c8514 +* Contributors: David Anthony, Martin Jansa + +0.3.2 (2020-05-25) +------------------ +* Fix build with gpsd 3.20 (`#28 `_) +* Contributors: P. J. Reed + +0.3.1 (2020-03-05) +------------------ +* Fix for gpsd-3.19 compatibility (`#26 `_) +* Contributors: P. J. Reed + 0.3.0 (2019-10-03) ------------------ diff --git a/gpsd_client/package.xml b/gpsd_client/package.xml index 7ee936e..1e5edac 100644 --- a/gpsd_client/package.xml +++ b/gpsd_client/package.xml @@ -1,14 +1,15 @@ gpsd_client - 0.3.0 + 0.3.4 connects to a GPSd server and broadcasts GPS fixes using the NavSatFix message - Timo Roehling - P. J. Reed Ken Tossell Rob Thomson + Timo Roehling + P. J. Reed + Southwest Research Institute BSD http://ros.org/wiki/gpsd_client diff --git a/gpsd_client/src/client.cpp b/gpsd_client/src/client.cpp index 3df33db..e87b36d 100644 --- a/gpsd_client/src/client.cpp +++ b/gpsd_client/src/client.cpp @@ -23,12 +23,12 @@ class GPSDClient { privnode.param("frame_id", frame_id, frame_id); std::string host = "localhost"; - int port = 2947; + int port = atoi(DEFAULT_GPSD_PORT); privnode.getParam("host", host); privnode.getParam("port", port); char port_s[12]; - snprintf(port_s, 12, "%d", port); + snprintf(port_s, sizeof(port_s), "%d", port); gps_data_t *resp = NULL; @@ -59,7 +59,12 @@ class GPSDClient { if (!gps->waiting(1e6)) return; - gps_data_t *p = gps->read(); + // Read out all queued data and only act on the latest + gps_data_t* p = NULL; + while (gps->waiting(0)) + { + p = gps->read(); + } #else gps_data_t *p = gps->poll(); #endif @@ -85,8 +90,13 @@ class GPSDClient { if (p == NULL) return; - if (!p->online) +#if GPSD_API_MAJOR_VERSION >= 9 + if (!p->online.tv_sec && !p->online.tv_nsec) { +#else + if (!p->online) { +#endif return; + } process_data_gps(p); process_data_navsat(p); @@ -143,17 +153,34 @@ class GPSDClient { #endif } - if ((p->status & STATUS_FIX) && !(check_fix_by_variance && std::isnan(p->fix.epx))) { +#if GPSD_API_MAJOR_VERSION >= 12 + if ((p->fix.status != STATUS_NO_FIX) && !(check_fix_by_variance && std::isnan(p->fix.epx))) { +#elif GPSD_API_MAJOR_VERSION >= 10 + if ((p->fix.status != STATUS_NO_FIX) && !(check_fix_by_variance && std::isnan(p->fix.epx))) { +#else + if ((p->status != STATUS_NO_FIX) && !(check_fix_by_variance && std::isnan(p->fix.epx))) { +#endif + status.status = 0; // FIXME: gpsmm puts its constants in the global // namespace, so `GPSStatus::STATUS_FIX' is illegal. -// STATUS_DGPS_FIX was removed in API version 6 but re-added afterward +// STATUS_DGPS_FIX was removed in API version 6 but re-added afterward and next renamed since the version 12 #if GPSD_API_MAJOR_VERSION != 6 +#if GPSD_API_MAJOR_VERSION >= 12 + if (p->fix.status & STATUS_DGPS) +#elif GPSD_API_MAJOR_VERSION >= 10 + if (p->fix.status & STATUS_DGPS_FIX) +#else if (p->status & STATUS_DGPS_FIX) +#endif status.status |= 18; // same here #endif +#if GPSD_API_MAJOR_VERSION >= 9 + fix.time = (double)(p->fix.time.tv_sec) + (double)(p->fix.time.tv_nsec) / 1000000.; +#else fix.time = p->fix.time; +#endif fix.latitude = p->fix.latitude; fix.longitude = p->fix.longitude; fix.altitude = p->fix.altitude; @@ -188,7 +215,7 @@ class GPSDClient { /* TODO: attitude */ } else { - status.status = -1; // STATUS_NO_FIX + status.status = -1; // STATUS_NO_FIX or STATUS_UNK } fix.status = status; @@ -197,55 +224,73 @@ class GPSDClient { } void process_data_navsat(struct gps_data_t* p) { - NavSatFixPtr fix(new NavSatFix); + NavSatFix fix; /* TODO: Support SBAS and other GBAS. */ - if (use_gps_time && !std::isnan(p->fix.time)) - fix->header.stamp = ros::Time(p->fix.time); - else - fix->header.stamp = ros::Time::now(); +#if GPSD_API_MAJOR_VERSION >= 9 + if (use_gps_time && (p->online.tv_sec || p->online.tv_nsec)) { + fix.header.stamp = ros::Time(p->fix.time.tv_sec, p->fix.time.tv_nsec); +#else + if (use_gps_time && !std::isnan(p->fix.time)) { + fix.header.stamp = ros::Time(p->fix.time); +#endif + } + else { + fix.header.stamp = ros::Time::now(); + } - fix->header.frame_id = frame_id; + fix.header.frame_id = frame_id; /* gpsmm pollutes the global namespace with STATUS_, * so we need to use the ROS message's integer values * for status.status */ +#if GPSD_API_MAJOR_VERSION >= 10 + switch (p->fix.status) { +#else switch (p->status) { +#endif +#if GPSD_API_MAJOR_VERSION >= 12 + case STATUS_GPS: +#else case STATUS_NO_FIX: - fix->status.status = -1; // NavSatStatus::STATUS_NO_FIX; - break; - case STATUS_FIX: - fix->status.status = 0; // NavSatStatus::STATUS_FIX; +#endif + fix.status.status = 0; // NavSatStatus::STATUS_FIX or NavSatStatus::STATUS_GPS; break; -// STATUS_DGPS_FIX was removed in API version 6 but re-added afterward -#if GPSD_API_MAJOR_VERSION != 6 +// STATUS_DGPS_FIX was removed in API version 6 but re-added afterward and next renamed since the version 12 +#if GPSD_API_MAJOR_VERSION != 6 +#if GPSD_API_MAJOR_VERSION >= 12 + case STATUS_DGPS: +#else case STATUS_DGPS_FIX: - fix->status.status = 2; // NavSatStatus::STATUS_GBAS_FIX; +#endif + fix.status.status = 2; // NavSatStatus::STATUS_GBAS_FIX; break; #endif } - fix->status.service = NavSatStatus::SERVICE_GPS; + fix.status.service = NavSatStatus::SERVICE_GPS; - fix->latitude = p->fix.latitude; - fix->longitude = p->fix.longitude; - fix->altitude = p->fix.altitude; + fix.latitude = p->fix.latitude; + fix.longitude = p->fix.longitude; + fix.altitude = p->fix.altitude; /* gpsd reports status=OK even when there is no current fix, * as long as there has been a fix previously. Throw out these * fake results, which have NaN variance */ if (std::isnan(p->fix.epx) && check_fix_by_variance) { + ROS_DEBUG_THROTTLE(1, + "GPS status was reported as OK, but variance was invalid"); return; } - fix->position_covariance[0] = p->fix.epx; - fix->position_covariance[4] = p->fix.epy; - fix->position_covariance[8] = p->fix.epv; + fix.position_covariance[0] = p->fix.epx; + fix.position_covariance[4] = p->fix.epy; + fix.position_covariance[8] = p->fix.epv; - fix->position_covariance_type = NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; + fix.position_covariance_type = NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; navsat_fix_pub.publish(fix); }