From d33aa6a1e82f8e038911954cbc1aa16de1012dbc Mon Sep 17 00:00:00 2001 From: Brian Cochran Date: Thu, 28 Apr 2022 16:54:35 -0400 Subject: [PATCH 01/17] add support for larger spartn corrections messages --- ublox_gps/include/ublox_gps/async_worker.h | 2 +- ublox_gps/include/ublox_gps/gps.h | 5 +++++ ublox_gps/include/ublox_gps/node.h | 1 + ublox_gps/src/gps.cpp | 7 +++++-- ublox_gps/src/node.cpp | 5 +++++ 5 files changed, 17 insertions(+), 3 deletions(-) diff --git a/ublox_gps/include/ublox_gps/async_worker.h b/ublox_gps/include/ublox_gps/async_worker.h index c41e74d1..5e6a2b70 100644 --- a/ublox_gps/include/ublox_gps/async_worker.h +++ b/ublox_gps/include/ublox_gps/async_worker.h @@ -61,7 +61,7 @@ class AsyncWorker : public Worker { */ AsyncWorker(boost::shared_ptr stream, boost::shared_ptr io_service, - std::size_t buffer_size = 8192); + std::size_t buffer_size = 16384); virtual ~AsyncWorker(); /** diff --git a/ublox_gps/include/ublox_gps/gps.h b/ublox_gps/include/ublox_gps/gps.h index 477e6ec1..36c8d51b 100644 --- a/ublox_gps/include/ublox_gps/gps.h +++ b/ublox_gps/include/ublox_gps/gps.h @@ -120,6 +120,11 @@ class Gps { */ bool sendRtcm(const std::vector &message); + /** + * @brief Send SPARTN correction message. + */ + bool sendSpartn(const std::vector &message); + /** * @brief Closes the I/O port, and initiates save on shutdown procedure * if enabled. diff --git a/ublox_gps/include/ublox_gps/node.h b/ublox_gps/include/ublox_gps/node.h index 2bb05691..5c60498b 100644 --- a/ublox_gps/include/ublox_gps/node.h +++ b/ublox_gps/include/ublox_gps/node.h @@ -50,6 +50,7 @@ #include #include #include +#include // Other U-Blox package includes #include // Ublox GPS includes diff --git a/ublox_gps/src/gps.cpp b/ublox_gps/src/gps.cpp index 02711173..593247e5 100644 --- a/ublox_gps/src/gps.cpp +++ b/ublox_gps/src/gps.cpp @@ -533,8 +533,11 @@ bool Gps::setUseAdr(bool enable, float protocol_version) { } bool Gps::sendRtcm(const std::vector& rtcm){ - worker_->send(rtcm.data(), rtcm.size()); - return true; + return worker_->send(rtcm.data(), rtcm.size()); +} + +bool Gps::sendSpartn(const std::vector& message){ + return worker_->send(message.data(), message.size()); } bool Gps::poll(uint8_t class_id, uint8_t message_id, diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 61642b33..f2451759 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -1813,10 +1813,15 @@ void rtcmCallback(const rtcm_msgs::Message::ConstPtr &msg) { gps.sendRtcm(msg->message); } +void spartnCallback(const std_msgs::UInt8MultiArray::ConstPtr &msg) { + gps.sendSpartn(msg->data); +} + int main(int argc, char** argv) { ros::init(argc, argv, "ublox_gps"); nh.reset(new ros::NodeHandle("~")); ros::Subscriber subRtcm = nh->subscribe("/rtcm", 10, rtcmCallback); + ros::Subscriber subSpartn = nh->subscribe("/spartn", 10, spartnCallback); nh->param("debug", ublox_gps::debug, 1); if(ublox_gps::debug) { if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, From 549fe26342eb7ab6753fb143f406933786573f4a Mon Sep 17 00:00:00 2001 From: Kristian Kabbabe Date: Mon, 2 May 2022 12:29:59 -0400 Subject: [PATCH 02/17] Debianize Package 1 --- ublox_gps/debian/changelog | 268 ++++++++++++++++++++++ ublox_gps/debian/compat | 1 + ublox_gps/debian/control | 12 + ublox_gps/debian/rules | 61 +++++ ublox_gps/debian/source/format | 1 + ublox_gps/debian/source/options | 5 + ublox_msg_filters/debian/changelog | 13 ++ ublox_msg_filters/debian/compat | 1 + ublox_msg_filters/debian/control | 12 + ublox_msg_filters/debian/rules | 61 +++++ ublox_msg_filters/debian/source/format | 1 + ublox_msg_filters/debian/source/options | 5 + ublox_msgs/debian/changelog | 158 +++++++++++++ ublox_msgs/debian/compat | 1 + ublox_msgs/debian/control | 12 + ublox_msgs/debian/rules | 61 +++++ ublox_msgs/debian/source/format | 1 + ublox_msgs/debian/source/options | 5 + ublox_serialization/debian/changelog | 112 +++++++++ ublox_serialization/debian/compat | 1 + ublox_serialization/debian/control | 12 + ublox_serialization/debian/rules | 61 +++++ ublox_serialization/debian/source/format | 1 + ublox_serialization/debian/source/options | 5 + 24 files changed, 871 insertions(+) create mode 100644 ublox_gps/debian/changelog create mode 100644 ublox_gps/debian/compat create mode 100644 ublox_gps/debian/control create mode 100755 ublox_gps/debian/rules create mode 100644 ublox_gps/debian/source/format create mode 100644 ublox_gps/debian/source/options create mode 100644 ublox_msg_filters/debian/changelog create mode 100644 ublox_msg_filters/debian/compat create mode 100644 ublox_msg_filters/debian/control create mode 100755 ublox_msg_filters/debian/rules create mode 100644 ublox_msg_filters/debian/source/format create mode 100644 ublox_msg_filters/debian/source/options create mode 100644 ublox_msgs/debian/changelog create mode 100644 ublox_msgs/debian/compat create mode 100644 ublox_msgs/debian/control create mode 100755 ublox_msgs/debian/rules create mode 100644 ublox_msgs/debian/source/format create mode 100644 ublox_msgs/debian/source/options create mode 100644 ublox_serialization/debian/changelog create mode 100644 ublox_serialization/debian/compat create mode 100644 ublox_serialization/debian/control create mode 100755 ublox_serialization/debian/rules create mode 100644 ublox_serialization/debian/source/format create mode 100644 ublox_serialization/debian/source/options diff --git a/ublox_gps/debian/changelog b/ublox_gps/debian/changelog new file mode 100644 index 00000000..0c96a3e7 --- /dev/null +++ b/ublox_gps/debian/changelog @@ -0,0 +1,268 @@ +ros-noetic-ublox-gps (1.5.0-greenzie-focal1) focal; urgency=medium + + * Debianize + + -- Kristian Kabbabe Mon, 02 May 2022 12:25:53 -0400 + +ros-noetic-ublox-gps (1.5.0-0focal) focal; urgency=high + + * Add rtcm_msgs dependency + * Add RTK support via rtcm + * GPS coordinate precision corrected for high accuracy. + * Diagonstics for Differential GNSS updated. + * Add a new parameter to set the search path for the param_file_name. Add a default to the node_name parameter. + * Add zed-f9p configuration + * Contributors: Balamurugan Kandan, Chris Iverach-Brereton, Igor + + -- Gareth Cross Fri, 15 Oct 2021 00:00:00 -0000 + +ros-noetic-ublox-gps (1.4.1-0focal) focal; urgency=high + + * fix signs in sensor_msgs::Imu output + * Contributors: Raphael Riebl + + -- Gareth Cross Thu, 04 Jun 2020 00:00:00 -0000 + +ros-noetic-ublox-gps (1.4.0-0focal) focal; urgency=high + + * Bump CMake minimum version to 3.0.2 + * Move variables from .h to .cpp to solve linking issues + * added support for protocol version >= 18 + * Contributors: Firat Kasmis, Gonçalo Pereira + + -- Gareth Cross Thu, 28 May 2020 00:00:00 -0000 + +ros-noetic-ublox-gps (1.3.1-0focal) focal; urgency=high + + * Fix unit in covariance calculation + * Contributors: Ferry Schoenmakers + + -- Gareth Cross Thu, 12 Mar 2020 00:00:00 -0000 + +ros-noetic-ublox-gps (1.3.0-0focal) focal; urgency=high + + * Fix heading output to comply with REP-103 + When not reporting valid heading, overwrite covariance with big number (0 otherwise) + * CfgNAV5: add dynamic model bike + * Contributors: Ferry Schoenmakers, Raphael Riebl + + -- Gareth Cross Fri, 10 Jan 2020 00:00:00 -0000 + +ros-noetic-ublox-gps (1.2.0-0focal) focal; urgency=high + + * Add support for ZED-F9P new RELPOSNED message and provide heading output + Fix whitespacing... + Add RELPOSNED9 message to compile + * Fix for corrupted diagnostics messages + Before the diagnostic structs were copied, but the pointers in FrequencyStatusParams still pointed to the old/freed objects. + * Show TMODE3 diagnostics OK if disabled + Since there is no default for TMODE3 this is a deliberate choice + * added simple (remote) logger node for raw data stream logging + * updated raw data stream logging + + moved all global/node functions to new class RawDataStreamPa + (raw_data_pa .h/.c) + + changed messagetype to uint8-multiarray + (string can not handle non-characters) + * fix #52 + * FIX: overflow bug when the nano field of the NavPVT message (which is signed and can be negative) is assigned to the nsec value of a ros time stamp (which is unsigned) + * deactivated config checks for base parts, if config_on_startup is false + * Added flag config_on_startup to deactivate configuration of ublox. + * fixes to raw data stream + + moving write_callback_ before the read_callback_, to avoid buffer copying + (write_callback_ == publishing ros messages and writing to file) + + publishing empty ros message during config phase to force instantiation + of publisher + * renamed new topic and internal variables for raw data stream + + from raw_data_xxx to raw_data_stream_xxx + + this is to avoid confusion with the RawDataProduct + * updated debug message for measurement rate + (added "hz" and "cycles" as units) + * TUC-ProAut: added raw data output + (publishing ros messages and storing to file) + * boost::posix_time::seconds constructor requires integer argument + * Add TIM product and M8U functionality as well as the TIM-TM2 message (#27 ) + * Initialize set_usb_ to false by default + * Set serial port to raw mode, needed for Boost versions < 1.66.0 + * Minor fixes for very old devices + * Fix potential segfault when printing Inf messages + The Inf message strings are not null terminated, so we need to construct + the string of the correct size from the vector of bytes instead of just + printing using %s. + * In AsyncWorker::doClose(), close the stream instead of just cancelling operations + * Cleanup + modernize to make compatible with C++11 + * Fix compilation with newer GCC and Boost + As of now, doesn't compile with C++11 or later. + * added clear params arg + * updated config files + * added save and load configuration parameters and functions. changed how GNSS is configured & reset. + * added raw data product class and structs for frequency diagnostics + * Contributors: Chao Qu, Evan Flynn, Ferry Schoenmakers, Kartik Mohta, Michael Stoll, Peter Weissig, Stewart Worrall, Tim Clephas, Veronica Lane + + -- Gareth Cross Tue, 19 Nov 2019 00:00:00 -0000 + +ros-noetic-ublox-gps (1.1.2-0focal) focal; urgency=high + + * README and package xml updates + * Fixed bug with enabling INF messages. Changed how messages which update fix are enabled and changed name of subscribe param namespace to publish. + * added USB Cfg PRT parameters and configuration + * Changed how I/O is initialized so that u-blox node parses the device parameter, and then calls either initializeSerial or initializeTcp in the GPS class with the appropriate parameters. Also cleaned up doxygen comments + * Added doxygen comments and made minor cleanup changes. + * Added doxygen comments + * Fixed bug with ARP Position HP conversion, which was multiplied by the wrong conversion factor. The ARP Position HP parameter is now an int8 vector instead of a float vector. Also added a getRosInt method in the node to get int8 and int16 params and changed the name of getRosParam to getRosUint. + * removing unnecessary include + * Changed how ACKs are handled. They are now handled through callback functions and are included in the CallbackHandlers. + * Created a CallbackHandlers class and migrated callbacks functionality from Gps class to the CallbackHandlers class + * Node can now save flash memory on shutdown and clear flash memory based on user params + * BUG FIX: Fix status only uses NavPVT time if the time is valid, otherwise it uses ros time. This prevents invalid times. + * added respawn params to launch file + * moved getRosParam template functions into node.h and used checkRange function for the getRosParam functions + * In config files, changed reset mode since it seems to work better + * Changed how unsigned int parameters are handled. + * Added NMEA flag params for firmware version 6 and updated readme to include NMEA params. + * Renamed cfg_gnss param namespace to gnss. Fixed bug with NMEA configuration for compat variable. Added sample config file for NMEA. + * added comments + * After resetting the device when re-configuring the GNSS, the node shuts down & must be relaunched since device address may change & serial port resets. + * Made ACK atomic since it is accessed by 2 threads (the main node & the i/o callback) + * BUG FIXES: fixed bug with waiting for acknowledgements, which wasn't timing out. Fixed bug with CfgGNSS which wasn't properly verifying the current GPS config to see if it was correct. Also added NMEA configuration functions + * debug variable is no longer static so that ublox node can set it from ROS params + * Removed ublox_version param, value is now determined by parsing MonVER. Changed name of UbloxInterface to ComponentInterface for clarity. + * Additional changes to parameters + * Moved most parameters into namespaces + * Cleaned up how parameters are check and moved the parameter parsing functions from the gps namespace to the node namespace since the node handles parameter checks. Also added CfgDAT capabilities, if dat/set param is set. + * updates to sample config files + * Change rtcm_rate parameter to a vector instead of a scalar, now each RTCM id can be set to a different rate. + * BUG FIX: Fix diagnostics num sv was displaying incorrectly. For firmware versions >=7, the flags are now compared to the constants from NavPVT not NavSOL. Also cleaned up how the diagnostics are displayed & included units. Added Carrier Phase diagnostics for HPG rovers. + * fixed bug with file path in ublox_device.launch and updated README to include information on launch files and subscribing/configuring new messages + * Contributors: Veronica Lane + + -- Gareth Cross Wed, 02 Aug 2017 00:00:00 -0000 + +ros-noetic-ublox-gps (1.1.0-0focal) focal; urgency=high + + * Updated package xmls with new version number and corrected my email address. Also updated readme to include information about new version plus new parameter + * Updated sample config files + * Added Cfg RST message declaration and reset function. For Firmware 8, after reconfiguring the GNSS, a cold restart is initiated. + * node now configures INF messages + * Added constants for HPG Rover Diagnostic updater. Cleaned up GPS class: made method and parameter names consistent, reordered methods for clarity, and privatized some methods. + * Added NavPVT7 message since NavPVT message is a different length for firmware version 7. UbloxFirmware7Plus class now uses a template function to update diagnostics from NavPVT messages and to publish fix messages from NavPVT messages. + * Code cleanup - clarified a function name + comments + * Implemented interface for ADR/UDR messages. Added unimplemented skeleton interface for FTS messages. Added warning message if device type was not parsed correctly from MonVER. + * Cleaned up formatting + modified debug/info statements + * Changed debug statements so that they print to ROS DEBUG console. DEBUG log level is set in main node based on value of debug ros param. + * Modified Cfg GNSS for Firmware version 7, so it configures SBAS and QZSS if supported by the device + * changed receive message error print statements to only print in debug mode + * cleaned up how the tmode state was tracked for HPG reference stations. For ublox >=8, GNSS is now only configured if the current configuration is different from the desired configuration. This prevents the need for a hard-reset and prevents survey-in mode from resetting on HPG devices with the correct configuration + * I/O initialization has been entirely migrated to the GPS class, previously it was handled in both the node and GPS class. Split the HPG class into two classes, one for the REF station and one for the rover since the configuration & params did not intersect at all. + * BUG FIX: baudrate config, serial ASIO baudrate now set correctly + * Cleaned up debug print statements + code cleanup + * Added print functions for INF messages and subscribers for new MON messages + * Added NavSAT message and moved subscribers for messages deprecated in version 8 to version specific subscribe methods + * Added a UbloxInterface class. UbloxNode and Ublox firmware and hardware specific classes implement the interface. Ublox Node contains pointers to the firmware and hardware classes and calls their functions during configuration. + Added a skeleton class for UbloxTim which subscribes to RawX and SFRBX messages, but has unimplemented configuration and getRosParams methods + * Changed UbloxNxNode class, ublox firmware version classes with version specific methods now inherit from UbloxFirmware. Hardware specific classes inherit from UbloxHardware. UbloxNode contains instances of each and calls the appropriate functions. + * Made NodeHandle a global variable in ublox_node namespace, publish is no longer a member function. Also took out additional node handles that were created to get parameters and just used the global node handle + * BUG FIX Firmware Version 6: nav status variable was never updated, using information from nav sol instead. CODE CLEANUP: added trailing underscores to a few class member variables. Removed * 3 multiplier for covariance in version 6. Added a diagnostic function for RTCM (currently not being used, will incorporate later) + * BUG FIX: For ublox 6 changed publisher of NavPOSLLH, NavVELNED, and NavSOL to call the custom method and not the template function. Also removed NavPOSLLH, NavVELNED, and NavSOL publishers from ublox 7 & 8 since NavPVT should be used. BUG FIX: Removed hardcoded value for NumTrackChs for CfgGNSS. CODE CLEANUP: added constants for hardcoded values + additional comments. + * For High Precision GNSS: Changed the way TMODE3 & RTCM messages are configured. If in survey-in mode, it first configures the device to survey-in, then when the survey is complete enables the RTCM messages. + * Fixed bug in Wait for ACK, it now checks that the ACK is for the expected class id and message id, also changed a few debug and error messages. + * Added Error message for ASIO read read errors and fixed a comment in cfg rate + * Includes BUG FIX (keep reading). Added Ublox messages (and subscribers or configuration methods + params) for High Precision GNSS devices: CfgDGNSS, NavRELPOSNED, NavSVIN. Also added subscriber & message for RxmRTCM. Changed MonVER processing, it now determines the protocol version, hardware type (e.g. SPG/HPG), and supported GNSS (e.g. Glonass, SBAS). SBAS can now be disabled on SBAS supported devices (previously SBAS settings were ignored if enable_sbas was false to prevent crashes, now it checks the MonVER message before trying to configure SBAS. + * Removed commented out lines which were unnecessary and added error message in async worker for read errors from asio + * Contributors: Veronica Lane + + -- Gareth Cross Mon, 17 Jul 2017 00:00:00 -0000 + +ros-noetic-ublox-gps (1.0.0-0focal) focal; urgency=high + + * added myself as maintainer to package xmls and updated version numbers of modified packages. + * Modified example launch file to include params, also added example launch which loads paramaters from yaml file + * more code cleanup + * Code cleanup of node + * Made a node class structure. An abstract class represents nodes for all firmware versions. Version nodes inherit from this node and implement version specific functions. + * add ros console include so ros error message would print + * Moved callback class functions from gps files to callback.h + * Added read lock to async worker. Read + write buffers are now lockedduring operations + * Fixed Thread safety issues with async worker. Now uses MRSW lock and each function which makes changes to shared variables acquires the lock + * BUG FIX: fixed issues in gps & node that caused run time crashes. FrequencyStatusParam arguments were in the wrong order. Reverted to old initialize method which incremently set the serial baudrate. + * added constants for hard-coded values in gps class + * Baud rate and in/out protocol mask are now configurable through params and are no longer hard coded. + * Removed hardcoded configuration values and added constants and params for these values. Fixed MonVER print warning issue. Added RTCM config function. Removed FixMode & DynamicMode enums and used constants from messages. Changed setBaudrate name to configUart1 since it was configuring all params. If enable SBAS is set to false, does not call enable SBAS (need to change this so that it calls if SBAS is available) to prevent errors for devices without SBAS. Changed std::cout statements to ROS_INFO. + * Formatting of copyright so it's <80 char and changed std::cout in Async worker to ROS_INFO messages + * Update CfgGNSS message and serialization which now publishes and receives blocks and reads and configures all GNSS settings at once. Updated MonVER message and serialization, MonVER settings are displayed during initialization, including extension chars. Changed various std::cout messages to ROS_INFO and ROS_ERROR messages. + * Updated AID, RXM, and NAV messages to ublox 8 protocol. Added RxmSFRBX and RxmRAWX messages. Also did a 2nd pass on CFG messages for ublox 8 update. Need to serialize SFRBX. + * forgot to add new files in last commit + * Publishes Fix and Fix velocity from Nav PVT messages. Fix time stamps are from Nav PVT time instead of ros time now + * Publishes fix from Nav PVT info instead of Nav Pos LLH info. No longer compatible with firmware <=6. Now uses template publish function for most messages. + * Added Nav PVT message for protocol 8 and added publisher for ECEF messages in node. + * In C++11 shared_ptr has an explicit bool conversion + * Contributors: Kartik Mohta, Veronica Lane + + -- Gareth Cross Fri, 23 Jun 2017 00:00:00 -0000 + +ros-noetic-ublox-gps (0.0.5-0focal) focal; urgency=high + + * Various small changes + 1. package.xml use format 2 + 2. change some default values in launch files and node + 3. update readme + * clang format + * Contributors: Chao Qu + + -- Gareth Cross Sat, 06 Aug 2016 00:00:00 -0000 + +ros-noetic-ublox-gps (0.0.4-0focal) focal; urgency=high + + * Update version number to reflect merge. + * Add install targets + * Reverted default in launch file + * Contributors: Gareth Cross, Kartik Mohta + + -- Gareth Cross Mon, 08 Dec 2014 00:00:00 -0000 + +ros-noetic-ublox-gps (0.0.3-0focal) focal; urgency=high + + * Updated readme to reflect changes + * Added hacky ublox_version parameter to handle current limitations in driver structure + * Added MonVER, cleaned up make files a bit + * Added warning for ppp + * Added method to enable PPP + * Added settings for beidou and glonass + * Added option to run in gps only mode + * Changed param in roslaunch + * Contributors: Gareth Cross + + -- Gareth Cross Sat, 18 Oct 2014 00:00:00 -0000 + +ros-noetic-ublox-gps (0.0.2-0focal) focal; urgency=high + + * Set better default for dr_limit in launch file + * Changed launch file to match readme + * Changed meas_rate to rate + * fix frame_id default + * add an option to specify node nanme + * Update ublox_gps.launch + * Update ublox_gps.launch + * Change to node + * Fixed erroneous max delay in diagnostic settings + * Removed unused option form launch file and readme + * Added diagnostic support + * Added options to ublox node, see README for details on changes + * Contributors: Chao Qu, Gareth Cross + + -- Gareth Cross Fri, 03 Oct 2014 00:00:00 -0000 + +ros-noetic-ublox-gps (0.0.1-0focal) focal; urgency=high + + * Making fixes for second deployment + * Contributors: Gareth Cross + + -- Gareth Cross Fri, 15 Aug 2014 00:00:00 -0000 + +ros-noetic-ublox-gps (0.0.0-0focal) focal; urgency=high + + * ublox: first commit + * Contributors: Chao Qu + + -- Gareth Cross Mon, 23 Jun 2014 00:00:00 -0000 + + diff --git a/ublox_gps/debian/compat b/ublox_gps/debian/compat new file mode 100644 index 00000000..ec635144 --- /dev/null +++ b/ublox_gps/debian/compat @@ -0,0 +1 @@ +9 diff --git a/ublox_gps/debian/control b/ublox_gps/debian/control new file mode 100644 index 00000000..7f7d35a8 --- /dev/null +++ b/ublox_gps/debian/control @@ -0,0 +1,12 @@ +Source: ros-noetic-ublox-gps +Section: misc +Priority: optional +Maintainer: Gareth Cross +Build-Depends: debhelper (>= 9.0.0), ros-noetic-catkin, ros-noetic-diagnostic-updater, ros-noetic-roscpp, ros-noetic-roscpp-serialization, ros-noetic-rtcm-msgs, ros-noetic-tf, ros-noetic-ublox-msgs, ros-noetic-ublox-serialization +Homepage: http://ros.org/wiki/ublox +Standards-Version: 3.9.2 + +Package: ros-noetic-ublox-gps +Architecture: any +Depends: ${shlibs:Depends}, ${misc:Depends}, ros-noetic-diagnostic-updater, ros-noetic-roscpp, ros-noetic-roscpp-serialization, ros-noetic-rtcm-msgs, ros-noetic-tf, ros-noetic-ublox-msgs, ros-noetic-ublox-serialization +Description: Driver for u-blox GPS devices. diff --git a/ublox_gps/debian/rules b/ublox_gps/debian/rules new file mode 100755 index 00000000..86926ab0 --- /dev/null +++ b/ublox_gps/debian/rules @@ -0,0 +1,61 @@ +#!/usr/bin/make -f +# -*- makefile -*- +# Sample debian/rules that uses debhelper. +# This file was originally written by Joey Hess and Craig Small. +# As a special exception, when this file is copied by dh-make into a +# dh-make output file, you may use that output file without restriction. +# This special exception was added by Craig Small in version 0.37 of dh-make. + +# Uncomment this to turn on verbose mode. +export DH_VERBOSE=1 +# TODO: remove the LDFLAGS override. It's here to avoid esoteric problems +# of this sort: +# https://code.ros.org/trac/ros/ticket/2977 +# https://code.ros.org/trac/ros/ticket/3842 +export LDFLAGS= +export PKG_CONFIG_PATH=/opt/ros/noetic/lib/pkgconfig +# Explicitly enable -DNDEBUG, see: +# https://github.com/ros-infrastructure/bloom/issues/327 +export DEB_CXXFLAGS_MAINT_APPEND=-DNDEBUG + +%: + dh $@ -v --buildsystem=cmake + +override_dh_auto_configure: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_configure -- \ + -DCATKIN_BUILD_BINARY_PACKAGE="1" \ + -DCMAKE_INSTALL_PREFIX="/opt/ros/noetic" \ + -DCMAKE_PREFIX_PATH="/opt/ros/noetic" + +override_dh_auto_build: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_build + +override_dh_auto_test: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + echo -- Running tests. Even if one of them fails the build is not canceled. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_test || true + +override_dh_shlibdeps: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_shlibdeps -l$(CURDIR)/debian/ros-noetic-ublox-gps//opt/ros/noetic/lib/ + +override_dh_auto_install: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_install diff --git a/ublox_gps/debian/source/format b/ublox_gps/debian/source/format new file mode 100644 index 00000000..163aaf8d --- /dev/null +++ b/ublox_gps/debian/source/format @@ -0,0 +1 @@ +3.0 (quilt) diff --git a/ublox_gps/debian/source/options b/ublox_gps/debian/source/options new file mode 100644 index 00000000..8bc9182a --- /dev/null +++ b/ublox_gps/debian/source/options @@ -0,0 +1,5 @@ +# Automatically add upstream changes to the quilt overlay. +# http://manpages.ubuntu.com/manpages/trusty/man1/dpkg-source.1.html +# This supports reusing the orig.tar.gz for debian increments. +auto-commit + diff --git a/ublox_msg_filters/debian/changelog b/ublox_msg_filters/debian/changelog new file mode 100644 index 00000000..897c098b --- /dev/null +++ b/ublox_msg_filters/debian/changelog @@ -0,0 +1,13 @@ +ros-noetic-ublox-msg-filters (1.5.0-greenzie-focal1) focal; urgency=medium + + * Debianize + + -- Kristian Kabbabe Mon, 02 May 2022 12:26:01 -0400 + +ros-noetic-ublox-msg-filters (0.0.0-0focal) focal; urgency=high + + * Autogenerated, no changelog for this version found in CHANGELOG.rst. + + -- Kevin Hallenbeck Mon, 02 May 2022 16:23:25 -0000 + + diff --git a/ublox_msg_filters/debian/compat b/ublox_msg_filters/debian/compat new file mode 100644 index 00000000..ec635144 --- /dev/null +++ b/ublox_msg_filters/debian/compat @@ -0,0 +1 @@ +9 diff --git a/ublox_msg_filters/debian/control b/ublox_msg_filters/debian/control new file mode 100644 index 00000000..ecd214de --- /dev/null +++ b/ublox_msg_filters/debian/control @@ -0,0 +1,12 @@ +Source: ros-noetic-ublox-msg-filters +Section: misc +Priority: optional +Maintainer: Kevin Hallenbeck +Build-Depends: debhelper (>= 9.0.0), ros-noetic-catkin, ros-noetic-message-filters, ros-noetic-roscpp, ros-noetic-roslaunch, ros-noetic-rospy, ros-noetic-ublox-msgs +Homepage: http://ros.org/wiki/ublox +Standards-Version: 3.9.2 + +Package: ros-noetic-ublox-msg-filters +Architecture: any +Depends: ${shlibs:Depends}, ${misc:Depends}, ros-noetic-message-filters, ros-noetic-ublox-msgs +Description: Time synchronize multiple uBlox messages to get a single callback diff --git a/ublox_msg_filters/debian/rules b/ublox_msg_filters/debian/rules new file mode 100755 index 00000000..97377640 --- /dev/null +++ b/ublox_msg_filters/debian/rules @@ -0,0 +1,61 @@ +#!/usr/bin/make -f +# -*- makefile -*- +# Sample debian/rules that uses debhelper. +# This file was originally written by Joey Hess and Craig Small. +# As a special exception, when this file is copied by dh-make into a +# dh-make output file, you may use that output file without restriction. +# This special exception was added by Craig Small in version 0.37 of dh-make. + +# Uncomment this to turn on verbose mode. +export DH_VERBOSE=1 +# TODO: remove the LDFLAGS override. It's here to avoid esoteric problems +# of this sort: +# https://code.ros.org/trac/ros/ticket/2977 +# https://code.ros.org/trac/ros/ticket/3842 +export LDFLAGS= +export PKG_CONFIG_PATH=/opt/ros/noetic/lib/pkgconfig +# Explicitly enable -DNDEBUG, see: +# https://github.com/ros-infrastructure/bloom/issues/327 +export DEB_CXXFLAGS_MAINT_APPEND=-DNDEBUG + +%: + dh $@ -v --buildsystem=cmake + +override_dh_auto_configure: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_configure -- \ + -DCATKIN_BUILD_BINARY_PACKAGE="1" \ + -DCMAKE_INSTALL_PREFIX="/opt/ros/noetic" \ + -DCMAKE_PREFIX_PATH="/opt/ros/noetic" + +override_dh_auto_build: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_build + +override_dh_auto_test: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + echo -- Running tests. Even if one of them fails the build is not canceled. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_test || true + +override_dh_shlibdeps: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_shlibdeps -l$(CURDIR)/debian/ros-noetic-ublox-msg-filters//opt/ros/noetic/lib/ + +override_dh_auto_install: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_install diff --git a/ublox_msg_filters/debian/source/format b/ublox_msg_filters/debian/source/format new file mode 100644 index 00000000..163aaf8d --- /dev/null +++ b/ublox_msg_filters/debian/source/format @@ -0,0 +1 @@ +3.0 (quilt) diff --git a/ublox_msg_filters/debian/source/options b/ublox_msg_filters/debian/source/options new file mode 100644 index 00000000..8bc9182a --- /dev/null +++ b/ublox_msg_filters/debian/source/options @@ -0,0 +1,5 @@ +# Automatically add upstream changes to the quilt overlay. +# http://manpages.ubuntu.com/manpages/trusty/man1/dpkg-source.1.html +# This supports reusing the orig.tar.gz for debian increments. +auto-commit + diff --git a/ublox_msgs/debian/changelog b/ublox_msgs/debian/changelog new file mode 100644 index 00000000..70c64dcb --- /dev/null +++ b/ublox_msgs/debian/changelog @@ -0,0 +1,158 @@ +ros-noetic-ublox-msgs (1.5.0-greenzie-focal1) focal; urgency=medium + + * Debianize + + -- Kristian Kabbabe Mon, 02 May 2022 12:26:07 -0400 + +ros-noetic-ublox-msgs (1.5.0-0focal) focal; urgency=high + + * Fixed reconfig value in UBX_CFG_ANT. + * Contributors: ito-san + + -- Veronica Lane Fri, 15 Oct 2021 00:00:00 -0000 + +ros-noetic-ublox-msgs (1.4.1-0focal) focal; urgency=high + + + + -- Veronica Lane Thu, 04 Jun 2020 00:00:00 -0000 + +ros-noetic-ublox-msgs (1.4.0-0focal) focal; urgency=high + + * Bump CMake minimum version to 3.0.2 + * Contributors: Gonçalo Pereira + + -- Veronica Lane Thu, 28 May 2020 00:00:00 -0000 + +ros-noetic-ublox-msgs (1.3.1-0focal) focal; urgency=high + + + + -- Veronica Lane Thu, 12 Mar 2020 00:00:00 -0000 + +ros-noetic-ublox-msgs (1.3.0-0focal) focal; urgency=high + + * Fix heading accuracy unit in comments + * CfgNAV5: add dynamic model bike + * Contributors: Ferry Schoenmakers, Raphael Riebl + + -- Veronica Lane Fri, 10 Jan 2020 00:00:00 -0000 + +ros-noetic-ublox-msgs (1.2.0-0focal) focal; urgency=high + + * Add support for ZED-F9P new RELPOSNED message and provide heading output + Fix whitespacing... + Add RELPOSNED9 message to compile + * Add TIM product and M8U functionality as well as the TIM-TM2 message (#27 ) + * comment cleanup + * added MonHW message for Firmware 6. + * fixed incorrect message id + * Contributors: Evan Flynn, Ferry Schoenmakers, Veronica Lane + + -- Veronica Lane Tue, 19 Nov 2019 00:00:00 -0000 + +ros-noetic-ublox-msgs (1.1.2-0focal) focal; urgency=high + + * README and package xml updates + * added USB Cfg PRT parameters and configuration + * Added doxygen comments and made minor cleanup changes. + * Added typedefs, cleaned up formatting, and added doxygen comments. Removed try-catch statements for serializing optional blocks and used the message count to determine the size of the optional block. + * Added doxygen comments + * Changed how ACKs are handled. They are now handled through callback functions and are included in the CallbackHandlers. + * Added UPD messages. + * Added Cfg NMEA messages for firmware 6 & 7, since the messages are a different length + * Added the CfgDAT message with custom serialization for get/set (read/write) since the messages are different lengths + * corrected message comment which had incorrect units + * Added CfgNMEA message + * Added constants for NavSAT flags bitmask + * Contributors: Veronica Lane + + -- Veronica Lane Wed, 02 Aug 2017 00:00:00 -0000 + +ros-noetic-ublox-msgs (1.1.0-0focal) focal; urgency=high + + * Updated package xmls with new version number and corrected my email address. Also updated readme to include information about new version plus new parameter + * changed name of macro for clarity + * Added Cfg RST message declaration and reset function. For Firmware 8, after reconfiguring the GNSS, a cold restart is initiated. + * node now configures INF messages + * added CfgINF message + * added comment about deprecated setting + * Added NavPVT7 message since NavPVT message is a different length for firmware version 7. UbloxFirmware7Plus class now uses a template function to update diagnostics from NavPVT messages and to publish fix messages from NavPVT messages. + * Added NavATT message + * Added message declarations for newly included messages as well as serialization classes for messages with repeating blocks. + * Added all ESF messages. Added HNR cfg message and HNR PVT message. Added MGA GAL message. + * added constants for SBAS reserved and max tracking channels + * comments cleanup + * Added additional MON messages. Received INF messages are now printed to the ROS console. Also added constants and comments to serialization and a new macro so 1 message can have multiple class, message ID pairs. + * Added NavSAT message and moved subscribers for messages deprecated in version 8 to version specific subscribe methods + * Added Error message for ASIO read read errors and fixed a comment in cfg rate + * Includes BUG FIX (keep reading). Added Ublox messages (and subscribers or configuration methods + params) for High Precision GNSS devices: CfgDGNSS, NavRELPOSNED, NavSVIN. Also added subscriber & message for RxmRTCM. Changed MonVER processing, it now determines the protocol version, hardware type (e.g. SPG/HPG), and supported GNSS (e.g. Glonass, SBAS). SBAS can now be disabled on SBAS supported devices (previously SBAS settings were ignored if enable_sbas was false to prevent crashes, now it checks the MonVER message before trying to configure SBAS. + * Added CfgTMODE3 message for High Precision GNSS products + * Contributors: Veronica Lane + + -- Veronica Lane Mon, 17 Jul 2017 00:00:00 -0000 + +ros-noetic-ublox-msgs (1.0.0-0focal) focal; urgency=high + + * added myself as maintainer to package xmls and updated version numbers of modified packages. + * Baud rate and in/out protocol mask are now configurable through params and are no longer hard coded. + * Added constants for RTCM and ACK messages + * Message comment updates + * PRT message is now generic. Added constants for SPI, USB, and DDC configurations. + * Update CfgGNSS message and serialization which now publishes and receives blocks and reads and configures all GNSS settings at once. Updated MonVER message and serialization, MonVER settings are displayed during initialization, including extension chars. Changed various std::cout messages to ROS_INFO and ROS_ERROR messages. + * updated comments + * Serialization for RxmSFRBX and RxmRAWX + * Updated AID, RXM, and NAV messages to ublox 8 protocol. Added RxmSFRBX and RxmRAWX messages. Also did a 2nd pass on CFG messages for ublox 8 update. Need to serialize SFRBX. + * Fixed build errors with nav msgs and updated remaining cfg messages for firmware 8 + * updated Cfg Nav 5 and X5 messages for firmware version 8 + * comment cleanup for nav pvt msg + * Added Nav PVT message for protocol 8 and added publisher for ECEF messages in node. + * Fix value of GPS_TIME_ONLY_FIX constant + * Contributors: Kartik Mohta, Veronica Lane + + -- Veronica Lane Fri, 23 Jun 2017 00:00:00 -0000 + +ros-noetic-ublox-msgs (0.0.5-0focal) focal; urgency=high + + + + -- Veronica Lane Sat, 06 Aug 2016 00:00:00 -0000 + +ros-noetic-ublox-msgs (0.0.4-0focal) focal; urgency=high + + * Add install targets + * Contributors: Kartik Mohta + + -- Veronica Lane Mon, 08 Dec 2014 00:00:00 -0000 + +ros-noetic-ublox-msgs (0.0.3-0focal) focal; urgency=high + + * Added MonVER, cleaned up make files a bit + * Adde c++ stuff for NAVX5 message + * Added message for NAVX5 + * Added option to run in gps only mode + * Added message type for GNSS config + * Contributors: Gareth Cross + + -- Veronica Lane Sat, 18 Oct 2014 00:00:00 -0000 + +ros-noetic-ublox-msgs (0.0.2-0focal) focal; urgency=high + + + + -- Veronica Lane Fri, 03 Oct 2014 00:00:00 -0000 + +ros-noetic-ublox-msgs (0.0.1-0focal) focal; urgency=high + + + + -- Veronica Lane Fri, 15 Aug 2014 00:00:00 -0000 + +ros-noetic-ublox-msgs (0.0.0-0focal) focal; urgency=high + + * ublox: first commit + * Contributors: Chao Qu + + -- Veronica Lane Mon, 23 Jun 2014 00:00:00 -0000 + + diff --git a/ublox_msgs/debian/compat b/ublox_msgs/debian/compat new file mode 100644 index 00000000..ec635144 --- /dev/null +++ b/ublox_msgs/debian/compat @@ -0,0 +1 @@ +9 diff --git a/ublox_msgs/debian/control b/ublox_msgs/debian/control new file mode 100644 index 00000000..09742ad4 --- /dev/null +++ b/ublox_msgs/debian/control @@ -0,0 +1,12 @@ +Source: ros-noetic-ublox-msgs +Section: misc +Priority: optional +Maintainer: Veronica Lane +Build-Depends: debhelper (>= 9.0.0), ros-noetic-catkin, ros-noetic-message-generation, ros-noetic-sensor-msgs, ros-noetic-std-msgs, ros-noetic-ublox-serialization +Homepage: http://ros.org/wiki/ublox +Standards-Version: 3.9.2 + +Package: ros-noetic-ublox-msgs +Architecture: any +Depends: ${shlibs:Depends}, ${misc:Depends}, ros-noetic-message-runtime, ros-noetic-sensor-msgs, ros-noetic-std-msgs, ros-noetic-ublox-serialization +Description: ublox_msgs contains raw messages for u-blox GNSS devices. diff --git a/ublox_msgs/debian/rules b/ublox_msgs/debian/rules new file mode 100755 index 00000000..8e0bc174 --- /dev/null +++ b/ublox_msgs/debian/rules @@ -0,0 +1,61 @@ +#!/usr/bin/make -f +# -*- makefile -*- +# Sample debian/rules that uses debhelper. +# This file was originally written by Joey Hess and Craig Small. +# As a special exception, when this file is copied by dh-make into a +# dh-make output file, you may use that output file without restriction. +# This special exception was added by Craig Small in version 0.37 of dh-make. + +# Uncomment this to turn on verbose mode. +export DH_VERBOSE=1 +# TODO: remove the LDFLAGS override. It's here to avoid esoteric problems +# of this sort: +# https://code.ros.org/trac/ros/ticket/2977 +# https://code.ros.org/trac/ros/ticket/3842 +export LDFLAGS= +export PKG_CONFIG_PATH=/opt/ros/noetic/lib/pkgconfig +# Explicitly enable -DNDEBUG, see: +# https://github.com/ros-infrastructure/bloom/issues/327 +export DEB_CXXFLAGS_MAINT_APPEND=-DNDEBUG + +%: + dh $@ -v --buildsystem=cmake + +override_dh_auto_configure: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_configure -- \ + -DCATKIN_BUILD_BINARY_PACKAGE="1" \ + -DCMAKE_INSTALL_PREFIX="/opt/ros/noetic" \ + -DCMAKE_PREFIX_PATH="/opt/ros/noetic" + +override_dh_auto_build: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_build + +override_dh_auto_test: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + echo -- Running tests. Even if one of them fails the build is not canceled. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_test || true + +override_dh_shlibdeps: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_shlibdeps -l$(CURDIR)/debian/ros-noetic-ublox-msgs//opt/ros/noetic/lib/ + +override_dh_auto_install: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_install diff --git a/ublox_msgs/debian/source/format b/ublox_msgs/debian/source/format new file mode 100644 index 00000000..163aaf8d --- /dev/null +++ b/ublox_msgs/debian/source/format @@ -0,0 +1 @@ +3.0 (quilt) diff --git a/ublox_msgs/debian/source/options b/ublox_msgs/debian/source/options new file mode 100644 index 00000000..8bc9182a --- /dev/null +++ b/ublox_msgs/debian/source/options @@ -0,0 +1,5 @@ +# Automatically add upstream changes to the quilt overlay. +# http://manpages.ubuntu.com/manpages/trusty/man1/dpkg-source.1.html +# This supports reusing the orig.tar.gz for debian increments. +auto-commit + diff --git a/ublox_serialization/debian/changelog b/ublox_serialization/debian/changelog new file mode 100644 index 00000000..cf481aff --- /dev/null +++ b/ublox_serialization/debian/changelog @@ -0,0 +1,112 @@ +ros-noetic-ublox-serialization (1.5.0-greenzie-focal1) focal; urgency=medium + + * Debianize + + -- Kristian Kabbabe Mon, 02 May 2022 12:26:16 -0400 + +ros-noetic-ublox-serialization (1.5.0-0focal) focal; urgency=high + + + + -- Veronica Lane Fri, 15 Oct 2021 00:00:00 -0000 + +ros-noetic-ublox-serialization (1.4.1-0focal) focal; urgency=high + + + + -- Veronica Lane Thu, 04 Jun 2020 00:00:00 -0000 + +ros-noetic-ublox-serialization (1.4.0-0focal) focal; urgency=high + + * Bump CMake minimum version to 3.0.2 + * Contributors: Gonçalo Pereira + + -- Veronica Lane Thu, 28 May 2020 00:00:00 -0000 + +ros-noetic-ublox-serialization (1.3.1-0focal) focal; urgency=high + + + + -- Veronica Lane Thu, 12 Mar 2020 00:00:00 -0000 + +ros-noetic-ublox-serialization (1.3.0-0focal) focal; urgency=high + + + + -- Veronica Lane Fri, 10 Jan 2020 00:00:00 -0000 + +ros-noetic-ublox-serialization (1.2.0-0focal) focal; urgency=high + + + + -- Veronica Lane Tue, 19 Nov 2019 00:00:00 -0000 + +ros-noetic-ublox-serialization (1.1.2-0focal) focal; urgency=high + + * README and package xml updates + * Fixed bug with enabling INF messages. Changed how messages which update fix are enabled and changed name of subscribe param namespace to publish. + * Added doxygen comments and made minor cleanup changes. + * Added doxygen comments + * Contributors: Veronica Lane + + -- Veronica Lane Wed, 02 Aug 2017 00:00:00 -0000 + +ros-noetic-ublox-serialization (1.1.0-0focal) focal; urgency=high + + * Updated package xmls with new version number and corrected my email address. Also updated readme to include information about new version plus new parameter + * changed name of macro for clarity + * changed receive message error print statements to only print in debug mode + * Added additional MON messages. Received INF messages are now printed to the ROS console. Also added constants and comments to serialization and a new macro so 1 message can have multiple class, message ID pairs. + * Contributors: Veronica Lane + + -- Veronica Lane Mon, 17 Jul 2017 00:00:00 -0000 + +ros-noetic-ublox-serialization (1.0.0-0focal) focal; urgency=high + + * added myself as maintainer to package xmls and updated version numbers of modified packages. + * formatting + * Formatting of copyright so it's <80 char and changed std::cout in Async worker to ROS_INFO messages + * Update CfgGNSS message and serialization which now publishes and receives blocks and reads and configures all GNSS settings at once. Updated MonVER message and serialization, MonVER settings are displayed during initialization, including extension chars. Changed various std::cout messages to ROS_INFO and ROS_ERROR messages. + * Contributors: Veronica Lane + + -- Veronica Lane Fri, 23 Jun 2017 00:00:00 -0000 + +ros-noetic-ublox-serialization (0.0.5-0focal) focal; urgency=high + + + + -- Veronica Lane Sat, 06 Aug 2016 00:00:00 -0000 + +ros-noetic-ublox-serialization (0.0.4-0focal) focal; urgency=high + + * Add install targets + * Contributors: Kartik Mohta + + -- Veronica Lane Mon, 08 Dec 2014 00:00:00 -0000 + +ros-noetic-ublox-serialization (0.0.3-0focal) focal; urgency=high + + + + -- Veronica Lane Sat, 18 Oct 2014 00:00:00 -0000 + +ros-noetic-ublox-serialization (0.0.2-0focal) focal; urgency=high + + + + -- Veronica Lane Fri, 03 Oct 2014 00:00:00 -0000 + +ros-noetic-ublox-serialization (0.0.1-0focal) focal; urgency=high + + + + -- Veronica Lane Fri, 15 Aug 2014 00:00:00 -0000 + +ros-noetic-ublox-serialization (0.0.0-0focal) focal; urgency=high + + * ublox: first commit + * Contributors: Chao Qu + + -- Veronica Lane Mon, 23 Jun 2014 00:00:00 -0000 + + diff --git a/ublox_serialization/debian/compat b/ublox_serialization/debian/compat new file mode 100644 index 00000000..ec635144 --- /dev/null +++ b/ublox_serialization/debian/compat @@ -0,0 +1 @@ +9 diff --git a/ublox_serialization/debian/control b/ublox_serialization/debian/control new file mode 100644 index 00000000..600a77a5 --- /dev/null +++ b/ublox_serialization/debian/control @@ -0,0 +1,12 @@ +Source: ros-noetic-ublox-serialization +Section: misc +Priority: optional +Maintainer: Veronica Lane +Build-Depends: debhelper (>= 9.0.0), ros-noetic-catkin, ros-noetic-roscpp, ros-noetic-roscpp-serialization +Homepage: http://ros.org/wiki/ublox +Standards-Version: 3.9.2 + +Package: ros-noetic-ublox-serialization +Architecture: any +Depends: ${shlibs:Depends}, ${misc:Depends}, ros-noetic-roscpp, ros-noetic-roscpp-serialization +Description: ublox_serialization provides header files for serialization of ROS messages to and from u-blox message format. diff --git a/ublox_serialization/debian/rules b/ublox_serialization/debian/rules new file mode 100755 index 00000000..4c57e4f7 --- /dev/null +++ b/ublox_serialization/debian/rules @@ -0,0 +1,61 @@ +#!/usr/bin/make -f +# -*- makefile -*- +# Sample debian/rules that uses debhelper. +# This file was originally written by Joey Hess and Craig Small. +# As a special exception, when this file is copied by dh-make into a +# dh-make output file, you may use that output file without restriction. +# This special exception was added by Craig Small in version 0.37 of dh-make. + +# Uncomment this to turn on verbose mode. +export DH_VERBOSE=1 +# TODO: remove the LDFLAGS override. It's here to avoid esoteric problems +# of this sort: +# https://code.ros.org/trac/ros/ticket/2977 +# https://code.ros.org/trac/ros/ticket/3842 +export LDFLAGS= +export PKG_CONFIG_PATH=/opt/ros/noetic/lib/pkgconfig +# Explicitly enable -DNDEBUG, see: +# https://github.com/ros-infrastructure/bloom/issues/327 +export DEB_CXXFLAGS_MAINT_APPEND=-DNDEBUG + +%: + dh $@ -v --buildsystem=cmake + +override_dh_auto_configure: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_configure -- \ + -DCATKIN_BUILD_BINARY_PACKAGE="1" \ + -DCMAKE_INSTALL_PREFIX="/opt/ros/noetic" \ + -DCMAKE_PREFIX_PATH="/opt/ros/noetic" + +override_dh_auto_build: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_build + +override_dh_auto_test: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + echo -- Running tests. Even if one of them fails the build is not canceled. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_test || true + +override_dh_shlibdeps: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_shlibdeps -l$(CURDIR)/debian/ros-noetic-ublox-serialization//opt/ros/noetic/lib/ + +override_dh_auto_install: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_install diff --git a/ublox_serialization/debian/source/format b/ublox_serialization/debian/source/format new file mode 100644 index 00000000..163aaf8d --- /dev/null +++ b/ublox_serialization/debian/source/format @@ -0,0 +1 @@ +3.0 (quilt) diff --git a/ublox_serialization/debian/source/options b/ublox_serialization/debian/source/options new file mode 100644 index 00000000..8bc9182a --- /dev/null +++ b/ublox_serialization/debian/source/options @@ -0,0 +1,5 @@ +# Automatically add upstream changes to the quilt overlay. +# http://manpages.ubuntu.com/manpages/trusty/man1/dpkg-source.1.html +# This supports reusing the orig.tar.gz for debian increments. +auto-commit + From 1c220411e2d27532de1a8a2bb0cdebddaca5c89a Mon Sep 17 00:00:00 2001 From: Kristian Kabbabe Date: Mon, 2 May 2022 12:35:55 -0400 Subject: [PATCH 03/17] fixing format and constraining dependencies to specific version --- ublox_gps/debian/control | 20 ++++++++++++++++++-- ublox_msg_filters/debian/control | 13 +++++++++++-- ublox_msgs/debian/control | 14 ++++++++++++-- ublox_serialization/debian/control | 10 ++++++++-- 4 files changed, 49 insertions(+), 8 deletions(-) diff --git a/ublox_gps/debian/control b/ublox_gps/debian/control index 7f7d35a8..ed12bd94 100644 --- a/ublox_gps/debian/control +++ b/ublox_gps/debian/control @@ -2,11 +2,27 @@ Source: ros-noetic-ublox-gps Section: misc Priority: optional Maintainer: Gareth Cross -Build-Depends: debhelper (>= 9.0.0), ros-noetic-catkin, ros-noetic-diagnostic-updater, ros-noetic-roscpp, ros-noetic-roscpp-serialization, ros-noetic-rtcm-msgs, ros-noetic-tf, ros-noetic-ublox-msgs, ros-noetic-ublox-serialization +Build-Depends: debhelper (>= 9.0.0), + ros-noetic-catkin, + ros-noetic-diagnostic-updater, + ros-noetic-roscpp, + ros-noetic-roscpp-serialization, + ros-noetic-rtcm-msgs, + ros-noetic-tf, + ros-noetic-ublox-msgs (= ${binary:Version}), + ros-noetic-ublox-serialization (= ${binary:Version}) Homepage: http://ros.org/wiki/ublox Standards-Version: 3.9.2 Package: ros-noetic-ublox-gps Architecture: any -Depends: ${shlibs:Depends}, ${misc:Depends}, ros-noetic-diagnostic-updater, ros-noetic-roscpp, ros-noetic-roscpp-serialization, ros-noetic-rtcm-msgs, ros-noetic-tf, ros-noetic-ublox-msgs, ros-noetic-ublox-serialization +Depends: ${shlibs:Depends}, + ${misc:Depends}, + ros-noetic-diagnostic-updater, + ros-noetic-roscpp, + ros-noetic-roscpp-serialization, + ros-noetic-rtcm-msgs, + ros-noetic-tf, + ros-noetic-ublox-msgs (= ${binary:Version}), + ros-noetic-ublox-serialization (= ${binary:Version}) Description: Driver for u-blox GPS devices. diff --git a/ublox_msg_filters/debian/control b/ublox_msg_filters/debian/control index ecd214de..d4066650 100644 --- a/ublox_msg_filters/debian/control +++ b/ublox_msg_filters/debian/control @@ -2,11 +2,20 @@ Source: ros-noetic-ublox-msg-filters Section: misc Priority: optional Maintainer: Kevin Hallenbeck -Build-Depends: debhelper (>= 9.0.0), ros-noetic-catkin, ros-noetic-message-filters, ros-noetic-roscpp, ros-noetic-roslaunch, ros-noetic-rospy, ros-noetic-ublox-msgs +Build-Depends: debhelper (>= 9.0.0), + ros-noetic-catkin, + ros-noetic-message-filters, + ros-noetic-roscpp, + ros-noetic-roslaunch, + ros-noetic-rospy, + ros-noetic-ublox-msgs (= ${binary:Version}) Homepage: http://ros.org/wiki/ublox Standards-Version: 3.9.2 Package: ros-noetic-ublox-msg-filters Architecture: any -Depends: ${shlibs:Depends}, ${misc:Depends}, ros-noetic-message-filters, ros-noetic-ublox-msgs +Depends: ${shlibs:Depends}, + ${misc:Depends}, + ros-noetic-message-filters, + ros-noetic-ublox-msgs (= ${binary:Version}) Description: Time synchronize multiple uBlox messages to get a single callback diff --git a/ublox_msgs/debian/control b/ublox_msgs/debian/control index 09742ad4..76de3f26 100644 --- a/ublox_msgs/debian/control +++ b/ublox_msgs/debian/control @@ -2,11 +2,21 @@ Source: ros-noetic-ublox-msgs Section: misc Priority: optional Maintainer: Veronica Lane -Build-Depends: debhelper (>= 9.0.0), ros-noetic-catkin, ros-noetic-message-generation, ros-noetic-sensor-msgs, ros-noetic-std-msgs, ros-noetic-ublox-serialization +Build-Depends: debhelper (>= 9.0.0), + ros-noetic-catkin, + ros-noetic-message-generation, + ros-noetic-sensor-msgs, + ros-noetic-std-msgs, + ros-noetic-ublox-serialization (= ${binary:Version}) Homepage: http://ros.org/wiki/ublox Standards-Version: 3.9.2 Package: ros-noetic-ublox-msgs Architecture: any -Depends: ${shlibs:Depends}, ${misc:Depends}, ros-noetic-message-runtime, ros-noetic-sensor-msgs, ros-noetic-std-msgs, ros-noetic-ublox-serialization +Depends: ${shlibs:Depends}, + ${misc:Depends}, + ros-noetic-message-runtime, + ros-noetic-sensor-msgs, + ros-noetic-std-msgs, + ros-noetic-ublox-serialization (= ${binary:Version}) Description: ublox_msgs contains raw messages for u-blox GNSS devices. diff --git a/ublox_serialization/debian/control b/ublox_serialization/debian/control index 600a77a5..7047187c 100644 --- a/ublox_serialization/debian/control +++ b/ublox_serialization/debian/control @@ -2,11 +2,17 @@ Source: ros-noetic-ublox-serialization Section: misc Priority: optional Maintainer: Veronica Lane -Build-Depends: debhelper (>= 9.0.0), ros-noetic-catkin, ros-noetic-roscpp, ros-noetic-roscpp-serialization +Build-Depends: debhelper (>= 9.0.0), + ros-noetic-catkin, + ros-noetic-roscpp, + ros-noetic-roscpp-serialization Homepage: http://ros.org/wiki/ublox Standards-Version: 3.9.2 Package: ros-noetic-ublox-serialization Architecture: any -Depends: ${shlibs:Depends}, ${misc:Depends}, ros-noetic-roscpp, ros-noetic-roscpp-serialization +Depends: ${shlibs:Depends}, + ${misc:Depends}, + ros-noetic-roscpp, + ros-noetic-roscpp-serialization Description: ublox_serialization provides header files for serialization of ROS messages to and from u-blox message format. From 0b945a38b03dcea3ee1ab41ac782ee8d53f0b2db Mon Sep 17 00:00:00 2001 From: Kristian Kabbabe Date: Mon, 2 May 2022 12:41:56 -0400 Subject: [PATCH 04/17] Debianbizing last package --- ublox/debian/changelog | 105 ++++++++++++++++++++++++++++++++++++ ublox/debian/compat | 1 + ublox/debian/control | 18 +++++++ ublox/debian/rules | 61 +++++++++++++++++++++ ublox/debian/source/format | 1 + ublox/debian/source/options | 5 ++ 6 files changed, 191 insertions(+) create mode 100644 ublox/debian/changelog create mode 100644 ublox/debian/compat create mode 100644 ublox/debian/control create mode 100755 ublox/debian/rules create mode 100644 ublox/debian/source/format create mode 100644 ublox/debian/source/options diff --git a/ublox/debian/changelog b/ublox/debian/changelog new file mode 100644 index 00000000..0ee65993 --- /dev/null +++ b/ublox/debian/changelog @@ -0,0 +1,105 @@ +ros-noetic-ublox (1.5.0-greenzie-focal1) focal; urgency=medium + + * Debianize + + -- Kristian Kabbabe Mon, 02 May 2022 12:39:14 -0400 + +ros-noetic-ublox (1.5.0-0focal) focal; urgency=high + + + + -- Veronica Lane Fri, 15 Oct 2021 00:00:00 -0000 + +ros-noetic-ublox (1.4.1-0focal) focal; urgency=high + + + + -- Veronica Lane Thu, 04 Jun 2020 00:00:00 -0000 + +ros-noetic-ublox (1.4.0-0focal) focal; urgency=high + + * Bump CMake minimum version to 3.0.2 + * Contributors: Gonçalo Pereira + + -- Veronica Lane Thu, 28 May 2020 00:00:00 -0000 + +ros-noetic-ublox (1.3.1-0focal) focal; urgency=high + + * Add metapackage dependencies + * Contributors: Mateusz Sadowski, Tim Clephas + + -- Veronica Lane Thu, 12 Mar 2020 00:00:00 -0000 + +ros-noetic-ublox (1.3.0-0focal) focal; urgency=high + + + + -- Veronica Lane Fri, 10 Jan 2020 00:00:00 -0000 + +ros-noetic-ublox (1.2.0-0focal) focal; urgency=high + + + + -- Veronica Lane Tue, 19 Nov 2019 00:00:00 -0000 + +ros-noetic-ublox (1.1.2-0focal) focal; urgency=high + + * README and package xml updates + * Contributors: Veronica Lane + + -- Veronica Lane Wed, 02 Aug 2017 00:00:00 -0000 + +ros-noetic-ublox (1.1.0-0focal) focal; urgency=high + + * Updated package xmls with new version number and corrected my email address. Also updated readme to include information about new version plus new parameter + * README and ublox package version + * Contributors: Veronica Lane + + -- Veronica Lane Mon, 17 Jul 2017 00:00:00 -0000 + +ros-noetic-ublox (1.0.0-0focal) focal; urgency=high + + * added myself as maintainer to package xmls and updated version numbers of modified packages. + * Contributors: Veronica Lane + + -- Veronica Lane Fri, 23 Jun 2017 00:00:00 -0000 + +ros-noetic-ublox (0.0.5-0focal) focal; urgency=high + + + + -- Veronica Lane Sat, 06 Aug 2016 00:00:00 -0000 + +ros-noetic-ublox (0.0.4-0focal) focal; urgency=high + + + + -- Veronica Lane Mon, 08 Dec 2014 00:00:00 -0000 + +ros-noetic-ublox (0.0.3-0focal) focal; urgency=high + + * Updated readme to reflect changes + * Contributors: Gareth Cross + + -- Veronica Lane Sat, 18 Oct 2014 00:00:00 -0000 + +ros-noetic-ublox (0.0.2-0focal) focal; urgency=high + + + + -- Veronica Lane Fri, 03 Oct 2014 00:00:00 -0000 + +ros-noetic-ublox (0.0.1-0focal) focal; urgency=high + + + + -- Veronica Lane Fri, 15 Aug 2014 00:00:00 -0000 + +ros-noetic-ublox (0.0.0-0focal) focal; urgency=high + + * ublox: first commit + * Contributors: Chao Qu + + -- Veronica Lane Mon, 23 Jun 2014 00:00:00 -0000 + + diff --git a/ublox/debian/compat b/ublox/debian/compat new file mode 100644 index 00000000..ec635144 --- /dev/null +++ b/ublox/debian/compat @@ -0,0 +1 @@ +9 diff --git a/ublox/debian/control b/ublox/debian/control new file mode 100644 index 00000000..4b64616f --- /dev/null +++ b/ublox/debian/control @@ -0,0 +1,18 @@ +Source: ros-noetic-ublox +Section: misc +Priority: optional +Maintainer: Veronica Lane +Build-Depends: debhelper (>= 9.0.0), + ros-noetic-catkin +Homepage: http://wiki.ros.org/ublox +Standards-Version: 3.9.2 + +Package: ros-noetic-ublox +Architecture: any +Depends: ${shlibs:Depends}, + ${misc:Depends}, + ros-noetic-ublox-gps (= ${binary:Version}), + ros-noetic-ublox-msgs (= ${binary:Version}), + ros-noetic-ublox-msg-filters (= ${binary:Version}), + ros-noetic-ublox-serialization (= ${binary:Version}) +Description: Provides a ublox_gps node for u-blox GPS receivers, messages, and serialization packages for the binary UBX protocol. diff --git a/ublox/debian/rules b/ublox/debian/rules new file mode 100755 index 00000000..62671917 --- /dev/null +++ b/ublox/debian/rules @@ -0,0 +1,61 @@ +#!/usr/bin/make -f +# -*- makefile -*- +# Sample debian/rules that uses debhelper. +# This file was originally written by Joey Hess and Craig Small. +# As a special exception, when this file is copied by dh-make into a +# dh-make output file, you may use that output file without restriction. +# This special exception was added by Craig Small in version 0.37 of dh-make. + +# Uncomment this to turn on verbose mode. +export DH_VERBOSE=1 +# TODO: remove the LDFLAGS override. It's here to avoid esoteric problems +# of this sort: +# https://code.ros.org/trac/ros/ticket/2977 +# https://code.ros.org/trac/ros/ticket/3842 +export LDFLAGS= +export PKG_CONFIG_PATH=/opt/ros/noetic/lib/pkgconfig +# Explicitly enable -DNDEBUG, see: +# https://github.com/ros-infrastructure/bloom/issues/327 +export DEB_CXXFLAGS_MAINT_APPEND=-DNDEBUG + +%: + dh $@ -v --buildsystem=cmake + +override_dh_auto_configure: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_configure -- \ + -DCATKIN_BUILD_BINARY_PACKAGE="1" \ + -DCMAKE_INSTALL_PREFIX="/opt/ros/noetic" \ + -DCMAKE_PREFIX_PATH="/opt/ros/noetic" + +override_dh_auto_build: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_build + +override_dh_auto_test: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + echo -- Running tests. Even if one of them fails the build is not canceled. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_test || true + +override_dh_shlibdeps: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_shlibdeps -l$(CURDIR)/debian/ros-noetic-ublox//opt/ros/noetic/lib/ + +override_dh_auto_install: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_install diff --git a/ublox/debian/source/format b/ublox/debian/source/format new file mode 100644 index 00000000..163aaf8d --- /dev/null +++ b/ublox/debian/source/format @@ -0,0 +1 @@ +3.0 (quilt) diff --git a/ublox/debian/source/options b/ublox/debian/source/options new file mode 100644 index 00000000..8bc9182a --- /dev/null +++ b/ublox/debian/source/options @@ -0,0 +1,5 @@ +# Automatically add upstream changes to the quilt overlay. +# http://manpages.ubuntu.com/manpages/trusty/man1/dpkg-source.1.html +# This supports reusing the orig.tar.gz for debian increments. +auto-commit + From 3959b9667eb5adecd1ddcbe086aed20ade17121e Mon Sep 17 00:00:00 2001 From: Jonathan Sanabria Date: Wed, 4 May 2022 16:38:20 -0400 Subject: [PATCH 05/17] tested service. --- ublox_gps/include/ublox_gps/async_worker.h | 179 ++- ublox_gps/include/ublox_gps/callback.h | 138 +- ublox_gps/include/ublox_gps/gps.h | 153 +- ublox_gps/include/ublox_gps/mkgmtime.h | 8 +- ublox_gps/include/ublox_gps/node.h | 424 ++--- ublox_gps/include/ublox_gps/raw_data_pa.h | 165 +- ublox_gps/include/ublox_gps/utils.h | 19 +- ublox_gps/include/ublox_gps/worker.h | 21 +- ublox_gps/src/gps.cpp | 416 ++--- ublox_gps/src/logger_node_pa.cpp | 16 +- ublox_gps/src/node.cpp | 1412 +++++++++-------- ublox_gps/src/raw_data_pa.cpp | 255 +-- .../include/ublox_msg_filters/exact_time.h | 184 ++- ublox_msg_filters/src/example.cpp | 13 +- ublox_msg_filters/tests/test.cpp | 73 +- ublox_msgs/CMakeLists.txt | 5 + .../include/ublox/serialization/ublox_msgs.h | 501 +++--- ublox_msgs/include/ublox_msgs/ublox_msgs.h | 262 +-- ublox_msgs/src/ublox_msgs.cpp | 255 +-- ublox_msgs/srv/GetVersionInfo.srv | 7 + ublox_serialization/include/ublox/checksum.h | 36 +- .../include/ublox/serialization.h | 326 ++-- .../include/ublox/serialization_ros.h | 28 +- 23 files changed, 2641 insertions(+), 2255 deletions(-) create mode 100644 ublox_msgs/srv/GetVersionInfo.srv diff --git a/ublox_gps/include/ublox_gps/async_worker.h b/ublox_gps/include/ublox_gps/async_worker.h index 19c18e29..ae741c4f 100644 --- a/ublox_gps/include/ublox_gps/async_worker.h +++ b/ublox_gps/include/ublox_gps/async_worker.h @@ -37,19 +37,19 @@ #include #include - #include "worker.h" -namespace ublox_gps { - -int debug; //!< Used to determine which debug messages to display +namespace ublox_gps +{ +int debug; //!< Used to determine which debug messages to display /** * @brief Handles Asynchronous I/O reading and writing. */ template -class AsyncWorker : public Worker { - public: +class AsyncWorker : public Worker +{ +public: typedef boost::mutex Mutex; typedef boost::mutex::scoped_lock ScopedLock; @@ -68,13 +68,19 @@ class AsyncWorker : public Worker { * @brief Set the callback function which handles input messages. * @param callback the read callback which handles received messages */ - void setCallback(const Callback& callback) { read_callback_ = callback; } + void setCallback(const Callback& callback) + { + read_callback_ = callback; + } /** * @brief Set the callback function which handles raw data. * @param callback the write callback which handles raw data */ - void setRawDataCallback(const Callback& callback) { write_callback_ = callback; } + void setRawDataCallback(const Callback& callback) + { + write_callback_ = callback; + } /** * @brief Send the data bytes via the I/O stream. @@ -88,9 +94,12 @@ class AsyncWorker : public Worker { */ void wait(const boost::posix_time::time_duration& timeout); - bool isOpen() const { return stream_->is_open(); } + bool isOpen() const + { + return stream_->is_open(); + } - protected: +protected: /** * @brief Read the input stream. */ @@ -113,32 +122,33 @@ class AsyncWorker : public Worker { */ void doClose(); - boost::shared_ptr stream_; //!< The I/O stream - boost::shared_ptr io_service_; //!< The I/O service + boost::shared_ptr stream_; //!< The I/O stream + boost::shared_ptr io_service_; //!< The I/O service - Mutex read_mutex_; //!< Lock for the input buffer + Mutex read_mutex_; //!< Lock for the input buffer boost::condition read_condition_; - std::vector in_; //!< The input buffer - std::size_t in_buffer_size_; //!< number of bytes currently in the input - //!< buffer + std::vector in_; //!< The input buffer + std::size_t in_buffer_size_; //!< number of bytes currently in the input + //!< buffer - Mutex write_mutex_; //!< Lock for the output buffer + Mutex write_mutex_; //!< Lock for the output buffer boost::condition write_condition_; - std::vector out_; //!< The output buffer + std::vector out_; //!< The output buffer - boost::shared_ptr background_thread_; //!< thread for the I/O - //!< service - Callback read_callback_; //!< Callback function to handle received messages - Callback write_callback_; //!< Callback function to handle raw data + boost::shared_ptr background_thread_; //!< thread for the I/O + //!< service + Callback read_callback_; //!< Callback function to handle received messages + Callback write_callback_; //!< Callback function to handle raw data - bool stopping_; //!< Whether or not the I/O service is closed + bool stopping_; //!< Whether or not the I/O service is closed }; template AsyncWorker::AsyncWorker(boost::shared_ptr stream, - boost::shared_ptr io_service, - std::size_t buffer_size) - : stopping_(false) { + boost::shared_ptr io_service, + std::size_t buffer_size) + : stopping_(false) +{ stream_ = stream; io_service_ = io_service; in_.resize(buffer_size); @@ -147,27 +157,29 @@ AsyncWorker::AsyncWorker(boost::shared_ptr stream, out_.reserve(buffer_size); io_service_->post(boost::bind(&AsyncWorker::doRead, this)); - background_thread_.reset(new boost::thread( - boost::bind(&boost::asio::io_service::run, io_service_))); + background_thread_.reset(new boost::thread(boost::bind(&boost::asio::io_service::run, io_service_))); } template -AsyncWorker::~AsyncWorker() { +AsyncWorker::~AsyncWorker() +{ io_service_->post(boost::bind(&AsyncWorker::doClose, this)); background_thread_->join(); - //io_service_->reset(); + // io_service_->reset(); } template -bool AsyncWorker::send(const unsigned char* data, - const unsigned int size) { +bool AsyncWorker::send(const unsigned char* data, const unsigned int size) +{ ScopedLock lock(write_mutex_); - if(size == 0) { + if (size == 0) + { ROS_ERROR("Ublox AsyncWorker::send: Size of message to send is 0"); return true; } - if (out_.capacity() - out_.size() < size) { + if (out_.capacity() - out_.size() < size) + { ROS_ERROR("Ublox AsyncWorker::send: Output buffer too full to send message"); return false; } @@ -178,20 +190,22 @@ bool AsyncWorker::send(const unsigned char* data, } template -void AsyncWorker::doWrite() { +void AsyncWorker::doWrite() +{ ScopedLock lock(write_mutex_); // Do nothing if out buffer is empty - if (out_.size() == 0) { + if (out_.size() == 0) + { return; } // Write all the data in the out buffer boost::asio::write(*stream_, boost::asio::buffer(out_.data(), out_.size())); - if (debug >= 2) { + if (debug >= 2) + { // Print the data that was sent std::ostringstream oss; - for (std::vector::iterator it = out_.begin(); - it != out_.end(); ++it) + for (std::vector::iterator it = out_.begin(); it != out_.end(); ++it) oss << boost::format("%02x") % static_cast(*it) << " "; ROS_DEBUG("U-Blox sent %li bytes: \n%s", out_.size(), oss.str().c_str()); } @@ -200,20 +214,22 @@ void AsyncWorker::doWrite() { write_condition_.notify_all(); } template <> -inline void AsyncWorker::doWrite() { +inline void AsyncWorker::doWrite() +{ ScopedLock lock(write_mutex_); // Do nothing if out buffer is empty - if (out_.size() == 0) { + if (out_.size() == 0) + { return; } // Write all the data in the out buffer stream_->send(boost::asio::buffer(out_.data(), out_.size())); - if (debug >= 2) { + if (debug >= 2) + { // Print the data that was sent std::ostringstream oss; - for (std::vector::iterator it = out_.begin(); - it != out_.end(); ++it) + for (std::vector::iterator it = out_.begin(); it != out_.end(); ++it) oss << boost::format("%02x") % static_cast(*it) << " "; ROS_DEBUG("U-Blox sent %li bytes: \n%s", out_.size(), oss.str().c_str()); } @@ -223,51 +239,52 @@ inline void AsyncWorker::doWrite() { } template -void AsyncWorker::doRead() { +void AsyncWorker::doRead() +{ ScopedLock lock(read_mutex_); - stream_->async_read_some( - boost::asio::buffer(in_.data() + in_buffer_size_, - in_.size() - in_buffer_size_), - boost::bind(&AsyncWorker::readEnd, this, - boost::asio::placeholders::error, - boost::asio::placeholders::bytes_transferred)); + stream_->async_read_some(boost::asio::buffer(in_.data() + in_buffer_size_, in_.size() - in_buffer_size_), + boost::bind(&AsyncWorker::readEnd, + this, + boost::asio::placeholders::error, + boost::asio::placeholders::bytes_transferred)); } template <> -inline void AsyncWorker::doRead() { +inline void AsyncWorker::doRead() +{ ScopedLock lock(read_mutex_); - stream_->async_receive( - boost::asio::buffer(in_.data() + in_buffer_size_, - in_.size() - in_buffer_size_), - boost::bind(&AsyncWorker::readEnd, this, - boost::asio::placeholders::error, - boost::asio::placeholders::bytes_transferred)); + stream_->async_receive(boost::asio::buffer(in_.data() + in_buffer_size_, in_.size() - in_buffer_size_), + boost::bind(&AsyncWorker::readEnd, + this, + boost::asio::placeholders::error, + boost::asio::placeholders::bytes_transferred)); } template -void AsyncWorker::readEnd(const boost::system::error_code& error, - std::size_t bytes_transfered) { +void AsyncWorker::readEnd(const boost::system::error_code& error, std::size_t bytes_transfered) +{ ScopedLock lock(read_mutex_); - if (error) { - ROS_ERROR("U-Blox ASIO input buffer read error: %s, %li", - error.message().c_str(), - bytes_transfered); - } else if (bytes_transfered > 0) { + if (error) + { + ROS_ERROR("U-Blox ASIO input buffer read error: %s, %li", error.message().c_str(), bytes_transfered); + } + else if (bytes_transfered > 0) + { in_buffer_size_ += bytes_transfered; - unsigned char *pRawDataStart = &(*(in_.begin() + (in_buffer_size_ - bytes_transfered))); + unsigned char* pRawDataStart = &(*(in_.begin() + (in_buffer_size_ - bytes_transfered))); std::size_t raw_data_stream_size = bytes_transfered; if (write_callback_) write_callback_(pRawDataStart, raw_data_stream_size); - if (debug >= 4) { + if (debug >= 4) + { std::ostringstream oss; - for (std::vector::iterator it = - in_.begin() + in_buffer_size_ - bytes_transfered; - it != in_.begin() + in_buffer_size_; ++it) + for (std::vector::iterator it = in_.begin() + in_buffer_size_ - bytes_transfered; + it != in_.begin() + in_buffer_size_; + ++it) oss << boost::format("%02x") % static_cast(*it) << " "; - ROS_DEBUG("U-Blox received %li bytes \n%s", bytes_transfered, - oss.str().c_str()); + ROS_DEBUG("U-Blox received %li bytes \n%s", bytes_transfered, oss.str().c_str()); } if (read_callback_) @@ -280,9 +297,9 @@ void AsyncWorker::readEnd(const boost::system::error_code& error, // The remaining buffer size in_.size() - in_buffer_size_ must be at least // one byte. Otherwise readEnd() and doRead() start to busy-wait without // a chance to recover. - if (in_buffer_size_ >= in_.size()) { - ROS_ERROR("U-Blox ASIO input buffer overflow, dropping %zu bytes", - in_buffer_size_); + if (in_buffer_size_ >= in_.size()) + { + ROS_ERROR("U-Blox ASIO input buffer overflow, dropping %zu bytes", in_buffer_size_); in_buffer_size_ = 0; } @@ -291,19 +308,19 @@ void AsyncWorker::readEnd(const boost::system::error_code& error, } template -void AsyncWorker::doClose() { +void AsyncWorker::doClose() +{ ScopedLock lock(read_mutex_); stopping_ = true; boost::system::error_code error; stream_->close(error); - if(error) - ROS_ERROR_STREAM( - "Error while closing the AsyncWorker stream: " << error.message()); + if (error) + ROS_ERROR_STREAM("Error while closing the AsyncWorker stream: " << error.message()); } template -void AsyncWorker::wait( - const boost::posix_time::time_duration& timeout) { +void AsyncWorker::wait(const boost::posix_time::time_duration& timeout) +{ ScopedLock lock(read_mutex_); read_condition_.timed_wait(lock, timeout); } diff --git a/ublox_gps/include/ublox_gps/callback.h b/ublox_gps/include/ublox_gps/callback.h index bbf51f2d..809b6f82 100644 --- a/ublox_gps/include/ublox_gps/callback.h +++ b/ublox_gps/include/ublox_gps/callback.h @@ -14,9 +14,9 @@ // 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 +// 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 BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; @@ -35,13 +35,14 @@ #include #include -namespace ublox_gps { - +namespace ublox_gps +{ /** * @brief A callback handler for a u-blox message. */ -class CallbackHandler { - public: +class CallbackHandler +{ +public: /** * @brief Decode the u-blox message. */ @@ -50,14 +51,15 @@ class CallbackHandler { /** * @brief Wait for on the condition. */ - bool wait(const boost::posix_time::time_duration& timeout) { + bool wait(const boost::posix_time::time_duration& timeout) + { boost::mutex::scoped_lock lock(mutex_); return condition_.timed_wait(lock, timeout); } - protected: - boost::mutex mutex_; //!< Lock for the handler - boost::condition_variable condition_; //!< Condition for the handler lock +protected: + boost::mutex mutex_; //!< Lock for the handler + boost::condition_variable condition_; //!< Condition for the handler lock }; /** @@ -65,40 +67,51 @@ class CallbackHandler { * @typedef T the message type */ template -class CallbackHandler_ : public CallbackHandler { - public: - typedef boost::function Callback; //!< A callback function +class CallbackHandler_ : public CallbackHandler +{ +public: + typedef boost::function Callback; //!< A callback function - /** + /** * @brief Initialize the Callback Handler with a callback function * @param func a callback function for the message, defaults to none */ - CallbackHandler_(const Callback& func = Callback()) : func_(func) {} - + CallbackHandler_(const Callback& func = Callback()) : func_(func) + { + } + /** * @brief Get the last received message. */ - virtual const T& get() { return message_; } + virtual const T& get() + { + return message_; + } /** * @brief Decode the U-Blox message & call the callback function if it exists. * @param reader a reader to decode the message buffer */ - void handle(ublox::Reader& reader) { + void handle(ublox::Reader& reader) + { boost::mutex::scoped_lock lock(mutex_); - try { - if (!reader.read(message_)) { - ROS_DEBUG_COND(debug >= 2, - "U-Blox Decoder error for 0x%02x / 0x%02x (%d bytes)", + try + { + if (!reader.read(message_)) + { + ROS_DEBUG_COND(debug >= 2, + "U-Blox Decoder error for 0x%02x / 0x%02x (%d bytes)", static_cast(reader.classId()), static_cast(reader.messageId()), reader.length()); condition_.notify_all(); return; } - } catch (std::runtime_error& e) { - ROS_DEBUG_COND(debug >= 2, - "U-Blox Decoder error for 0x%02x / 0x%02x (%d bytes)", + } + catch (std::runtime_error& e) + { + ROS_DEBUG_COND(debug >= 2, + "U-Blox Decoder error for 0x%02x / 0x%02x (%d bytes)", static_cast(reader.classId()), static_cast(reader.messageId()), reader.length()); @@ -106,36 +119,38 @@ class CallbackHandler_ : public CallbackHandler { return; } - if (func_) func_(message_); + if (func_) + func_(message_); condition_.notify_all(); } - - private: - Callback func_; //!< the callback function to handle the message - T message_; //!< The last received message + +private: + Callback func_; //!< the callback function to handle the message + T message_; //!< The last received message }; /** * @brief Callback handlers for incoming u-blox messages. */ -class CallbackHandlers { - public: +class CallbackHandlers +{ +public: /** * @brief Add a callback handler for the given message type. * @param callback the callback handler for the message * @typedef.a ublox_msgs message with CLASS_ID and MESSAGE_ID constants */ template - void insert(typename CallbackHandler_::Callback callback) { + void insert(typename CallbackHandler_::Callback callback) + { boost::mutex::scoped_lock lock(callback_mutex_); CallbackHandler_* handler = new CallbackHandler_(callback); callbacks_.insert( - std::make_pair(std::make_pair(T::CLASS_ID, T::MESSAGE_ID), - boost::shared_ptr(handler))); + std::make_pair(std::make_pair(T::CLASS_ID, T::MESSAGE_ID), boost::shared_ptr(handler))); } /** - * @brief Add a callback handler for the given message type and ID. This is + * @brief Add a callback handler for the given message type and ID. This is * used for messages in which have the same structure (and therefore msg file) * and same class ID but different message IDs. (e.g. INF, ACK) * @param callback the callback handler for the message @@ -143,27 +158,25 @@ class CallbackHandlers { * @typedef.a ublox_msgs message with a CLASS_ID constant */ template - void insert( - typename CallbackHandler_::Callback callback, - unsigned int message_id) { + void insert(typename CallbackHandler_::Callback callback, unsigned int message_id) + { boost::mutex::scoped_lock lock(callback_mutex_); CallbackHandler_* handler = new CallbackHandler_(callback); callbacks_.insert( - std::make_pair(std::make_pair(T::CLASS_ID, message_id), - boost::shared_ptr(handler))); + std::make_pair(std::make_pair(T::CLASS_ID, message_id), boost::shared_ptr(handler))); } /** * @brief Calls the callback handler for the message in the reader. * @param reader a reader containing a u-blox message */ - void handle(ublox::Reader& reader) { + void handle(ublox::Reader& reader) + { // Find the callback handlers for the message & decode it boost::mutex::scoped_lock lock(callback_mutex_); - Callbacks::key_type key = - std::make_pair(reader.classId(), reader.messageId()); - for (Callbacks::iterator callback = callbacks_.lower_bound(key); - callback != callbacks_.upper_bound(key); ++callback) + Callbacks::key_type key = std::make_pair(reader.classId(), reader.messageId()); + for (Callbacks::iterator callback = callbacks_.lower_bound(key); callback != callbacks_.upper_bound(key); + ++callback) callback->second->handle(reader); } @@ -173,22 +186,23 @@ class CallbackHandlers { * @param timeout the amount of time to wait for the desired message */ template - bool read(T& message, const boost::posix_time::time_duration& timeout) { + bool read(T& message, const boost::posix_time::time_duration& timeout) + { bool result = false; // Create a callback handler for this message callback_mutex_.lock(); CallbackHandler_* handler = new CallbackHandler_(); Callbacks::iterator callback = callbacks_.insert( - (std::make_pair(std::make_pair(T::CLASS_ID, T::MESSAGE_ID), - boost::shared_ptr(handler)))); + (std::make_pair(std::make_pair(T::CLASS_ID, T::MESSAGE_ID), boost::shared_ptr(handler)))); callback_mutex_.unlock(); // Wait for the message - if (handler->wait(timeout)) { + if (handler->wait(timeout)) + { message = handler->get(); result = true; } - + // Remove the callback handler callback_mutex_.lock(); callbacks_.erase(callback); @@ -202,18 +216,19 @@ class CallbackHandlers { * @param data the buffer of u-blox messages to process * @param size the size of the buffer */ - void readCallback(unsigned char* data, std::size_t& size) { + void readCallback(unsigned char* data, std::size_t& size) + { ublox::Reader reader(data, size); // Read all U-Blox messages in buffer - while (reader.search() != reader.end() && reader.found()) { - if (debug >= 3) { + while (reader.search() != reader.end() && reader.found()) + { + if (debug >= 3) + { // Print the received bytes std::ostringstream oss; - for (ublox::Reader::iterator it = reader.pos(); - it != reader.pos() + reader.length() + 8; ++it) + for (ublox::Reader::iterator it = reader.pos(); it != reader.pos() + reader.length() + 8; ++it) oss << boost::format("%02x") % static_cast(*it) << " "; - ROS_DEBUG("U-blox: reading %d bytes\n%s", reader.length() + 8, - oss.str().c_str()); + ROS_DEBUG("U-blox: reading %d bytes\n%s", reader.length() + 8, oss.str().c_str()); } handle(reader); @@ -224,9 +239,8 @@ class CallbackHandlers { size -= reader.pos() - data; } - private: - typedef std::multimap, - boost::shared_ptr > Callbacks; +private: + typedef std::multimap, boost::shared_ptr > Callbacks; // Call back handlers for u-blox messages Callbacks callbacks_; diff --git a/ublox_gps/include/ublox_gps/gps.h b/ublox_gps/include/ublox_gps/gps.h index 504c2a2a..b91be5f4 100644 --- a/ublox_gps/include/ublox_gps/gps.h +++ b/ublox_gps/include/ublox_gps/gps.h @@ -53,21 +53,16 @@ * This namespace is for I/O communication with the u-blox device, including * read callbacks. */ -namespace ublox_gps { +namespace ublox_gps +{ //! Possible baudrates for u-blox devices -constexpr static unsigned int kBaudrates[] = { 4800, - 9600, - 19200, - 38400, - 57600, - 115200, - 230400, - 460800 }; +constexpr static unsigned int kBaudrates[] = { 4800, 9600, 19200, 38400, 57600, 115200, 230400, 460800 }; /** * @brief Handles communication with and configuration of the u-blox device */ -class Gps { - public: +class Gps +{ +public: //! Default timeout for ACK messages in seconds constexpr static double kDefaultAckTimeout = 1.0; //! Size of write buffer for output messages @@ -80,7 +75,8 @@ class Gps { * @brief If called, when the node shuts down, it will send a command to * save the flash memory. */ - void setSaveOnShutdown(bool save_on_shutdown) { + void setSaveOnShutdown(bool save_on_shutdown) + { save_on_shutdown_ = save_on_shutdown; } @@ -88,7 +84,10 @@ class Gps { * @brief Set the internal flag for enabling or disabling the initial configurations. * @param config_on_startup boolean flag */ - void setConfigOnStartup(const bool config_on_startup) { config_on_startup_flag_ = config_on_startup; } + void setConfigOnStartup(const bool config_on_startup) + { + config_on_startup_flag_ = config_on_startup; + } /** * @brief Initialize TCP I/O. @@ -111,8 +110,7 @@ class Gps { * @param uart_in the UART In protocol, see CfgPRT for options * @param uart_out the UART Out protocol, see CfgPRT for options */ - void initializeSerial(std::string port, unsigned int baudrate, - uint16_t uart_in, uint16_t uart_out); + void initializeSerial(std::string port, unsigned int baudrate, uint16_t uart_in, uint16_t uart_out); /** * @brief Reset the Serial I/O port after u-blox reset. @@ -125,13 +123,13 @@ class Gps { /** * @brief Send rtcm correction message. - */ - bool sendRtcm(const std::vector &message); + */ + bool sendRtcm(const std::vector& message); /** * @brief Send SPARTN correction message. - */ - bool sendSpartn(const std::vector &message); + */ + bool sendSpartn(const std::vector& message); /** * @brief Closes the I/O port, and initiates save on shutdown procedure @@ -160,8 +158,7 @@ class Gps { * @return true if the GNSS was configured, the device was reset, and the * I/O reset successfully */ - bool configGnss(ublox_msgs::CfgGNSS gnss, - const boost::posix_time::time_duration& wait); + bool configGnss(ublox_msgs::CfgGNSS gnss, const boost::posix_time::time_duration& wait); /** * @brief Send a message to the receiver to delete the BBR data stored in @@ -177,8 +174,7 @@ class Gps { * @param out_proto_mask the out protocol mask, see CfgPRT message * @return true on ACK, false on other conditions. */ - bool configUart1(unsigned int baudrate, uint16_t in_proto_mask, - uint16_t out_proto_mask); + bool configUart1(unsigned int baudrate, uint16_t in_proto_mask, uint16_t out_proto_mask); /** * @brief Disable the UART Port. Sets in/out protocol masks to 0. Does not @@ -196,8 +192,7 @@ class Gps { * @param out_proto_mask the out protocol mask, see CfgPRT message * @return true on ACK, false on other conditions. */ - bool configUsb(uint16_t tx_ready, uint16_t in_proto_mask, - uint16_t out_proto_mask); + bool configUsb(uint16_t tx_ready, uint16_t in_proto_mask, uint16_t out_proto_mask); /** * @brief Configure the device navigation and measurement rate settings. @@ -315,7 +310,7 @@ class Gps { bool setUseAdr(bool enable, float protocol_version); /** - * @brief Configure the U-Blox to UTC time + * @brief Configure the U-Blox to UTC time * @return true on ACK, false on other conditions. * * @note This is part of the expert settings. It is recommended you check @@ -332,7 +327,7 @@ class Gps { * the ublox manual first. */ bool setTimtm2(uint8_t rate); - + /** * @brief Configure the U-Blox send rate of the message & subscribe to the * given message @@ -340,8 +335,7 @@ class Gps { * @param rate the rate in Hz of the message */ template - void subscribe(typename CallbackHandler_::Callback callback, - unsigned int rate); + void subscribe(typename CallbackHandler_::Callback callback, unsigned int rate); /** * @brief Subscribe to the given Ublox message. * @param the callback handler for the message @@ -357,8 +351,7 @@ class Gps { * @param message_id the U-Blox message ID */ template - void subscribeId(typename CallbackHandler_::Callback callback, - unsigned int message_id); + void subscribeId(typename CallbackHandler_::Callback callback, unsigned int message_id); /** * Read a u-blox message of the given type. @@ -366,12 +359,20 @@ class Gps { * @param timeout the amount of time to wait for the desired message */ template - bool read(T& message, - const boost::posix_time::time_duration& timeout = default_timeout_); + bool read(T& message, const boost::posix_time::time_duration& timeout = default_timeout_); - bool isInitialized() const { return worker_ != 0; } - bool isConfigured() const { return isInitialized() && configured_; } - bool isOpen() const { return worker_->isOpen(); } + bool isInitialized() const + { + return worker_ != 0; + } + bool isConfigured() const + { + return isInitialized() && configured_; + } + bool isOpen() const + { + return worker_->isOpen(); + } /** * Poll a u-blox message of the given type. @@ -392,8 +393,7 @@ class Gps { * defaults to empty * @param timeout the amount of time to wait for the desired message */ - bool poll(uint8_t class_id, uint8_t message_id, - const std::vector& payload = std::vector()); + bool poll(uint8_t class_id, uint8_t message_id, const std::vector& payload = std::vector()); /** * @brief Send the given configuration message. @@ -412,8 +412,7 @@ class Gps { * @param msg_id the expected message ID of the ACK * @return true if expected ACK received, false otherwise */ - bool waitForAcknowledge(const boost::posix_time::time_duration& timeout, - uint8_t class_id, uint8_t msg_id); + bool waitForAcknowledge(const boost::posix_time::time_duration& timeout, uint8_t class_id, uint8_t msg_id); /** * @brief Set the callback function which handles raw data. @@ -421,19 +420,21 @@ class Gps { */ void setRawDataCallback(const Worker::Callback& callback); - private: +private: //! Types for ACK/NACK messages, WAIT is used when waiting for an ACK - enum AckType { - NACK, //! Not acknowledged - ACK, //! Acknowledge - WAIT //! Waiting for ACK + enum AckType + { + NACK, //! Not acknowledged + ACK, //! Acknowledge + WAIT //! Waiting for ACK }; //! Stores ACK/NACK messages - struct Ack { - AckType type; //!< The ACK type - uint8_t class_id; //!< The class ID of the ACK - uint8_t msg_id; //!< The message ID of the ACK + struct Ack + { + AckType type; //!< The ACK type + uint8_t class_id; //!< The class ID of the ACK + uint8_t msg_id; //!< The message ID of the ACK }; /** @@ -451,19 +452,19 @@ class Gps { * @brief Callback handler for UBX-ACK message. * @param m the message to process */ - void processAck(const ublox_msgs::Ack &m); + void processAck(const ublox_msgs::Ack& m); /** * @brief Callback handler for UBX-NACK message. * @param m the message to process */ - void processNack(const ublox_msgs::Ack &m); + void processNack(const ublox_msgs::Ack& m); /** * @brief Callback handler for UBX-UPD-SOS-ACK message. * @param m the message to process */ - void processUpdSosAck(const ublox_msgs::UpdSOS_Ack &m); + void processUpdSosAck(const ublox_msgs::UpdSOS_Ack& m); /** * @brief Execute save on shutdown procedure. @@ -486,7 +487,6 @@ class Gps { //!< Whether or not initial configuration to the hardware is done bool config_on_startup_flag_; - //! The default timeout for ACK messages static const boost::posix_time::time_duration default_timeout_; //! Stores last received ACK accessed by multiple threads @@ -499,40 +499,46 @@ class Gps { }; template -void Gps::subscribe( - typename CallbackHandler_::Callback callback, unsigned int rate) { - if (!setRate(T::CLASS_ID, T::MESSAGE_ID, rate)) return; +void Gps::subscribe(typename CallbackHandler_::Callback callback, unsigned int rate) +{ + if (!setRate(T::CLASS_ID, T::MESSAGE_ID, rate)) + return; subscribe(callback); } template -void Gps::subscribe(typename CallbackHandler_::Callback callback) { +void Gps::subscribe(typename CallbackHandler_::Callback callback) +{ callbacks_.insert(callback); } template -void Gps::subscribeId(typename CallbackHandler_::Callback callback, - unsigned int message_id) { +void Gps::subscribeId(typename CallbackHandler_::Callback callback, unsigned int message_id) +{ callbacks_.insert(callback, message_id); } template -bool Gps::poll(ConfigT& message, - const std::vector& payload, - const boost::posix_time::time_duration& timeout) { - if (!poll(ConfigT::CLASS_ID, ConfigT::MESSAGE_ID, payload)) return false; +bool Gps::poll(ConfigT& message, const std::vector& payload, const boost::posix_time::time_duration& timeout) +{ + if (!poll(ConfigT::CLASS_ID, ConfigT::MESSAGE_ID, payload)) + return false; return read(message, timeout); } template -bool Gps::read(T& message, const boost::posix_time::time_duration& timeout) { - if (!worker_) return false; +bool Gps::read(T& message, const boost::posix_time::time_duration& timeout) +{ + if (!worker_) + return false; return callbacks_.read(message, timeout); } template -bool Gps::configure(const ConfigT& message, bool wait) { - if (!worker_) return false; +bool Gps::configure(const ConfigT& message, bool wait) +{ + if (!worker_) + return false; // Reset ack Ack ack; @@ -542,20 +548,19 @@ bool Gps::configure(const ConfigT& message, bool wait) { // Encode the message std::vector out(kWriterSize); ublox::Writer writer(out.data(), out.size()); - if (!writer.write(message)) { - ROS_ERROR("Failed to encode config message 0x%02x / 0x%02x", - message.CLASS_ID, message.MESSAGE_ID); + if (!writer.write(message)) + { + ROS_ERROR("Failed to encode config message 0x%02x / 0x%02x", message.CLASS_ID, message.MESSAGE_ID); return false; } // Send the message to the device worker_->send(out.data(), writer.end() - out.data()); - if (!wait) return true; + if (!wait) + return true; // Wait for an acknowledgment and return whether or not it was received - return waitForAcknowledge(default_timeout_, - message.CLASS_ID, - message.MESSAGE_ID); + return waitForAcknowledge(default_timeout_, message.CLASS_ID, message.MESSAGE_ID); } } // namespace ublox_gps diff --git a/ublox_gps/include/ublox_gps/mkgmtime.h b/ublox_gps/include/ublox_gps/mkgmtime.h index 8537379c..e4ce8029 100644 --- a/ublox_gps/include/ublox_gps/mkgmtime.h +++ b/ublox_gps/include/ublox_gps/mkgmtime.h @@ -1,6 +1,6 @@ /* mkgmtime.h -- make a time_t from a gmtime struct tm $Id: mkgmtime.h,v 1.5 2003/02/13 20:15:41 rjs3 Exp $ - + * Copyright (c) 1998-2003 Carnegie Mellon University. All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -8,7 +8,7 @@ * are met: * * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. + * 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 @@ -18,7 +18,7 @@ * 3. The name "Carnegie Mellon University" must not be used to * endorse or promote products derived from this software without * prior written permission. For permission or any other legal - * details, please contact + * details, please contact * Office of Technology Transfer * Carnegie Mellon University * 5000 Forbes Avenue @@ -50,6 +50,6 @@ * @brief Get the UTC time in seconds and nano-seconds from a time struct in * GM time. */ -extern time_t mkgmtime(struct tm * const tmp); +extern time_t mkgmtime(struct tm* const tmp); #endif /* INCLUDED_MKGMTIME_H */ \ No newline at end of file diff --git a/ublox_gps/include/ublox_gps/node.h b/ublox_gps/include/ublox_gps/node.h index 8163b773..91bce001 100644 --- a/ublox_gps/include/ublox_gps/node.h +++ b/ublox_gps/include/ublox_gps/node.h @@ -53,6 +53,7 @@ #include // Other U-Blox package includes #include +#include // Ublox GPS includes #include #include @@ -85,8 +86,8 @@ * This namespace is for the ROS u-blox node and handles anything regarding * ROS parameters, message passing, diagnostics, etc. */ -namespace ublox_node { - +namespace ublox_node +{ //! Queue size for ROS publishers constexpr static uint32_t kROSQueueSize = 1; //! Default measurement period for HPG devices @@ -128,10 +129,12 @@ std::vector rtcm_rates; //! Flag for enabling configuration on startup bool config_on_startup_flag_; - //! Topic diagnostics for u-blox messages -struct UbloxTopicDiagnostic { - UbloxTopicDiagnostic() {} +struct UbloxTopicDiagnostic +{ + UbloxTopicDiagnostic() + { + } // Must not copy this struct (would confuse FrequencyStatusParam pointers) UbloxTopicDiagnostic(const UbloxTopicDiagnostic&) = delete; @@ -144,15 +147,13 @@ struct UbloxTopicDiagnostic { * @param freq_tol the tolerance [%] for the topic frequency * @param freq_window the number of messages to use for diagnostic statistics */ - UbloxTopicDiagnostic (std::string topic, double freq_tol, int freq_window) { - const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate); // Hz + UbloxTopicDiagnostic(std::string topic, double freq_tol, int freq_window) + { + const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate); // Hz min_freq = target_freq; max_freq = target_freq; - diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, - freq_tol, freq_window); - diagnostic = new diagnostic_updater::HeaderlessTopicDiagnostic(topic, - *updater, - freq_param); + diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, freq_tol, freq_window); + diagnostic = new diagnostic_updater::HeaderlessTopicDiagnostic(topic, *updater, freq_param); } /** @@ -165,19 +166,16 @@ struct UbloxTopicDiagnostic { * @param freq_tol the tolerance [%] for the topic frequency * @param freq_window the number of messages to use for diagnostic statistics */ - UbloxTopicDiagnostic (std::string topic, double freq_min, double freq_max, - double freq_tol, int freq_window) { + UbloxTopicDiagnostic(std::string topic, double freq_min, double freq_max, double freq_tol, int freq_window) + { min_freq = freq_min; max_freq = freq_max; - diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, - freq_tol, freq_window); - diagnostic = new diagnostic_updater::HeaderlessTopicDiagnostic(topic, - *updater, - freq_param); + diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, freq_tol, freq_window); + diagnostic = new diagnostic_updater::HeaderlessTopicDiagnostic(topic, *updater, freq_param); } //! Topic frequency diagnostic updater - diagnostic_updater::HeaderlessTopicDiagnostic *diagnostic; + diagnostic_updater::HeaderlessTopicDiagnostic* diagnostic; //! Minimum allow frequency of topic double min_freq; //! Maximum allow frequency of topic @@ -185,8 +183,11 @@ struct UbloxTopicDiagnostic { }; //! Topic diagnostics for fix / fix_velocity messages -struct FixDiagnostic { - FixDiagnostic() {} +struct FixDiagnostic +{ + FixDiagnostic() + { + } // Must not copy this struct (would confuse FrequencyStatusParam pointers) FixDiagnostic(const FixDiagnostic&) = delete; @@ -200,23 +201,19 @@ struct FixDiagnostic { * @param freq_window the number of messages to use for diagnostic statistics * @param stamp_min the minimum allowed time delay */ - FixDiagnostic (std::string name, double freq_tol, int freq_window, - double stamp_min) { - const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate); // Hz + FixDiagnostic(std::string name, double freq_tol, int freq_window, double stamp_min) + { + const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate); // Hz min_freq = target_freq; max_freq = target_freq; - diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, - freq_tol, freq_window); + diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, freq_tol, freq_window); double stamp_max = meas_rate * 1e-3 * (1 + freq_tol); diagnostic_updater::TimeStampStatusParam time_param(stamp_min, stamp_max); - diagnostic = new diagnostic_updater::TopicDiagnostic(name, - *updater, - freq_param, - time_param); + diagnostic = new diagnostic_updater::TopicDiagnostic(name, *updater, freq_param, time_param); } //! Topic frequency diagnostic updater - diagnostic_updater::TopicDiagnostic *diagnostic; + diagnostic_updater::TopicDiagnostic* diagnostic; //! Minimum allow frequency of topic double min_freq; //! Maximum allow frequency of topic @@ -262,8 +259,10 @@ uint8_t fixModeFromString(const std::string& mode); * @throws std::runtime_error if it is below the minimum */ template -void checkMin(V val, T min, std::string name) { - if(val < min) { +void checkMin(V val, T min, std::string name) +{ + if (val < min) + { std::stringstream oss; oss << "Invalid settings: " << name << " must be > " << min; throw std::runtime_error(oss.str()); @@ -279,11 +278,12 @@ void checkMin(V val, T min, std::string name) { * @throws std::runtime_error if it is out of bounds */ template -void checkRange(V val, T min, T max, std::string name) { - if(val < min || val > max) { +void checkRange(V val, T min, T max, std::string name) +{ + if (val < min || val > max) + { std::stringstream oss; - oss << "Invalid settings: " << name << " must be in range [" << min << - ", " << max << "]."; + oss << "Invalid settings: " << name << " must be in range [" << min << ", " << max << "]."; throw std::runtime_error(oss.str()); } } @@ -297,8 +297,10 @@ void checkRange(V val, T min, T max, std::string name) { * @throws std::runtime_error value it is out of bounds */ template -void checkRange(std::vector val, T min, T max, std::string name) { - for(size_t i = 0; i < val.size(); i++) { +void checkRange(std::vector val, T min, T max, std::string name) +{ + for (size_t i = 0; i < val.size(); i++) + { std::stringstream oss; oss << name << "[" << i << "]"; checkRange(val[i], min, max, oss.str()); @@ -313,15 +315,17 @@ void checkRange(std::vector val, T min, T max, std::string name) { * @return true if found, false if not found. */ template -bool getRosUint(const std::string& key, U &u) { +bool getRosUint(const std::string& key, U& u) +{ int param; - if (!nh->getParam(key, param)) return false; + if (!nh->getParam(key, param)) + return false; // Check the bounds U min = std::numeric_limits::lowest(); U max = std::numeric_limits::max(); checkRange(param, min, max, key); // set the output - u = (U) param; + u = (U)param; return true; } @@ -334,8 +338,9 @@ bool getRosUint(const std::string& key, U &u) { * @return true if found, false if not found. */ template -void getRosUint(const std::string& key, U &u, V default_val) { - if(!getRosUint(key, u)) +void getRosUint(const std::string& key, U& u, V default_val) +{ + if (!getRosUint(key, u)) u = default_val; } @@ -345,9 +350,11 @@ void getRosUint(const std::string& key, U &u, V default_val) { * @return true if found, false if not found. */ template -bool getRosUint(const std::string& key, std::vector &u) { +bool getRosUint(const std::string& key, std::vector& u) +{ std::vector param; - if (!nh->getParam(key, param)) return false; + if (!nh->getParam(key, param)) + return false; // Check the bounds U min = std::numeric_limits::lowest(); @@ -367,15 +374,17 @@ bool getRosUint(const std::string& key, std::vector &u) { * @return true if found, false if not found. */ template -bool getRosInt(const std::string& key, I &u) { +bool getRosInt(const std::string& key, I& u) +{ int param; - if (!nh->getParam(key, param)) return false; + if (!nh->getParam(key, param)) + return false; // Check the bounds I min = std::numeric_limits::lowest(); I max = std::numeric_limits::max(); checkRange(param, min, max, key); // set the output - u = (I) param; + u = (I)param; return true; } @@ -388,8 +397,9 @@ bool getRosInt(const std::string& key, I &u) { * @return true if found, false if not found. */ template -void getRosInt(const std::string& key, U &u, V default_val) { - if(!getRosInt(key, u)) +void getRosInt(const std::string& key, U& u, V default_val) +{ + if (!getRosInt(key, u)) u = default_val; } @@ -399,9 +409,11 @@ void getRosInt(const std::string& key, U &u, V default_val) { * @return true if found, false if not found. */ template -bool getRosInt(const std::string& key, std::vector &i) { +bool getRosInt(const std::string& key, std::vector& i) +{ std::vector param; - if (!nh->getParam(key, param)) return false; + if (!nh->getParam(key, param)) + return false; // Check the bounds I min = std::numeric_limits::lowest(); @@ -422,9 +434,9 @@ bool getRosInt(const std::string& key, std::vector &i) { * @param topic the topic to publish the message on */ template -void publish(const MessageT& m, const std::string& topic) { - static ros::Publisher publisher = nh->advertise(topic, - kROSQueueSize); +void publish(const MessageT& m, const std::string& topic) +{ + static ros::Publisher publisher = nh->advertise(topic, kROSQueueSize); publisher.publish(m); } @@ -433,7 +445,8 @@ void publish(const MessageT& m, const std::string& topic) { * i.e. GPS, GLO, GAL, BDS, QZSS, SBAS, IMES * @return true if the device supports the given GNSS */ -bool supportsGnss(std::string gnss) { +bool supportsGnss(std::string gnss) +{ return supported.count(gnss) > 0; } @@ -443,8 +456,9 @@ bool supportsGnss(std::string gnss) { * @details This interface is generic and can be implemented for other features * besides the main node, hardware versions, and firmware versions. */ -class ComponentInterface { - public: +class ComponentInterface +{ +public: /** * @brief Get the ROS parameters. * @throws std::runtime_error if a parameter is invalid or required @@ -488,8 +502,9 @@ typedef boost::shared_ptr ComponentPtr; * The UbloxNode calls the public methods of ComponentInterface for each * element in the components vector. */ -class UbloxNode : public virtual ComponentInterface { - public: +class UbloxNode : public virtual ComponentInterface +{ +public: //! How often (in seconds) to send keep-alive message constexpr static double kKeepAlivePeriod = 10.0; //! How often (in seconds) to call poll messages @@ -533,10 +548,15 @@ class UbloxNode : public virtual ComponentInterface { /** * @brief Print an INF message to the ROS console. */ - void printInf(const ublox_msgs::Inf &m, uint8_t id); + void printInf(const ublox_msgs::Inf& m, uint8_t id); - private: + /** + * @brief A ROS service that helps developers reliably get version info from the ublox device. + */ + bool getVersionInfo(ublox_msgs::GetVersionInfo::Request &req, + ublox_msgs::GetVersionInfo::Response &res); +private: /** * @brief Initialize the I/O handling. */ @@ -579,8 +599,7 @@ class UbloxNode : public virtual ComponentInterface { * @param for HPG/TIM products, this value is either REF or ROV, for other * products this string is empty */ - void addProductInterface(std::string product_category, - std::string ref_rov = ""); + void addProductInterface(std::string product_category, std::string ref_rov = ""); /** * @brief Poll version message from the U-Blox device to keep socket active. @@ -633,7 +652,7 @@ class UbloxNode : public virtual ComponentInterface { //! USB in protocol (see CfgPRT message for constants) uint16_t usb_in_; //! USB out protocol (see CfgPRT message for constants) - uint16_t usb_out_ ; + uint16_t usb_out_; //! The measurement rate in Hz double rate_; //! If true, set configure the User-Defined Datum @@ -666,26 +685,27 @@ class UbloxNode : public virtual ComponentInterface { * * @details The Firmware components update the fix diagnostics. */ -class UbloxFirmware : public virtual ComponentInterface { - public: +class UbloxFirmware : public virtual ComponentInterface +{ +public: /** * @brief Add the fix diagnostics to the updater. */ void initializeRosDiagnostics(); - protected: +protected: /** * @brief Handle to send fix status to ROS diagnostics. */ - virtual void fixDiagnostic( - diagnostic_updater::DiagnosticStatusWrapper& stat) = 0; + virtual void fixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) = 0; }; /** * @brief Implements functions for firmware version 6. */ -class UbloxFirmware6 : public UbloxFirmware { - public: +class UbloxFirmware6 : public UbloxFirmware +{ +public: UbloxFirmware6(); /** @@ -704,14 +724,14 @@ class UbloxFirmware6 : public UbloxFirmware { */ void subscribe(); - protected: +protected: /** * @brief Updates fix diagnostic from NavPOSLLH, NavVELNED, and NavSOL * messages. */ void fixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat); - private: +private: /** * @brief Publish the fix and call the fix diagnostic updater. * @@ -763,9 +783,10 @@ class UbloxFirmware6 : public UbloxFirmware { * * @typedef NavPVT the NavPVT message type for the given firmware version */ -template -class UbloxFirmware7Plus : public UbloxFirmware { - public: +template +class UbloxFirmware7Plus : public UbloxFirmware +{ +public: /** * @brief Publish a NavSatFix and TwistWithCovarianceStamped messages. * @@ -774,76 +795,80 @@ class UbloxFirmware7Plus : public UbloxFirmware { * is published. This function also calls the ROS diagnostics updater. * @param m the message to publish */ - void callbackNavPvt(const NavPVT& m) { - if(enabled["nav_pvt"]) { + void callbackNavPvt(const NavPVT& m) + { + if (enabled["nav_pvt"]) + { // NavPVT publisher - static ros::Publisher publisher = nh->advertise("navpvt", - kROSQueueSize); + static ros::Publisher publisher = nh->advertise("navpvt", kROSQueueSize); publisher.publish(m); } // // NavSatFix message // - static ros::Publisher fixPublisher = - nh->advertise("fix", kROSQueueSize); + static ros::Publisher fixPublisher = nh->advertise("fix", kROSQueueSize); sensor_msgs::NavSatFix fix; fix.header.frame_id = frame_id; // set the timestamp uint8_t valid_time = m.VALID_DATE | m.VALID_TIME | m.VALID_FULLY_RESOLVED; - if (((m.valid & valid_time) == valid_time) && - (m.flags2 & m.FLAGS2_CONFIRMED_AVAILABLE)) { + if (((m.valid & valid_time) == valid_time) && (m.flags2 & m.FLAGS2_CONFIRMED_AVAILABLE)) + { // Use NavPVT timestamp since it is valid // The time in nanoseconds from the NavPVT message can be between -1e9 and 1e9 // The ros time uses only unsigned values, so a negative nano seconds must be // converted to a positive value - if (m.nano < 0) { + if (m.nano < 0) + { fix.header.stamp.sec = toUtcSeconds(m) - 1; fix.header.stamp.nsec = (uint32_t)(m.nano + 1e9); } - else { + else + { fix.header.stamp.sec = toUtcSeconds(m); fix.header.stamp.nsec = (uint32_t)(m.nano); } - } else { + } + else + { // Use ROS time since NavPVT timestamp is not valid fix.header.stamp = ros::Time::now(); } // Set the LLA - fix.latitude = m.lat * 1e-7; // to deg - fix.longitude = m.lon * 1e-7; // to deg - fix.altitude = m.height * 1e-3; // to [m] + fix.latitude = m.lat * 1e-7; // to deg + fix.longitude = m.lon * 1e-7; // to deg + fix.altitude = m.height * 1e-3; // to [m] // Set the Fix status bool fixOk = m.flags & m.FLAGS_GNSS_FIX_OK; - if (fixOk && m.fixType >= m.FIX_TYPE_2D) { + if (fixOk && m.fixType >= m.FIX_TYPE_2D) + { fix.status.status = fix.status.STATUS_FIX; - if(m.flags & m.CARRIER_PHASE_FIXED) + if (m.flags & m.CARRIER_PHASE_FIXED) fix.status.status = fix.status.STATUS_GBAS_FIX; } - else { + else + { fix.status.status = fix.status.STATUS_NO_FIX; } // Set the service based on GNSS configuration fix.status.service = fix_status_service; // Set the position covariance - const double varH = pow(m.hAcc / 1000.0, 2); // to [m^2] - const double varV = pow(m.vAcc / 1000.0, 2); // to [m^2] + const double varH = pow(m.hAcc / 1000.0, 2); // to [m^2] + const double varV = pow(m.vAcc / 1000.0, 2); // to [m^2] fix.position_covariance[0] = varH; fix.position_covariance[4] = varH; fix.position_covariance[8] = varV; - fix.position_covariance_type = - sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; + fix.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; fixPublisher.publish(fix); // // Twist message // - static ros::Publisher velocityPublisher = - nh->advertise("fix_velocity", - kROSQueueSize); + static ros::Publisher velocityPublisher = nh->advertise("fix_velocity", + kROSQueueSize); geometry_msgs::TwistWithCovarianceStamped velocity; velocity.header.stamp = fix.header.stamp; velocity.header.frame_id = frame_id; @@ -870,51 +895,63 @@ class UbloxFirmware7Plus : public UbloxFirmware { updater->update(); } - protected: - +protected: /** * @brief Update the fix diagnostics from Nav PVT message. */ - void fixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) { + void fixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) + { // check the last message, convert to diagnostic - if (last_nav_pvt_.fixType == - ublox_msgs::NavPVT::FIX_TYPE_DEAD_RECKONING_ONLY) { + if (last_nav_pvt_.fixType == ublox_msgs::NavPVT::FIX_TYPE_DEAD_RECKONING_ONLY) + { stat.level = diagnostic_msgs::DiagnosticStatus::WARN; stat.message = "Dead reckoning only"; - } else if (last_nav_pvt_.fixType == ublox_msgs::NavPVT::FIX_TYPE_2D) { + } + else if (last_nav_pvt_.fixType == ublox_msgs::NavPVT::FIX_TYPE_2D) + { stat.level = diagnostic_msgs::DiagnosticStatus::WARN; stat.message = "2D fix"; - } else if (last_nav_pvt_.fixType == ublox_msgs::NavPVT::FIX_TYPE_3D) { + } + else if (last_nav_pvt_.fixType == ublox_msgs::NavPVT::FIX_TYPE_3D) + { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "3D fix"; - } else if (last_nav_pvt_.fixType == - ublox_msgs::NavPVT::FIX_TYPE_GNSS_DEAD_RECKONING_COMBINED) { + } + else if (last_nav_pvt_.fixType == ublox_msgs::NavPVT::FIX_TYPE_GNSS_DEAD_RECKONING_COMBINED) + { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "GPS and dead reckoning combined"; - } else if (last_nav_pvt_.fixType == - ublox_msgs::NavPVT::FIX_TYPE_TIME_ONLY) { + } + else if (last_nav_pvt_.fixType == ublox_msgs::NavPVT::FIX_TYPE_TIME_ONLY) + { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "Time only fix"; } - + // Check whether differential GNSS available - if (last_nav_pvt_.flags & ublox_msgs::NavPVT::FLAGS_DIFF_SOLN) { + if (last_nav_pvt_.flags & ublox_msgs::NavPVT::FLAGS_DIFF_SOLN) + { stat.message += ", DGNSS"; - } + } // If DGNSS, then update the differential solution status - if (last_nav_pvt_.flags & ublox_msgs::NavPVT::CARRIER_PHASE_FLOAT) { + if (last_nav_pvt_.flags & ublox_msgs::NavPVT::CARRIER_PHASE_FLOAT) + { stat.message += ", FLOAT FIX"; - } else if (last_nav_pvt_.flags & ublox_msgs::NavPVT::CARRIER_PHASE_FIXED) { + } + else if (last_nav_pvt_.flags & ublox_msgs::NavPVT::CARRIER_PHASE_FIXED) + { stat.message += ", RTK FIX"; } // If fix not ok (w/in DOP & Accuracy Masks), raise the diagnostic level - if (!(last_nav_pvt_.flags & ublox_msgs::NavPVT::FLAGS_GNSS_FIX_OK)) { + if (!(last_nav_pvt_.flags & ublox_msgs::NavPVT::FLAGS_GNSS_FIX_OK)) + { stat.level = diagnostic_msgs::DiagnosticStatus::WARN; stat.message += ", fix not ok"; } // Raise diagnostic level to error if no fix - if (last_nav_pvt_.fixType == ublox_msgs::NavPVT::FIX_TYPE_NO_FIX) { + if (last_nav_pvt_.fixType == ublox_msgs::NavPVT::FIX_TYPE_NO_FIX) + { stat.level = diagnostic_msgs::DiagnosticStatus::ERROR; stat.message = "No fix"; } @@ -954,8 +991,9 @@ class UbloxFirmware7Plus : public UbloxFirmware { /** * @brief Implements functions for firmware version 7. */ -class UbloxFirmware7 : public UbloxFirmware7Plus { - public: +class UbloxFirmware7 : public UbloxFirmware7Plus +{ +public: UbloxFirmware7(); /** @@ -977,21 +1015,22 @@ class UbloxFirmware7 : public UbloxFirmware7Plus { */ void subscribe(); - private: - //! Used to configure NMEA (if set_nmea_) - /*! - * Filled from ROS parameters - */ - ublox_msgs::CfgNMEA7 cfg_nmea_; - //! Whether or not to Configure the NMEA settings - bool set_nmea_; +private: + //! Used to configure NMEA (if set_nmea_) + /*! + * Filled from ROS parameters + */ + ublox_msgs::CfgNMEA7 cfg_nmea_; + //! Whether or not to Configure the NMEA settings + bool set_nmea_; }; /** * @brief Implements functions for firmware version 8. */ -class UbloxFirmware8 : public UbloxFirmware7Plus { - public: +class UbloxFirmware8 : public UbloxFirmware7Plus +{ +public: UbloxFirmware8(); /** @@ -1019,7 +1058,7 @@ class UbloxFirmware8 : public UbloxFirmware7Plus { */ void subscribe(); - private: +private: // Set from ROS parameters //! Whether or not to enable the Galileo GNSS bool enable_galileo_; @@ -1040,27 +1079,34 @@ class UbloxFirmware8 : public UbloxFirmware7Plus { * For now it simply re-uses the firmware version 8 class * but allows for future expansion of functionality */ -class UbloxFirmware9 : public UbloxFirmware8 { +class UbloxFirmware9 : public UbloxFirmware8 +{ }; /** * @brief Implements functions for Raw Data products. */ -class RawDataProduct: public virtual ComponentInterface { - public: +class RawDataProduct : public virtual ComponentInterface +{ +public: static constexpr double kRtcmFreqTol = 0.15; static constexpr int kRtcmFreqWindow = 25; /** * @brief Does nothing since there are no Raw Data product specific settings. */ - void getRosParams() {} + void getRosParams() + { + } /** * @brief Does nothing since there are no Raw Data product specific settings. * @return always returns true */ - bool configureUblox() { return true; } + bool configureUblox() + { + return true; + } /** * @brief Subscribe to Raw Data Product messages and set up ROS publishers. @@ -1074,7 +1120,7 @@ class RawDataProduct: public virtual ComponentInterface { */ void initializeRosDiagnostics(); - private: +private: //! Topic diagnostic updaters std::vector > freq_diagnostics_; }; @@ -1083,10 +1129,11 @@ class RawDataProduct: public virtual ComponentInterface { * @brief Implements functions for Automotive Dead Reckoning (ADR) and * Untethered Dead Reckoning (UDR) Devices. */ -class AdrUdrProduct: public virtual ComponentInterface { - public: +class AdrUdrProduct : public virtual ComponentInterface +{ +public: AdrUdrProduct(float protocol_version); - + /** * @brief Get the ADR/UDR parameters. * @@ -1113,34 +1160,36 @@ class AdrUdrProduct: public virtual ComponentInterface { * @brief Initialize the ROS diagnostics for the ADR/UDR device. * @todo unimplemented */ - void initializeRosDiagnostics() { + void initializeRosDiagnostics() + { ROS_WARN("ROS Diagnostics specific to u-blox ADR/UDR devices is %s", "unimplemented. See AdrUdrProduct class in node.h & node.cpp."); } - protected: +protected: //! Whether or not to enable dead reckoning - bool use_adr_; + bool use_adr_; float protocol_version_; - sensor_msgs::Imu imu_; sensor_msgs::TimeReference t_ref_; ublox_msgs::TimTM2 timtm2; - void callbackEsfMEAS(const ublox_msgs::EsfMEAS &m); + void callbackEsfMEAS(const ublox_msgs::EsfMEAS& m); }; /** * @brief Implements functions for FTS products. Currently unimplemented. * @todo Unimplemented. */ -class FtsProduct: public virtual ComponentInterface { +class FtsProduct : public virtual ComponentInterface +{ /** * @brief Get the FTS parameters. * @todo Currently unimplemented. */ - void getRosParams() { + void getRosParams() + { ROS_WARN("Functionality specific to u-blox FTS devices is %s", "unimplemented. See FtsProduct class in node.h & node.cpp."); } @@ -1149,27 +1198,35 @@ class FtsProduct: public virtual ComponentInterface { * @brief Configure FTS settings. * @todo Currently unimplemented. */ - bool configureUblox() { return false; } + bool configureUblox() + { + return false; + } /** * @brief Subscribe to FTS messages. * @todo Currently unimplemented. */ - void subscribe() {} + void subscribe() + { + } /** * @brief Adds diagnostic updaters for FTS status. * @todo Currently unimplemented. */ - void initializeRosDiagnostics() {} + void initializeRosDiagnostics() + { + } }; /** * @brief Implements functions for High Precision GNSS Reference station * devices. */ -class HpgRefProduct: public virtual ComponentInterface { - public: +class HpgRefProduct : public virtual ComponentInterface +{ +public: /** * @brief Get the ROS parameters specific to the Reference Station * configuration. @@ -1213,7 +1270,7 @@ class HpgRefProduct: public virtual ComponentInterface { */ void callbackNavSvIn(ublox_msgs::NavSVIN m); - protected: +protected: /** * @brief Update the TMODE3 diagnostics. * @@ -1265,20 +1322,22 @@ class HpgRefProduct: public virtual ComponentInterface { float sv_in_acc_lim_; //! Status of device time mode - enum { - INIT, //!< Initialization mode (before configuration) - FIXED, //!< Fixed mode (should switch to time mode almost immediately) - DISABLED, //!< Time mode disabled - SURVEY_IN, //!< Survey-In mode - TIME //!< Time mode, after survey-in or after configuring fixed mode + enum + { + INIT, //!< Initialization mode (before configuration) + FIXED, //!< Fixed mode (should switch to time mode almost immediately) + DISABLED, //!< Time mode disabled + SURVEY_IN, //!< Survey-In mode + TIME //!< Time mode, after survey-in or after configuring fixed mode } mode_; }; /** * @brief Implements functions for High Precision GNSS Rover devices. */ -class HpgRovProduct: public virtual ComponentInterface { - public: +class HpgRovProduct : public virtual ComponentInterface +{ +public: // Constants for diagnostic updater //! Diagnostic updater: RTCM topic frequency min [Hz] constexpr static double kRtcmFreqMin = 1; @@ -1314,21 +1373,19 @@ class HpgRovProduct: public virtual ComponentInterface { */ void initializeRosDiagnostics(); - protected: +protected: /** * @brief Update the rover diagnostics, including the carrier phase solution * status (float or fixed). */ - void carrierPhaseDiagnostics( - diagnostic_updater::DiagnosticStatusWrapper& stat); + void carrierPhaseDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat); /** * @brief Set the last received message and call rover diagnostic updater * * @details Publish received NavRELPOSNED messages if enabled */ - void callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED &m); - + void callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED& m); //! Last relative position (used for diagnostic updater) ublox_msgs::NavRELPOSNED last_rel_pos_; @@ -1341,15 +1398,15 @@ class HpgRovProduct: public virtual ComponentInterface { UbloxTopicDiagnostic freq_rtcm_; }; -class HpPosRecProduct: public virtual HpgRefProduct { - public: +class HpPosRecProduct : public virtual HpgRefProduct +{ +public: /** * @brief Subscribe to Rover messages, such as NavRELPOSNED. */ void subscribe(); - protected: - +protected: /** * @brief Publish a sensor_msgs/NavSatFix message upon receiving a HPPOSLLH UBX message */ @@ -1360,7 +1417,7 @@ class HpPosRecProduct: public virtual HpgRefProduct { * * @details Publish received NavRELPOSNED messages if enabled */ - void callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m); + void callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9& m); sensor_msgs::Imu imu_; @@ -1372,18 +1429,19 @@ class HpPosRecProduct: public virtual HpgRefProduct { * @brief Implements functions for Time Sync products. * @todo partially implemented */ -class TimProduct: public virtual ComponentInterface { +class TimProduct : public virtual ComponentInterface +{ /** * @brief Get the Time Sync parameters. * @todo Currently unimplemented. */ - void getRosParams(); - + void getRosParams(); + /** * @brief Configure Time Sync settings. * @todo Currently unimplemented. */ - bool configureUblox(); + bool configureUblox(); /** * @brief Subscribe to Time Sync messages. @@ -1398,16 +1456,16 @@ class TimProduct: public virtual ComponentInterface { */ void initializeRosDiagnostics(); - protected: +protected: /** - * @brief + * @brief * @details Publish recieved TimTM2 messages if enabled */ - void callbackTimTM2(const ublox_msgs::TimTM2 &m); - + void callbackTimTM2(const ublox_msgs::TimTM2& m); + sensor_msgs::TimeReference t_ref_; }; -} +} // namespace ublox_node #endif diff --git a/ublox_gps/include/ublox_gps/raw_data_pa.h b/ublox_gps/include/ublox_gps/raw_data_pa.h index 95d71ac9..8d1c15c6 100644 --- a/ublox_gps/include/ublox_gps/raw_data_pa.h +++ b/ublox_gps/include/ublox_gps/raw_data_pa.h @@ -32,7 +32,6 @@ // the data as ros messages. This is used by our group to also evaluate the // measured data with the rtklib. - #ifndef UBLOX_RAW_DATA_PA_H #define UBLOX_RAW_DATA_PA_H @@ -52,93 +51,91 @@ * This namespace is for the ROS u-blox node and handles anything regarding * ROS parameters, message passing, diagnostics, etc. */ -namespace ublox_node { - +namespace ublox_node +{ /** * @brief Implements functions for raw data stream. */ -class RawDataStreamPa { - public: - - - /** - * @brief Constructor. - * Initialises variables and the nodehandle. - */ - RawDataStreamPa(bool is_ros_subscriber = false); - - /** - * @brief Get the raw data stream parameters. - */ - void getRosParams(void); - - /** - * @brief Returns the if raw data streaming is enabled. - */ - bool isEnabled(void); - - /** - * @brief Initializes raw data streams - * If storing to file is enabled, the filename is created and the - * corresponding filedescriptor will be opened. - * If publishing ros messages is enabled, an empty msg will be published. - * (This will implicitly create the publisher) - */ - void initialize(void); - - /** - * @brief Callback function which handles raw data. - * @param data the buffer of u-blox messages to process - * @param size the size of the buffer - */ - void ubloxCallback(const unsigned char* data, - const std::size_t size); - - /** - * @brief Callback function which handles raw data. - * @param msg ros message - */ - void msgCallback(const std_msgs::UInt8MultiArray::ConstPtr& msg); - - private: - /** - * @brief Converts a string into an uint8 multibyte array - */ - std_msgs::UInt8MultiArray str2uint8(const std::string str); - - /** - * @brief Publishes data stream as ros message - * @param str raw data stream as string - */ - void publishMsg(const std::string str); - - /** - * @brief Stores data to given file - * @param str raw data stream as string - */ - void saveToFile(const std::string str); - - //! Directoy name for storing raw data - std::string file_dir_; - //! Filename for storing raw data - std::string file_name_; - //! Handle for file access - std::ofstream file_handle_; - - //! Flag for publishing raw data - bool flag_publish_; - - //! Internal flag - //! true : subscribing to ros messages and storing those to file - //! false: publishing ros messages and/or storing to file - bool is_ros_subscriber_; - - //! ROS private node handle (for params and publisher) - ros::NodeHandle pnh_; - //! ROS node handle (only for subscriber) - ros::NodeHandle nh_; +class RawDataStreamPa +{ +public: + /** + * @brief Constructor. + * Initialises variables and the nodehandle. + */ + RawDataStreamPa(bool is_ros_subscriber = false); + + /** + * @brief Get the raw data stream parameters. + */ + void getRosParams(void); + + /** + * @brief Returns the if raw data streaming is enabled. + */ + bool isEnabled(void); + + /** + * @brief Initializes raw data streams + * If storing to file is enabled, the filename is created and the + * corresponding filedescriptor will be opened. + * If publishing ros messages is enabled, an empty msg will be published. + * (This will implicitly create the publisher) + */ + void initialize(void); + + /** + * @brief Callback function which handles raw data. + * @param data the buffer of u-blox messages to process + * @param size the size of the buffer + */ + void ubloxCallback(const unsigned char* data, const std::size_t size); + + /** + * @brief Callback function which handles raw data. + * @param msg ros message + */ + void msgCallback(const std_msgs::UInt8MultiArray::ConstPtr& msg); + +private: + /** + * @brief Converts a string into an uint8 multibyte array + */ + std_msgs::UInt8MultiArray str2uint8(const std::string str); + + /** + * @brief Publishes data stream as ros message + * @param str raw data stream as string + */ + void publishMsg(const std::string str); + + /** + * @brief Stores data to given file + * @param str raw data stream as string + */ + void saveToFile(const std::string str); + + //! Directoy name for storing raw data + std::string file_dir_; + //! Filename for storing raw data + std::string file_name_; + //! Handle for file access + std::ofstream file_handle_; + + //! Flag for publishing raw data + bool flag_publish_; + + //! Internal flag + //! true : subscribing to ros messages and storing those to file + //! false: publishing ros messages and/or storing to file + bool is_ros_subscriber_; + + //! ROS private node handle (for params and publisher) + ros::NodeHandle pnh_; + //! ROS node handle (only for subscriber) + ros::NodeHandle nh_; }; -} +} // namespace ublox_node #endif diff --git a/ublox_gps/include/ublox_gps/utils.h b/ublox_gps/include/ublox_gps/utils.h index ee649eae..aa8468b1 100644 --- a/ublox_gps/include/ublox_gps/utils.h +++ b/ublox_gps/include/ublox_gps/utils.h @@ -6,24 +6,25 @@ #include "ublox_msgs/NavPVT.h" extern "C" { - #include "ublox_gps/mkgmtime.h" +#include "ublox_gps/mkgmtime.h" } /** * @brief Convert date/time to UTC time in seconds. */ -template -long toUtcSeconds(const NavPVT& msg) { +template +long toUtcSeconds(const NavPVT& msg) +{ // Create TM struct for mkgmtime - struct tm time = {0}; - time.tm_year = msg.year - 1900; - time.tm_mon = msg.month - 1; + struct tm time = { 0 }; + time.tm_year = msg.year - 1900; + time.tm_mon = msg.month - 1; time.tm_mday = msg.day; - time.tm_hour = msg.hour; - time.tm_min = msg.min; + time.tm_hour = msg.hour; + time.tm_min = msg.min; time.tm_sec = msg.sec; // C++ STL doesn't have a mkgmtime (though other libraries do) - // STL mktime converts date/time to seconds in local time + // STL mktime converts date/time to seconds in local time // A modified version of code external library is used for mkgmtime return mkgmtime(&time); } diff --git a/ublox_gps/include/ublox_gps/worker.h b/ublox_gps/include/ublox_gps/worker.h index 6a9bef52..b00aea2f 100644 --- a/ublox_gps/include/ublox_gps/worker.h +++ b/ublox_gps/include/ublox_gps/worker.h @@ -14,9 +14,9 @@ // 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 +// 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 BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; @@ -32,15 +32,18 @@ #include #include -namespace ublox_gps { - +namespace ublox_gps +{ /** * @brief Handles I/O reading and writing. */ -class Worker { - public: +class Worker +{ +public: typedef boost::function Callback; - virtual ~Worker() {} + virtual ~Worker() + { + } /** * @brief Set the callback function for received messages. @@ -60,7 +63,7 @@ class Worker { * @param size the size of the buffer */ virtual bool send(const unsigned char* data, const unsigned int size) = 0; - + /** * @brief Wait for an incoming message. * @param timeout the maximum time to wait. diff --git a/ublox_gps/src/gps.cpp b/ublox_gps/src/gps.cpp index 60cd41fb..8c3ea0de 100644 --- a/ublox_gps/src/gps.cpp +++ b/ublox_gps/src/gps.cpp @@ -30,44 +30,47 @@ #include #include -namespace ublox_gps { - +namespace ublox_gps +{ using namespace ublox_msgs; //! Sleep time [ms] after setting the baudrate constexpr static int kSetBaudrateSleepMs = 500; -const boost::posix_time::time_duration Gps::default_timeout_ = - boost::posix_time::milliseconds( - static_cast(Gps::kDefaultAckTimeout * 1000)); +const boost::posix_time::time_duration Gps::default_timeout_ = boost::posix_time::milliseconds( + static_cast(Gps::kDefaultAckTimeout * 1000)); -Gps::Gps() : configured_(false), config_on_startup_flag_(true) { - subscribeAcks(); +Gps::Gps() : configured_(false), config_on_startup_flag_(true) +{ + subscribeAcks(); } -Gps::~Gps() { close(); } +Gps::~Gps() +{ + close(); +} -void Gps::setWorker(const boost::shared_ptr& worker) { - if (worker_) return; +void Gps::setWorker(const boost::shared_ptr& worker) +{ + if (worker_) + return; worker_ = worker; - worker_->setCallback(boost::bind(&CallbackHandlers::readCallback, - &callbacks_, _1, _2)); + worker_->setCallback(boost::bind(&CallbackHandlers::readCallback, &callbacks_, _1, _2)); configured_ = static_cast(worker); } -void Gps::subscribeAcks() { +void Gps::subscribeAcks() +{ // Set NACK handler - subscribeId(boost::bind(&Gps::processNack, this, _1), - ublox_msgs::Message::ACK::NACK); + subscribeId(boost::bind(&Gps::processNack, this, _1), ublox_msgs::Message::ACK::NACK); // Set ACK handler - subscribeId(boost::bind(&Gps::processAck, this, _1), - ublox_msgs::Message::ACK::ACK); + subscribeId(boost::bind(&Gps::processAck, this, _1), ublox_msgs::Message::ACK::ACK); // Set UPD-SOS-ACK handler - subscribe( - boost::bind(&Gps::processUpdSosAck, this, _1)); + subscribe(boost::bind(&Gps::processUpdSosAck, this, _1)); } -void Gps::processAck(const ublox_msgs::Ack &m) { +void Gps::processAck(const ublox_msgs::Ack& m) +{ // Process ACK/NACK messages Ack ack; ack.type = ACK; @@ -75,11 +78,11 @@ void Gps::processAck(const ublox_msgs::Ack &m) { ack.msg_id = m.msgID; // store the ack atomically ack_.store(ack, boost::memory_order_seq_cst); - ROS_DEBUG_COND(debug >= 2, "U-blox: received ACK: 0x%02x / 0x%02x", - m.clsID, m.msgID); + ROS_DEBUG_COND(debug >= 2, "U-blox: received ACK: 0x%02x / 0x%02x", m.clsID, m.msgID); } -void Gps::processNack(const ublox_msgs::Ack &m) { +void Gps::processNack(const ublox_msgs::Ack& m) +{ // Process ACK/NACK messages Ack ack; ack.type = NACK; @@ -90,40 +93,41 @@ void Gps::processNack(const ublox_msgs::Ack &m) { ROS_ERROR("U-blox: received NACK: 0x%02x / 0x%02x", m.clsID, m.msgID); } -void Gps::processUpdSosAck(const ublox_msgs::UpdSOS_Ack &m) { - if (m.cmd == UpdSOS_Ack::CMD_BACKUP_CREATE_ACK) { +void Gps::processUpdSosAck(const ublox_msgs::UpdSOS_Ack& m) +{ + if (m.cmd == UpdSOS_Ack::CMD_BACKUP_CREATE_ACK) + { Ack ack; ack.type = (m.response == m.BACKUP_CREATE_ACK) ? ACK : NACK; ack.class_id = m.CLASS_ID; ack.msg_id = m.MESSAGE_ID; // store the ack atomically ack_.store(ack, boost::memory_order_seq_cst); - ROS_DEBUG_COND(ack.type == ACK && debug >= 2, - "U-blox: received UPD SOS Backup ACK"); - if(ack.type == NACK) + ROS_DEBUG_COND(ack.type == ACK && debug >= 2, "U-blox: received UPD SOS Backup ACK"); + if (ack.type == NACK) ROS_ERROR("U-blox: received UPD SOS Backup NACK"); } } -void Gps::initializeSerial(std::string port, unsigned int baudrate, - uint16_t uart_in, uint16_t uart_out) { +void Gps::initializeSerial(std::string port, unsigned int baudrate, uint16_t uart_in, uint16_t uart_out) +{ port_ = port; - boost::shared_ptr io_service( - new boost::asio::io_service); - boost::shared_ptr serial( - new boost::asio::serial_port(*io_service)); + boost::shared_ptr io_service(new boost::asio::io_service); + boost::shared_ptr serial(new boost::asio::serial_port(*io_service)); // open serial port - try { + try + { serial->open(port); - } catch (std::runtime_error& e) { - throw std::runtime_error("U-Blox: Could not open serial port :" - + port + " " + e.what()); + } + catch (std::runtime_error& e) + { + throw std::runtime_error("U-Blox: Could not open serial port :" + port + " " + e.what()); } ROS_INFO("U-Blox: Opened serial port %s", port.c_str()); - - if(BOOST_VERSION < 106600) + + if (BOOST_VERSION < 106600) { // NOTE(Kartik): Set serial port to "raw" mode. This is done in Boost but // until v1.66.0 there was a bug which didn't enable the relevant code, @@ -136,9 +140,9 @@ void Gps::initializeSerial(std::string port, unsigned int baudrate, } // Set the I/O worker - if (worker_) return; - setWorker(boost::shared_ptr( - new AsyncWorker(serial, io_service))); + if (worker_) + return; + setWorker(boost::shared_ptr(new AsyncWorker(serial, io_service))); configured_ = false; @@ -146,62 +150,67 @@ void Gps::initializeSerial(std::string port, unsigned int baudrate, boost::asio::serial_port_base::baud_rate current_baudrate; serial->get_option(current_baudrate); // Incrementally increase the baudrate to the desired value - for (int i = 0; i < sizeof(kBaudrates)/sizeof(kBaudrates[0]); i++) { + for (int i = 0; i < sizeof(kBaudrates) / sizeof(kBaudrates[0]); i++) + { if (current_baudrate.value() == baudrate) break; // Don't step down, unless the desired baudrate is lower - if(current_baudrate.value() > kBaudrates[i] && baudrate > kBaudrates[i]) + if (current_baudrate.value() > kBaudrates[i] && baudrate > kBaudrates[i]) continue; - serial->set_option( - boost::asio::serial_port_base::baud_rate(kBaudrates[i])); - boost::this_thread::sleep( - boost::posix_time::milliseconds(kSetBaudrateSleepMs)); + serial->set_option(boost::asio::serial_port_base::baud_rate(kBaudrates[i])); + boost::this_thread::sleep(boost::posix_time::milliseconds(kSetBaudrateSleepMs)); serial->get_option(current_baudrate); ROS_DEBUG("U-Blox: Set ASIO baudrate to %u", current_baudrate.value()); } - if (config_on_startup_flag_) { + if (config_on_startup_flag_) + { configured_ = configUart1(baudrate, uart_in, uart_out); - if(!configured_ || current_baudrate.value() != baudrate) { + if (!configured_ || current_baudrate.value() != baudrate) + { throw std::runtime_error("Could not configure serial baud rate"); } - } else { + } + else + { configured_ = true; } } -void Gps::resetSerial(std::string port) { - boost::shared_ptr io_service( - new boost::asio::io_service); - boost::shared_ptr serial( - new boost::asio::serial_port(*io_service)); +void Gps::resetSerial(std::string port) +{ + boost::shared_ptr io_service(new boost::asio::io_service); + boost::shared_ptr serial(new boost::asio::serial_port(*io_service)); // open serial port - try { + try + { serial->open(port); - } catch (std::runtime_error& e) { - throw std::runtime_error("U-Blox: Could not open serial port :" - + port + " " + e.what()); + } + catch (std::runtime_error& e) + { + throw std::runtime_error("U-Blox: Could not open serial port :" + port + " " + e.what()); } ROS_INFO("U-Blox: Reset serial port %s", port.c_str()); // Set the I/O worker - if (worker_) return; - setWorker(boost::shared_ptr( - new AsyncWorker(serial, io_service))); + if (worker_) + return; + setWorker(boost::shared_ptr(new AsyncWorker(serial, io_service))); configured_ = false; // Poll UART PRT Config std::vector payload; payload.push_back(CfgPRT::PORT_ID_UART1); - if (!poll(CfgPRT::CLASS_ID, CfgPRT::MESSAGE_ID, payload)) { + if (!poll(CfgPRT::CLASS_ID, CfgPRT::MESSAGE_ID, payload)) + { ROS_ERROR("Resetting Serial Port: Could not poll UART1 CfgPRT"); return; } CfgPRT prt; - if(!read(prt, default_timeout_)) { - ROS_ERROR("Resetting Serial Port: Could not read polled UART1 CfgPRT %s", - "message"); + if (!read(prt, default_timeout_)) + { + ROS_ERROR("Resetting Serial Port: Could not read polled UART1 CfgPRT %s", "message"); return; } @@ -210,81 +219,83 @@ void Gps::resetSerial(std::string port) { configured_ = true; } -void Gps::initializeTcp(std::string host, std::string port) { +void Gps::initializeTcp(std::string host, std::string port) +{ host_ = host; port_ = port; - boost::shared_ptr io_service( - new boost::asio::io_service); + boost::shared_ptr io_service(new boost::asio::io_service); boost::asio::ip::tcp::resolver::iterator endpoint; - try { + try + { boost::asio::ip::tcp::resolver resolver(*io_service); - endpoint = - resolver.resolve(boost::asio::ip::tcp::resolver::query(host, port)); - } catch (std::runtime_error& e) { - throw std::runtime_error("U-Blox: Could not resolve" + host + " " + - port + " " + e.what()); + endpoint = resolver.resolve(boost::asio::ip::tcp::resolver::query(host, port)); + } + catch (std::runtime_error& e) + { + throw std::runtime_error("U-Blox: Could not resolve" + host + " " + port + " " + e.what()); } - boost::shared_ptr socket( - new boost::asio::ip::tcp::socket(*io_service)); + boost::shared_ptr socket(new boost::asio::ip::tcp::socket(*io_service)); - try { + try + { socket->connect(*endpoint); - } catch (std::runtime_error& e) { - throw std::runtime_error("U-Blox: Could not connect to " + - endpoint->host_name() + ":" + - endpoint->service_name() + ": " + e.what()); + } + catch (std::runtime_error& e) + { + throw std::runtime_error("U-Blox: Could not connect to " + endpoint->host_name() + ":" + endpoint->service_name() + + ": " + e.what()); } - ROS_INFO("U-Blox: Connected to %s:%s.", endpoint->host_name().c_str(), - endpoint->service_name().c_str()); + ROS_INFO("U-Blox: Connected to %s:%s.", endpoint->host_name().c_str(), endpoint->service_name().c_str()); - if (worker_) return; - setWorker(boost::shared_ptr( - new AsyncWorker(socket, - io_service))); + if (worker_) + return; + setWorker(boost::shared_ptr(new AsyncWorker(socket, io_service))); } -void Gps::initializeUdp(std::string host, std::string port) { +void Gps::initializeUdp(std::string host, std::string port) +{ host_ = host; port_ = port; - boost::shared_ptr io_service( - new boost::asio::io_service); + boost::shared_ptr io_service(new boost::asio::io_service); boost::asio::ip::udp::resolver::iterator endpoint; - try { + try + { boost::asio::ip::udp::resolver resolver(*io_service); - endpoint = - resolver.resolve(boost::asio::ip::udp::resolver::query(host, port)); - } catch (std::runtime_error& e) { - throw std::runtime_error("U-Blox: Could not resolve" + host + " " + - port + " " + e.what()); + endpoint = resolver.resolve(boost::asio::ip::udp::resolver::query(host, port)); + } + catch (std::runtime_error& e) + { + throw std::runtime_error("U-Blox: Could not resolve" + host + " " + port + " " + e.what()); } - boost::shared_ptr socket( - new boost::asio::ip::udp::socket(*io_service)); + boost::shared_ptr socket(new boost::asio::ip::udp::socket(*io_service)); - try { + try + { socket->connect(*endpoint); - } catch (std::runtime_error& e) { - throw std::runtime_error("U-Blox: Could not connect to " + - endpoint->host_name() + ":" + - endpoint->service_name() + ": " + e.what()); + } + catch (std::runtime_error& e) + { + throw std::runtime_error("U-Blox: Could not connect to " + endpoint->host_name() + ":" + endpoint->service_name() + + ": " + e.what()); } - ROS_INFO("U-Blox: Connected to %s:%s.", endpoint->host_name().c_str(), - endpoint->service_name().c_str()); + ROS_INFO("U-Blox: Connected to %s:%s.", endpoint->host_name().c_str(), endpoint->service_name().c_str()); - if (worker_) return; - setWorker(boost::shared_ptr( - new AsyncWorker(socket, - io_service))); + if (worker_) + return; + setWorker(boost::shared_ptr(new AsyncWorker(socket, io_service))); } -void Gps::close() { - if(save_on_shutdown_) { - if(saveOnShutdown()) +void Gps::close() +{ + if (save_on_shutdown_) + { + if (saveOnShutdown()) ROS_INFO("U-Blox Flash BBR saved"); else ROS_INFO("U-Blox Flash BBR failed to save"); @@ -293,7 +304,8 @@ void Gps::close() { configured_ = false; } -void Gps::reset(const boost::posix_time::time_duration& wait) { +void Gps::reset(const boost::posix_time::time_duration& wait) +{ worker_.reset(); configured_ = false; // sleep because of undefined behavior after I/O reset @@ -304,9 +316,9 @@ void Gps::reset(const boost::posix_time::time_duration& wait) { initializeTcp(host_, port_); } -bool Gps::configReset(uint16_t nav_bbr_mask, uint16_t reset_mode) { - ROS_WARN("Resetting u-blox. If device address changes, %s", - "node must be relaunched."); +bool Gps::configReset(uint16_t nav_bbr_mask, uint16_t reset_mode) +{ + ROS_WARN("Resetting u-blox. If device address changes, %s", "node must be relaunched."); CfgRST rst; rst.navBbrMask = nav_bbr_mask; @@ -318,8 +330,8 @@ bool Gps::configReset(uint16_t nav_bbr_mask, uint16_t reset_mode) { return true; } -bool Gps::configGnss(CfgGNSS gnss, - const boost::posix_time::time_duration& wait) { +bool Gps::configGnss(CfgGNSS gnss, const boost::posix_time::time_duration& wait) +{ // Configure the GNSS settingshttps://mail.google.com/mail/u/0/#inbox ROS_DEBUG("Re-configuring GNSS."); if (!configure(gnss)) @@ -334,12 +346,13 @@ bool Gps::configGnss(CfgGNSS gnss, return isConfigured(); } -bool Gps::saveOnShutdown() { +bool Gps::saveOnShutdown() +{ // Command the receiver to stop CfgRST rst; rst.navBbrMask = rst.NAV_BBR_HOT_START; rst.resetMode = rst.RESET_MODE_GNSS_STOP; - if(!configure(rst)) + if (!configure(rst)) return false; // Command saving the contents of BBR to flash memory // And wait for UBX-UPD-SOS-ACK @@ -347,7 +360,8 @@ bool Gps::saveOnShutdown() { return configure(backup); } -bool Gps::clearBbr() { +bool Gps::clearBbr() +{ // Command saving the contents of BBR to flash memory // And wait for UBX-UPD-SOS-ACK UpdSOS sos; @@ -355,34 +369,36 @@ bool Gps::clearBbr() { return configure(sos); } -bool Gps::configUart1(unsigned int baudrate, uint16_t in_proto_mask, - uint16_t out_proto_mask) { - if (!worker_) return true; +bool Gps::configUart1(unsigned int baudrate, uint16_t in_proto_mask, uint16_t out_proto_mask) +{ + if (!worker_) + return true; - ROS_DEBUG("Configuring UART1 baud rate: %u, In/Out Protocol: %u / %u", - baudrate, in_proto_mask, out_proto_mask); + ROS_DEBUG("Configuring UART1 baud rate: %u, In/Out Protocol: %u / %u", baudrate, in_proto_mask, out_proto_mask); CfgPRT port; port.portID = CfgPRT::PORT_ID_UART1; port.baudRate = baudrate; - port.mode = CfgPRT::MODE_RESERVED1 | CfgPRT::MODE_CHAR_LEN_8BIT | - CfgPRT::MODE_PARITY_NO | CfgPRT::MODE_STOP_BITS_1; + port.mode = CfgPRT::MODE_RESERVED1 | CfgPRT::MODE_CHAR_LEN_8BIT | CfgPRT::MODE_PARITY_NO | CfgPRT::MODE_STOP_BITS_1; port.inProtoMask = in_proto_mask; port.outProtoMask = out_proto_mask; return configure(port); } -bool Gps::disableUart1(CfgPRT& prev_config) { +bool Gps::disableUart1(CfgPRT& prev_config) +{ ROS_DEBUG("Disabling UART1"); // Poll UART PRT Config std::vector payload; payload.push_back(CfgPRT::PORT_ID_UART1); - if (!poll(CfgPRT::CLASS_ID, CfgPRT::MESSAGE_ID, payload)) { + if (!poll(CfgPRT::CLASS_ID, CfgPRT::MESSAGE_ID, payload)) + { ROS_ERROR("disableUart: Could not poll UART1 CfgPRT"); return false; } - if(!read(prev_config, default_timeout_)) { + if (!read(prev_config, default_timeout_)) + { ROS_ERROR("disableUart: Could not read polled UART1 CfgPRT message"); return false; } @@ -398,13 +414,12 @@ bool Gps::disableUart1(CfgPRT& prev_config) { return configure(port); } -bool Gps::configUsb(uint16_t tx_ready, - uint16_t in_proto_mask, - uint16_t out_proto_mask) { - if (!worker_) return true; +bool Gps::configUsb(uint16_t tx_ready, uint16_t in_proto_mask, uint16_t out_proto_mask) +{ + if (!worker_) + return true; - ROS_DEBUG("Configuring USB tx_ready: %u, In/Out Protocol: %u / %u", - tx_ready, in_proto_mask, out_proto_mask); + ROS_DEBUG("Configuring USB tx_ready: %u, In/Out Protocol: %u / %u", tx_ready, in_proto_mask, out_proto_mask); CfgPRT port; port.portID = CfgPRT::PORT_ID_USB; @@ -414,9 +429,9 @@ bool Gps::configUsb(uint16_t tx_ready, return configure(port); } -bool Gps::configRate(uint16_t meas_rate, uint16_t nav_rate) { - ROS_DEBUG("Configuring measurement rate to %u ms and nav rate to %u cycles", - meas_rate, nav_rate); +bool Gps::configRate(uint16_t meas_rate, uint16_t nav_rate) +{ + ROS_DEBUG("Configuring measurement rate to %u ms and nav rate to %u cycles", meas_rate, nav_rate); CfgRATE rate; rate.measRate = meas_rate; @@ -425,10 +440,13 @@ bool Gps::configRate(uint16_t meas_rate, uint16_t nav_rate) { return configure(rate); } -bool Gps::configRtcm(std::vector ids, std::vector rates) { - for(size_t i = 0; i < ids.size(); ++i) { +bool Gps::configRtcm(std::vector ids, std::vector rates) +{ + for (size_t i = 0; i < ids.size(); ++i) + { ROS_DEBUG("Setting RTCM %d Rate %u", ids[i], rates[i]); - if(!setRate(ublox_msgs::Class::RTCM, (uint8_t)ids[i], rates[i])) { + if (!setRate(ublox_msgs::Class::RTCM, (uint8_t)ids[i], rates[i])) + { ROS_ERROR("Could not set RTCM %d to rate %u", ids[i], rates[i]); return false; } @@ -436,7 +454,8 @@ bool Gps::configRtcm(std::vector ids, std::vector rates) { return true; } -bool Gps::configSbas(bool enable, uint8_t usage, uint8_t max_sbas) { +bool Gps::configSbas(bool enable, uint8_t usage, uint8_t max_sbas) +{ ROS_DEBUG("Configuring SBAS: usage %u, max_sbas %u", usage, max_sbas); ublox_msgs::CfgSBAS msg; @@ -449,10 +468,11 @@ bool Gps::configSbas(bool enable, uint8_t usage, uint8_t max_sbas) { bool Gps::configTmode3Fixed(bool lla_flag, std::vector arp_position, std::vector arp_position_hp, - float fixed_pos_acc) { - if(arp_position.size() != 3 || arp_position_hp.size() != 3) { - ROS_ERROR("Configuring TMODE3 to Fixed: size of position %s", - "& arp_position_hp args must be 3"); + float fixed_pos_acc) +{ + if (arp_position.size() != 3 || arp_position_hp.size() != 3) + { + ROS_ERROR("Configuring TMODE3 to Fixed: size of position %s", "& arp_position_hp args must be 3"); return false; } @@ -463,12 +483,15 @@ bool Gps::configTmode3Fixed(bool lla_flag, tmode3.flags |= lla_flag ? tmode3.FLAGS_LLA : 0; // Set position - if(lla_flag) { + if (lla_flag) + { // Convert from [deg] to [deg * 1e-7] tmode3.ecefXOrLat = (int)round(arp_position[0] * 1e7); tmode3.ecefYOrLon = (int)round(arp_position[1] * 1e7); tmode3.ecefZOrAlt = (int)round(arp_position[2] * 1e7); - } else { + } + else + { // Convert from m to cm tmode3.ecefXOrLat = (int)round(arp_position[0] * 1e2); tmode3.ecefYOrLon = (int)round(arp_position[1] * 1e2); @@ -482,8 +505,8 @@ bool Gps::configTmode3Fixed(bool lla_flag, return configure(tmode3); } -bool Gps::configTmode3SurveyIn(unsigned int svin_min_dur, - float svin_acc_limit) { +bool Gps::configTmode3SurveyIn(unsigned int svin_min_dur, float svin_acc_limit) +{ CfgTMODE3 tmode3; ROS_DEBUG("Setting TMODE3 to Survey In"); tmode3.flags = tmode3.FLAGS_MODE_SURVEY_IN & tmode3.FLAGS_MODE_MASK; @@ -493,7 +516,8 @@ bool Gps::configTmode3SurveyIn(unsigned int svin_min_dur, return configure(tmode3); } -bool Gps::disableTmode3() { +bool Gps::disableTmode3() +{ ROS_DEBUG("Disabling TMODE3"); CfgTMODE3 tmode3; @@ -501,9 +525,9 @@ bool Gps::disableTmode3() { return configure(tmode3); } -bool Gps::setRate(uint8_t class_id, uint8_t message_id, uint8_t rate) { - ROS_DEBUG_COND(debug >= 2, "Setting rate 0x%02x, 0x%02x, %u", class_id, - message_id, rate); +bool Gps::setRate(uint8_t class_id, uint8_t message_id, uint8_t rate) +{ + ROS_DEBUG_COND(debug >= 2, "Setting rate 0x%02x, 0x%02x, %u", class_id, message_id, rate); ublox_msgs::CfgMSG msg; msg.msgClass = class_id; msg.msgID = message_id; @@ -511,7 +535,8 @@ bool Gps::setRate(uint8_t class_id, uint8_t message_id, uint8_t rate) { return configure(msg); } -bool Gps::setDynamicModel(uint8_t model) { +bool Gps::setDynamicModel(uint8_t model) +{ ROS_DEBUG("Setting dynamic model to %u", model); ublox_msgs::CfgNAV5 msg; @@ -520,7 +545,8 @@ bool Gps::setDynamicModel(uint8_t model) { return configure(msg); } -bool Gps::setFixMode(uint8_t mode) { +bool Gps::setFixMode(uint8_t mode) +{ ROS_DEBUG("Setting fix mode to %u", mode); ublox_msgs::CfgNAV5 msg; @@ -529,7 +555,8 @@ bool Gps::setFixMode(uint8_t mode) { return configure(msg); } -bool Gps::setDeadReckonLimit(uint8_t limit) { +bool Gps::setDeadReckonLimit(uint8_t limit) +{ ROS_DEBUG("Setting DR Limit to %u", limit); ublox_msgs::CfgNAV5 msg; @@ -538,47 +565,53 @@ bool Gps::setDeadReckonLimit(uint8_t limit) { return configure(msg); } -bool Gps::setPpp(bool enable, float protocol_version) { +bool Gps::setPpp(bool enable, float protocol_version) +{ ROS_DEBUG("%s PPP", (enable ? "Enabling" : "Disabling")); ublox_msgs::CfgNAVX5 msg; msg.usePPP = enable; - if(protocol_version >= 18) + if (protocol_version >= 18) msg.version = 2; msg.mask1 = ublox_msgs::CfgNAVX5::MASK1_PPP; return configure(msg); } -bool Gps::setDgnss(uint8_t mode) { +bool Gps::setDgnss(uint8_t mode) +{ CfgDGNSS cfg; ROS_DEBUG("Setting DGNSS mode to %u", mode); cfg.dgnssMode = mode; return configure(cfg); } -bool Gps::setUseAdr(bool enable, float protocol_version) { +bool Gps::setUseAdr(bool enable, float protocol_version) +{ ROS_DEBUG("%s ADR/UDR", (enable ? "Enabling" : "Disabling")); ublox_msgs::CfgNAVX5 msg; msg.useAdr = enable; - - if(protocol_version >= 18) + + if (protocol_version >= 18) msg.version = 2; msg.mask2 = ublox_msgs::CfgNAVX5::MASK2_ADR; return configure(msg); } -bool Gps::sendRtcm(const std::vector& rtcm){ +bool Gps::sendRtcm(const std::vector& rtcm) +{ return worker_->send(rtcm.data(), rtcm.size()); } -bool Gps::sendSpartn(const std::vector& message){ +bool Gps::sendSpartn(const std::vector& message) +{ return worker_->send(message.data(), message.size()); } -bool Gps::poll(uint8_t class_id, uint8_t message_id, - const std::vector& payload) { - if (!worker_) return false; +bool Gps::poll(uint8_t class_id, uint8_t message_id, const std::vector& payload) +{ + if (!worker_) + return false; std::vector out(kWriterSize); ublox::Writer writer(out.data(), out.size()); @@ -589,33 +622,31 @@ bool Gps::poll(uint8_t class_id, uint8_t message_id, return true; } -bool Gps::waitForAcknowledge(const boost::posix_time::time_duration& timeout, - uint8_t class_id, uint8_t msg_id) { - ROS_DEBUG_COND(debug >= 2, "Waiting for ACK 0x%02x / 0x%02x", - class_id, msg_id); - boost::posix_time::ptime wait_until( - boost::posix_time::second_clock::local_time() + timeout); +bool Gps::waitForAcknowledge(const boost::posix_time::time_duration& timeout, uint8_t class_id, uint8_t msg_id) +{ + ROS_DEBUG_COND(debug >= 2, "Waiting for ACK 0x%02x / 0x%02x", class_id, msg_id); + boost::posix_time::ptime wait_until(boost::posix_time::second_clock::local_time() + timeout); Ack ack = ack_.load(boost::memory_order_seq_cst); - while (boost::posix_time::second_clock::local_time() < wait_until - && (ack.class_id != class_id - || ack.msg_id != msg_id - || ack.type == WAIT)) { + while (boost::posix_time::second_clock::local_time() < wait_until && + (ack.class_id != class_id || ack.msg_id != msg_id || ack.type == WAIT)) + { worker_->wait(timeout); ack = ack_.load(boost::memory_order_seq_cst); } - bool result = ack.type == ACK - && ack.class_id == class_id - && ack.msg_id == msg_id; + bool result = ack.type == ACK && ack.class_id == class_id && ack.msg_id == msg_id; return result; } -void Gps::setRawDataCallback(const Worker::Callback& callback) { - if (! worker_) return; +void Gps::setRawDataCallback(const Worker::Callback& callback) +{ + if (!worker_) + return; worker_->setRawDataCallback(callback); } -bool Gps::setUTCtime() { +bool Gps::setUTCtime() +{ ROS_DEBUG("Setting time to UTC time"); ublox_msgs::CfgNAV5 msg; @@ -623,12 +654,13 @@ bool Gps::setUTCtime() { return configure(msg); } -bool Gps::setTimtm2(uint8_t rate) { - ROS_DEBUG("TIM-TM2 send rate on current port set to %u", rate ); +bool Gps::setTimtm2(uint8_t rate) +{ + ROS_DEBUG("TIM-TM2 send rate on current port set to %u", rate); ublox_msgs::CfgMSG msg; msg.msgClass = ublox_msgs::TimTM2::CLASS_ID; msg.msgID = ublox_msgs::TimTM2::MESSAGE_ID; - msg.rate = rate; + msg.rate = rate; return configure(msg); } } // namespace ublox_gps diff --git a/ublox_gps/src/logger_node_pa.cpp b/ublox_gps/src/logger_node_pa.cpp index 5cb4fb91..e34fec99 100644 --- a/ublox_gps/src/logger_node_pa.cpp +++ b/ublox_gps/src/logger_node_pa.cpp @@ -43,15 +43,15 @@ using namespace ublox_node; // Raw Data Stream (feature from TUC-ProAut) // -int main(int argc, char** argv) { +int main(int argc, char** argv) +{ + ros::init(argc, argv, "ublox_logger"); - ros::init(argc, argv, "ublox_logger"); - - RawDataStreamPa node(true); - node.getRosParams(); - node.initialize(); + RawDataStreamPa node(true); + node.getRosParams(); + node.initialize(); - ros::spin(); + ros::spin(); - return 0; + return 0; } diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 09c99cb6..ba4313f1 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -40,69 +40,102 @@ constexpr static int kResetWait = 10; // // ublox_node namespace // -uint8_t ublox_node::modelFromString(const std::string& model) { +uint8_t ublox_node::modelFromString(const std::string& model) +{ std::string lower = model; std::transform(lower.begin(), lower.end(), lower.begin(), ::tolower); - if(lower == "portable") { + if (lower == "portable") + { return ublox_msgs::CfgNAV5::DYN_MODEL_PORTABLE; - } else if(lower == "stationary") { + } + else if (lower == "stationary") + { return ublox_msgs::CfgNAV5::DYN_MODEL_STATIONARY; - } else if(lower == "pedestrian") { + } + else if (lower == "pedestrian") + { return ublox_msgs::CfgNAV5::DYN_MODEL_PEDESTRIAN; - } else if(lower == "automotive") { + } + else if (lower == "automotive") + { return ublox_msgs::CfgNAV5::DYN_MODEL_AUTOMOTIVE; - } else if(lower == "sea") { + } + else if (lower == "sea") + { return ublox_msgs::CfgNAV5::DYN_MODEL_SEA; - } else if(lower == "airborne1") { + } + else if (lower == "airborne1") + { return ublox_msgs::CfgNAV5::DYN_MODEL_AIRBORNE_1G; - } else if(lower == "airborne2") { + } + else if (lower == "airborne2") + { return ublox_msgs::CfgNAV5::DYN_MODEL_AIRBORNE_2G; - } else if(lower == "airborne4") { + } + else if (lower == "airborne4") + { return ublox_msgs::CfgNAV5::DYN_MODEL_AIRBORNE_4G; - } else if(lower == "wristwatch") { + } + else if (lower == "wristwatch") + { return ublox_msgs::CfgNAV5::DYN_MODEL_WRIST_WATCH; - } else if(lower == "bike") { + } + else if (lower == "bike") + { return ublox_msgs::CfgNAV5::DYN_MODEL_BIKE; } - throw std::runtime_error("Invalid settings: " + lower + - " is not a valid dynamic model."); + throw std::runtime_error("Invalid settings: " + lower + " is not a valid dynamic model."); } -uint8_t ublox_node::fixModeFromString(const std::string& mode) { +uint8_t ublox_node::fixModeFromString(const std::string& mode) +{ std::string lower = mode; std::transform(lower.begin(), lower.end(), lower.begin(), ::tolower); - if (lower == "2d") { + if (lower == "2d") + { return ublox_msgs::CfgNAV5::FIX_MODE_2D_ONLY; - } else if (lower == "3d") { + } + else if (lower == "3d") + { return ublox_msgs::CfgNAV5::FIX_MODE_3D_ONLY; - } else if (lower == "auto") { + } + else if (lower == "auto") + { return ublox_msgs::CfgNAV5::FIX_MODE_AUTO; } - throw std::runtime_error("Invalid settings: " + mode + - " is not a valid fix mode."); + throw std::runtime_error("Invalid settings: " + mode + " is not a valid fix mode."); } // // u-blox ROS Node // -UbloxNode::UbloxNode() { +UbloxNode::UbloxNode() +{ initialize(); } -void UbloxNode::addFirmwareInterface() { +void UbloxNode::addFirmwareInterface() +{ int ublox_version; - if (protocol_version_ < 14) { + if (protocol_version_ < 14) + { components_.push_back(ComponentPtr(new UbloxFirmware6)); ublox_version = 6; - } else if (protocol_version_ >= 14 && protocol_version_ <= 15) { + } + else if (protocol_version_ >= 14 && protocol_version_ <= 15) + { components_.push_back(ComponentPtr(new UbloxFirmware7)); ublox_version = 7; - } else if (protocol_version_ > 15 && protocol_version_ <= 23) { + } + else if (protocol_version_ > 15 && protocol_version_ <= 23) + { components_.push_back(ComponentPtr(new UbloxFirmware8)); ublox_version = 8; - } else { + } + else + { components_.push_back(ComponentPtr(new UbloxFirmware9)); ublox_version = 9; } @@ -110,9 +143,8 @@ void UbloxNode::addFirmwareInterface() { ROS_INFO("U-Blox Firmware Version: %d", ublox_version); } - -void UbloxNode::addProductInterface(std::string product_category, - std::string ref_rov) { +void UbloxNode::addProductInterface(std::string product_category, std::string ref_rov) +{ if (product_category.compare("HPG") == 0 && ref_rov.compare("REF") == 0) components_.push_back(ComponentPtr(new HpgRefProduct)); else if (product_category.compare("HPG") == 0 && ref_rov.compare("ROV") == 0) @@ -123,18 +155,19 @@ void UbloxNode::addProductInterface(std::string product_category, components_.push_back(ComponentPtr(new HpPosRecProduct)); else if (product_category.compare("TIM") == 0) components_.push_back(ComponentPtr(new TimProduct)); - else if (product_category.compare("ADR") == 0 || - product_category.compare("UDR") == 0) + else if (product_category.compare("ADR") == 0 || product_category.compare("UDR") == 0) components_.push_back(ComponentPtr(new AdrUdrProduct(protocol_version_))); else if (product_category.compare("FTS") == 0) components_.push_back(ComponentPtr(new FtsProduct)); - else if(product_category.compare("SPG") != 0) + else if (product_category.compare("SPG") != 0) ROS_WARN("Product category %s %s from MonVER message not recognized %s", - product_category.c_str(), ref_rov.c_str(), + product_category.c_str(), + ref_rov.c_str(), "options are HPG REF, HPG ROV, HPG #.#, HDG #.#, TIM, ADR, UDR, FTS, SPG"); } -void UbloxNode::getRosParams() { +void UbloxNode::getRosParams() +{ nh->param("device", device_, std::string("/dev/ttyACM0")); nh->param("frame_id", frame_id, std::string("gps")); @@ -146,65 +179,62 @@ void UbloxNode::getRosParams() { // UART 1 params getRosUint("uart1/baudrate", baudrate_, 9600); - getRosUint("uart1/in", uart_in_, ublox_msgs::CfgPRT::PROTO_UBX - | ublox_msgs::CfgPRT::PROTO_NMEA - | ublox_msgs::CfgPRT::PROTO_RTCM); + getRosUint("uart1/in", + uart_in_, + ublox_msgs::CfgPRT::PROTO_UBX | ublox_msgs::CfgPRT::PROTO_NMEA | ublox_msgs::CfgPRT::PROTO_RTCM); getRosUint("uart1/out", uart_out_, ublox_msgs::CfgPRT::PROTO_UBX); // USB params set_usb_ = false; - if (nh->hasParam("usb/in") || nh->hasParam("usb/out")) { + if (nh->hasParam("usb/in") || nh->hasParam("usb/out")) + { set_usb_ = true; - if(!getRosUint("usb/in", usb_in_)) { - throw std::runtime_error(std::string("usb/out is set, therefore ") + - "usb/in must be set"); + if (!getRosUint("usb/in", usb_in_)) + { + throw std::runtime_error(std::string("usb/out is set, therefore ") + "usb/in must be set"); } - if(!getRosUint("usb/out", usb_out_)) { - throw std::runtime_error(std::string("usb/in is set, therefore ") + - "usb/out must be set"); + if (!getRosUint("usb/out", usb_out_)) + { + throw std::runtime_error(std::string("usb/in is set, therefore ") + "usb/out must be set"); } getRosUint("usb/tx_ready", usb_tx_, 0); } // Measurement rate params - nh->param("rate", rate_, 4.0); // in Hz + nh->param("rate", rate_, 4.0); // in Hz getRosUint("nav_rate", nav_rate, 1); // # of measurement rate cycles // RTCM params - getRosUint("rtcm/ids", rtcm_ids); // RTCM output message IDs + getRosUint("rtcm/ids", rtcm_ids); // RTCM output message IDs getRosUint("rtcm/rates", rtcm_rates); // RTCM output message rates // PPP: Advanced Setting nh->param("enable_ppp", enable_ppp_, false); // SBAS params, only for some devices nh->param("sbas", enable_sbas_, false); - getRosUint("sbas/max", max_sbas_, 0); // Maximum number of SBAS channels + getRosUint("sbas/max", max_sbas_, 0); // Maximum number of SBAS channels getRosUint("sbas/usage", sbas_usage_, 0); nh->param("dynamic_model", dynamic_model_, std::string("portable")); nh->param("fix_mode", fix_mode_, std::string("auto")); - getRosUint("dr_limit", dr_limit_, 0); // Dead reckoning limit + getRosUint("dr_limit", dr_limit_, 0); // Dead reckoning limit if (enable_ppp_) ROS_WARN("Warning: PPP is enabled - this is an expert setting."); checkMin(rate_, 0, "rate"); - if(rtcm_ids.size() != rtcm_rates.size()) - throw std::runtime_error(std::string("Invalid settings: size of rtcm_ids") + - " must match size of rtcm_rates"); + if (rtcm_ids.size() != rtcm_rates.size()) + throw std::runtime_error(std::string("Invalid settings: size of rtcm_ids") + " must match size of rtcm_rates"); dmodel_ = modelFromString(dynamic_model_); fmode_ = fixModeFromString(fix_mode_); nh->param("dat/set", set_dat_, false); - if(set_dat_) { + if (set_dat_) + { std::vector shift, rot; - if (!nh->getParam("dat/majA", cfg_dat_.majA) - || nh->getParam("dat/flat", cfg_dat_.flat) - || nh->getParam("dat/shift", shift) - || nh->getParam("dat/rot", rot) - || nh->getParam("dat/scale", cfg_dat_.scale)) + if (!nh->getParam("dat/majA", cfg_dat_.majA) || nh->getParam("dat/flat", cfg_dat_.flat) || + nh->getParam("dat/shift", shift) || nh->getParam("dat/rot", rot) || nh->getParam("dat/scale", cfg_dat_.scale)) throw std::runtime_error(std::string("dat/set is true, therefore ") + - "dat/majA, dat/flat, dat/shift, dat/rot, & dat/scale must be set"); - if(shift.size() != 3 || rot.size() != 3) - throw std::runtime_error(std::string("size of dat/shift & dat/rot ") + - "must be 3"); + "dat/majA, dat/flat, dat/shift, dat/rot, & dat/scale must be set"); + if (shift.size() != 3 || rot.size() != 3) + throw std::runtime_error(std::string("size of dat/shift & dat/rot ") + "must be 3"); checkRange(cfg_dat_.majA, 6300000.0, 6500000.0, "dat/majA"); checkRange(cfg_dat_.flat, 0.0, 500.0, "dat/flat"); @@ -227,16 +257,18 @@ void UbloxNode::getRosParams() { // activate/deactivate any config nh->param("config_on_startup", config_on_startup_flag_, true); - // raw data stream logging + // raw data stream logging rawDataStreamPa_.getRosParams(); } -void UbloxNode::keepAlive(const ros::TimerEvent& event) { +void UbloxNode::keepAlive(const ros::TimerEvent& event) +{ // Poll version message to keep UDP socket active gps.poll(ublox_msgs::MonVER::CLASS_ID, ublox_msgs::MonVER::MESSAGE_ID); } -void UbloxNode::pollMessages(const ros::TimerEvent& event) { +void UbloxNode::pollMessages(const ros::TimerEvent& event) +{ static std::vector payload(1, 1); if (enabled["aid_alm"]) gps.poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::ALM, payload); @@ -246,12 +278,14 @@ void UbloxNode::pollMessages(const ros::TimerEvent& event) { gps.poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::HUI); payload[0]++; - if (payload[0] > 32) { + if (payload[0] > 32) + { payload[0] = 1; } } -void UbloxNode::printInf(const ublox_msgs::Inf &m, uint8_t id) { +void UbloxNode::printInf(const ublox_msgs::Inf& m, uint8_t id) +{ if (id == ublox_msgs::Message::INF::ERROR) ROS_ERROR_STREAM("INF: " << std::string(m.str.begin(), m.str.end())); else if (id == ublox_msgs::Message::INF::WARNING) @@ -262,7 +296,8 @@ void UbloxNode::printInf(const ublox_msgs::Inf &m, uint8_t id) { ROS_INFO_STREAM("INF: " << std::string(m.str.begin(), m.str.end())); } -void UbloxNode::subscribe() { +void UbloxNode::subscribe() +{ ROS_DEBUG("Subscribing to U-Blox messages"); // subscribe messages nh->param("publish/all", enabled["all"], false); @@ -275,76 +310,62 @@ void UbloxNode::subscribe() { // Nav Messages nh->param("publish/nav/status", enabled["nav_status"], enabled["nav"]); if (enabled["nav_status"]) - gps.subscribe(boost::bind( - publish, _1, "navstatus"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "navstatus"), kSubscribeRate); nh->param("publish/nav/posecef", enabled["nav_posecef"], enabled["nav"]); if (enabled["nav_posecef"]) - gps.subscribe(boost::bind( - publish, _1, "navposecef"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "navposecef"), + kSubscribeRate); nh->param("publish/nav/clock", enabled["nav_clock"], enabled["nav"]); if (enabled["nav_clock"]) - gps.subscribe(boost::bind( - publish, _1, "navclock"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "navclock"), kSubscribeRate); // INF messages nh->param("inf/debug", enabled["inf_debug"], false); if (enabled["inf_debug"]) - gps.subscribeId( - boost::bind(&UbloxNode::printInf, this, _1, - ublox_msgs::Message::INF::DEBUG), - ublox_msgs::Message::INF::DEBUG); + gps.subscribeId(boost::bind(&UbloxNode::printInf, this, _1, ublox_msgs::Message::INF::DEBUG), + ublox_msgs::Message::INF::DEBUG); nh->param("inf/error", enabled["inf_error"], enabled["inf"]); if (enabled["inf_error"]) - gps.subscribeId( - boost::bind(&UbloxNode::printInf, this, _1, - ublox_msgs::Message::INF::ERROR), - ublox_msgs::Message::INF::ERROR); + gps.subscribeId(boost::bind(&UbloxNode::printInf, this, _1, ublox_msgs::Message::INF::ERROR), + ublox_msgs::Message::INF::ERROR); nh->param("inf/notice", enabled["inf_notice"], enabled["inf"]); if (enabled["inf_notice"]) - gps.subscribeId( - boost::bind(&UbloxNode::printInf, this, _1, - ublox_msgs::Message::INF::NOTICE), - ublox_msgs::Message::INF::NOTICE); + gps.subscribeId(boost::bind(&UbloxNode::printInf, this, _1, ublox_msgs::Message::INF::NOTICE), + ublox_msgs::Message::INF::NOTICE); nh->param("inf/test", enabled["inf_test"], enabled["inf"]); if (enabled["inf_test"]) - gps.subscribeId( - boost::bind(&UbloxNode::printInf, this, _1, - ublox_msgs::Message::INF::TEST), - ublox_msgs::Message::INF::TEST); + gps.subscribeId(boost::bind(&UbloxNode::printInf, this, _1, ublox_msgs::Message::INF::TEST), + ublox_msgs::Message::INF::TEST); nh->param("inf/warning", enabled["inf_warning"], enabled["inf"]); if (enabled["inf_warning"]) - gps.subscribeId( - boost::bind(&UbloxNode::printInf, this, _1, - ublox_msgs::Message::INF::WARNING), - ublox_msgs::Message::INF::WARNING); + gps.subscribeId(boost::bind(&UbloxNode::printInf, this, _1, ublox_msgs::Message::INF::WARNING), + ublox_msgs::Message::INF::WARNING); // AID messages nh->param("publish/aid/alm", enabled["aid_alm"], enabled["aid"]); if (enabled["aid_alm"]) - gps.subscribe(boost::bind( - publish, _1, "aidalm"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "aidalm"), kSubscribeRate); nh->param("publish/aid/eph", enabled["aid_eph"], enabled["aid"]); if (enabled["aid_eph"]) - gps.subscribe(boost::bind( - publish, _1, "aideph"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "aideph"), kSubscribeRate); nh->param("publish/aid/hui", enabled["aid_hui"], enabled["aid"]); if (enabled["aid_hui"]) - gps.subscribe(boost::bind( - publish, _1, "aidhui"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "aidhui"), kSubscribeRate); - for(int i = 0; i < components_.size(); i++) + for (int i = 0; i < components_.size(); i++) components_[i]->subscribe(); } -void UbloxNode::initializeRosDiagnostics() { +void UbloxNode::initializeRosDiagnostics() +{ if (!nh->hasParam("diagnostic_period")) nh->setParam("diagnostic_period", kDiagnosticPeriod); @@ -352,61 +373,117 @@ void UbloxNode::initializeRosDiagnostics() { updater->setHardwareID("ublox"); // configure diagnostic updater for frequency - freq_diag.reset(new FixDiagnostic(std::string("fix"), kFixFreqTol, - kFixFreqWindow, kTimeStampStatusMin)); - for(int i = 0; i < components_.size(); i++) + freq_diag.reset(new FixDiagnostic(std::string("fix"), kFixFreqTol, kFixFreqWindow, kTimeStampStatusMin)); + for (int i = 0; i < components_.size(); i++) components_[i]->initializeRosDiagnostics(); } +bool UbloxNode::getVersionInfo(ublox_msgs::GetVersionInfo::Request &req, + ublox_msgs::GetVersionInfo::Response &res) +{ + ublox_msgs::MonVER monVer; + if (!gps.poll(monVer)) + { + throw std::runtime_error("Failed to poll MonVER for GPS version info."); + } + + // convert the ready made info from unsigned char* to const char* + res.software = (const char*)(monVer.swVersion.c_array()); + res.hardware = (const char*)(monVer.hwVersion.c_array()); -void UbloxNode::processMonVer() { + // Convert extension to vector of strings + std::vector extension; + extension.reserve(monVer.extension.size()); + for (std::size_t i = 0; i < monVer.extension.size(); ++i) + { + // Find the end of the string (null character) + unsigned char* end = std::find(monVer.extension[i].field.begin(), monVer.extension[i].field.end(), '\0'); + extension.push_back(std::string(monVer.extension[i].field.begin(), end)); + } + + // Get rest of version info + // Up to 2nd to last line, b.c. last 1-2 lines contain config support info like "GPS;GLO;GAL;BDS" and "SBAS;QZSS" + for (std::size_t i = 0; i < extension.size() - 2; ++i) + { + // parse formatted strings + std::vector strs; + boost::split(strs, extension[i], boost::is_any_of("=")); + if (strs.size() > 1) + { + if (strs[0].compare(std::string("PROTVER")) == 0) + { + // protocol_version < 18 may not include the rest of the version info + // see UbloxNode::processMonVer() for inferred version-info difference + res.protocol = strs[1]; + } + else if (strs[0].compare(std::string("FWVER")) == 0) + { + res.firmware = strs[1]; + } + else if (strs[0].compare(std::string("MOD")) == 0) + { + res.device_model = strs[1]; + } + } + } + return true; +} + +void UbloxNode::processMonVer() +{ ublox_msgs::MonVER monVer; if (!gps.poll(monVer)) throw std::runtime_error("Failed to poll MonVER & set relevant settings"); - ROS_DEBUG("%s, HW VER: %s", monVer.swVersion.c_array(), - monVer.hwVersion.c_array()); + ROS_DEBUG("%s, HW VER: %s", monVer.swVersion.c_array(), monVer.hwVersion.c_array()); // Convert extension to vector of strings std::vector extension; extension.reserve(monVer.extension.size()); - for(std::size_t i = 0; i < monVer.extension.size(); ++i) { + for (std::size_t i = 0; i < monVer.extension.size(); ++i) + { ROS_DEBUG("%s", monVer.extension[i].field.c_array()); // Find the end of the string (null character) - unsigned char* end = std::find(monVer.extension[i].field.begin(), - monVer.extension[i].field.end(), '\0'); + unsigned char* end = std::find(monVer.extension[i].field.begin(), monVer.extension[i].field.end(), '\0'); extension.push_back(std::string(monVer.extension[i].field.begin(), end)); } // Get the protocol version - for(std::size_t i = 0; i < extension.size(); ++i) { + for (std::size_t i = 0; i < extension.size(); ++i) + { std::size_t found = extension[i].find("PROTVER"); - if (found != std::string::npos) { - protocol_version_ = ::atof( - extension[i].substr(8, extension[i].size()-8).c_str()); + if (found != std::string::npos) + { + protocol_version_ = ::atof(extension[i].substr(8, extension[i].size() - 8).c_str()); break; } } if (protocol_version_ == 0) - ROS_WARN("Failed to parse MonVER and determine protocol version. %s", - "Defaulting to firmware version 6."); + ROS_WARN("Failed to parse MonVER and determine protocol version. %s", "Defaulting to firmware version 6."); addFirmwareInterface(); - if(protocol_version_ < 18) { + if (protocol_version_ < 18) + { // Final line contains supported GNSS delimited by ; std::vector strs; - if(extension.size() > 0) - boost::split(strs, extension[extension.size()-1], boost::is_any_of(";")); - for(size_t i = 0; i < strs.size(); i++) + if (extension.size() > 0) + boost::split(strs, extension[extension.size() - 1], boost::is_any_of(";")); + for (size_t i = 0; i < strs.size(); i++) supported.insert(strs[i]); - } else { - for(std::size_t i = 0; i < extension.size(); ++i) { + } + else + { + for (std::size_t i = 0; i < extension.size(); ++i) + { std::vector strs; // Up to 2nd to last line - if(i <= extension.size() - 2) { + if (i <= extension.size() - 2) + { boost::split(strs, extension[i], boost::is_any_of("=")); - if(strs.size() > 1) { - if (strs[0].compare(std::string("FWVER")) == 0) { - if(strs[1].length() > 8) + if (strs.size() > 1) + { + if (strs[0].compare(std::string("FWVER")) == 0) + { + if (strs[1].length() > 8) addProductInterface(strs[1].substr(0, 3), strs[1].substr(8, 10)); else addProductInterface(strs[1].substr(0, 3)); @@ -415,62 +492,66 @@ void UbloxNode::processMonVer() { } } // Last 1-2 lines contain supported GNSS - if(i >= extension.size() - 2) { + if (i >= extension.size() - 2) + { boost::split(strs, extension[i], boost::is_any_of(";")); - for(size_t i = 0; i < strs.size(); i++) + for (size_t i = 0; i < strs.size(); i++) supported.insert(strs[i]); } } } } -bool UbloxNode::configureUblox() { - try { +bool UbloxNode::configureUblox() +{ + try + { if (!gps.isInitialized()) throw std::runtime_error("Failed to initialize."); - if (load_.loadMask != 0) { + if (load_.loadMask != 0) + { ROS_DEBUG("Loading u-blox configuration from memory. %u", load_.loadMask); if (!gps.configure(load_)) - throw std::runtime_error(std::string("Failed to load configuration ") + - "from memory"); - if (load_.loadMask & load_.MASK_IO_PORT) { - ROS_DEBUG("Loaded I/O configuration from memory, resetting serial %s", - "communications."); + throw std::runtime_error(std::string("Failed to load configuration ") + "from memory"); + if (load_.loadMask & load_.MASK_IO_PORT) + { + ROS_DEBUG("Loaded I/O configuration from memory, resetting serial %s", "communications."); boost::posix_time::seconds wait(kResetWait); gps.reset(wait); if (!gps.isConfigured()) throw std::runtime_error(std::string("Failed to reset serial I/O") + - "after loading I/O configurations from device memory."); + "after loading I/O configurations from device memory."); } } - if (config_on_startup_flag_) { - if (set_usb_) { + if (config_on_startup_flag_) + { + if (set_usb_) + { gps.configUsb(usb_tx_, usb_in_, usb_out_); } - if (!gps.configRate(meas_rate, nav_rate)) { + if (!gps.configRate(meas_rate, nav_rate)) + { std::stringstream ss; - ss << "Failed to set measurement rate to " << meas_rate - << "ms and navigation rate to " << nav_rate; + ss << "Failed to set measurement rate to " << meas_rate << "ms and navigation rate to " << nav_rate; throw std::runtime_error(ss.str()); } // If device doesn't have SBAS, will receive NACK (causes exception) - if(supportsGnss("SBAS")) { - if (!gps.configSbas(enable_sbas_, sbas_usage_, max_sbas_)) { - throw std::runtime_error(std::string("Failed to ") + - ((enable_sbas_) ? "enable" : "disable") + - " SBAS."); + if (supportsGnss("SBAS")) + { + if (!gps.configSbas(enable_sbas_, sbas_usage_, max_sbas_)) + { + throw std::runtime_error(std::string("Failed to ") + ((enable_sbas_) ? "enable" : "disable") + " SBAS."); } } if (!gps.setPpp(enable_ppp_, protocol_version_)) - throw std::runtime_error(std::string("Failed to ") + - ((enable_ppp_) ? "enable" : "disable") - + " PPP."); + throw std::runtime_error(std::string("Failed to ") + ((enable_ppp_) ? "enable" : "disable") + " PPP."); if (!gps.setDynamicModel(dmodel_)) throw std::runtime_error("Failed to set model: " + dynamic_model_ + "."); if (!gps.setFixMode(fmode_)) throw std::runtime_error("Failed to set fix mode: " + fix_mode_ + "."); - if (!gps.setDeadReckonLimit(dr_limit_)) { + if (!gps.setDeadReckonLimit(dr_limit_)) + { std::stringstream ss; ss << "Failed to set dead reckoning limit: " << dr_limit_ << "."; throw std::runtime_error(ss.str()); @@ -478,25 +559,29 @@ bool UbloxNode::configureUblox() { if (set_dat_ && !gps.configure(cfg_dat_)) throw std::runtime_error("Failed to set user-defined datum."); // Configure each component - for (int i = 0; i < components_.size(); i++) { - if(!components_[i]->configureUblox()) + for (int i = 0; i < components_.size(); i++) + { + if (!components_[i]->configureUblox()) return false; } } - if (save_.saveMask != 0) { - ROS_DEBUG("Saving the u-blox configuration, mask %u, device %u", - save_.saveMask, save_.deviceMask); - if(!gps.configure(save_)) + if (save_.saveMask != 0) + { + ROS_DEBUG("Saving the u-blox configuration, mask %u, device %u", save_.saveMask, save_.deviceMask); + if (!gps.configure(save_)) ROS_ERROR("u-blox unable to save configuration to non-volatile memory"); } - } catch (std::exception& e) { + } + catch (std::exception& e) + { ROS_FATAL("Error configuring u-blox: %s", e.what()); return false; } return true; } -void UbloxNode::configureInf() { +void UbloxNode::configureInf() +{ ublox_msgs::CfgINF msg; // Subscribe to UBX INF messages ublox_msgs::CfgINF_Block block; @@ -504,8 +589,7 @@ void UbloxNode::configureInf() { // Enable desired INF messages on each UBX port uint8_t mask = (enabled["inf_error"] ? block.INF_MSG_ERROR : 0) | (enabled["inf_warning"] ? block.INF_MSG_WARNING : 0) | - (enabled["inf_notice"] ? block.INF_MSG_NOTICE : 0) | - (enabled["inf_test"] ? block.INF_MSG_TEST : 0) | + (enabled["inf_notice"] ? block.INF_MSG_NOTICE : 0) | (enabled["inf_test"] ? block.INF_MSG_TEST : 0) | (enabled["inf_debug"] ? block.INF_MSG_DEBUG : 0); for (int i = 0; i < block.infMsgMask.size(); i++) block.infMsgMask[i] = mask; @@ -513,7 +597,8 @@ void UbloxNode::configureInf() { msg.blocks.push_back(block); // IF NMEA is enabled - if (uart_in_ & ublox_msgs::CfgPRT::PROTO_NMEA) { + if (uart_in_ & ublox_msgs::CfgPRT::PROTO_NMEA) + { ublox_msgs::CfgINF_Block block; block.protocolID = block.PROTOCOL_ID_NMEA; // Enable desired INF messages on each NMEA port @@ -527,48 +612,57 @@ void UbloxNode::configureInf() { ROS_WARN("Failed to configure INF messages"); } -void UbloxNode::initializeIo() { +void UbloxNode::initializeIo() +{ gps.setConfigOnStartup(config_on_startup_flag_); boost::smatch match; - if (boost::regex_match(device_, match, - boost::regex("(tcp|udp)://(.+):(\\d+)"))) { + if (boost::regex_match(device_, match, boost::regex("(tcp|udp)://(.+):(\\d+)"))) + { std::string proto(match[1]); - if (proto == "tcp") { + if (proto == "tcp") + { std::string host(match[2]); std::string port(match[3]); - ROS_INFO("Connecting to %s://%s:%s ...", proto.c_str(), host.c_str(), - port.c_str()); + ROS_INFO("Connecting to %s://%s:%s ...", proto.c_str(), host.c_str(), port.c_str()); gps.initializeTcp(host, port); - } else if (proto == "udp") { + } + else if (proto == "udp") + { std::string host(match[2]); std::string port(match[3]); - ROS_INFO("Connecting to %s://%s:%s ...", proto.c_str(), host.c_str(), - port.c_str()); + ROS_INFO("Connecting to %s://%s:%s ...", proto.c_str(), host.c_str(), port.c_str()); gps.initializeUdp(host, port); - } else { + } + else + { throw std::runtime_error("Protocol '" + proto + "' is unsupported"); } - } else { + } + else + { gps.initializeSerial(device_, baudrate_, uart_in_, uart_out_); } // raw data stream logging - if (rawDataStreamPa_.isEnabled()) { - gps.setRawDataCallback( - boost::bind(&RawDataStreamPa::ubloxCallback,&rawDataStreamPa_, _1, _2)); + if (rawDataStreamPa_.isEnabled()) + { + gps.setRawDataCallback(boost::bind(&RawDataStreamPa::ubloxCallback, &rawDataStreamPa_, _1, _2)); rawDataStreamPa_.initialize(); } } -void UbloxNode::initialize() { +void UbloxNode::initialize() +{ // Params must be set before initializing IO getRosParams(); initializeIo(); + ros::ServiceServer service = nh->advertiseService("poll_gps_version", &UbloxNode::getVersionInfo, this); // Must process Mon VER before setting firmware/hardware params processMonVer(); - if(protocol_version_ <= 14) { - if(nh->param("raw_data", false)) + if (protocol_version_ <= 14) + { + if (nh->param("raw_data", false)) components_.push_back(ComponentPtr(new RawDataProduct)); } // Must set firmware & hardware params before initializing diagnostics @@ -577,7 +671,8 @@ void UbloxNode::initialize() { // Do this last initializeRosDiagnostics(); - if (configureUblox()) { + if (configureUblox()) + { ROS_INFO("U-Blox configured successfully."); // Subscribe to all U-Blox messages subscribe(); @@ -585,25 +680,24 @@ void UbloxNode::initialize() { configureInf(); ros::Timer keep_alive; - if (device_.substr(0, 6) == "udp://") { + if (device_.substr(0, 6) == "udp://") + { // Setup timer to poll version message to keep UDP socket active - keep_alive = nh->createTimer(ros::Duration(kKeepAlivePeriod), - &UbloxNode::keepAlive, - this); + keep_alive = nh->createTimer(ros::Duration(kKeepAlivePeriod), &UbloxNode::keepAlive, this); } ros::Timer poller; - poller = nh->createTimer(ros::Duration(kPollDuration), - &UbloxNode::pollMessages, - this); + poller = nh->createTimer(ros::Duration(kPollDuration), &UbloxNode::pollMessages, this); poller.start(); ros::spin(); } shutdown(); } -void UbloxNode::shutdown() { - if (gps.isInitialized()) { +void UbloxNode::shutdown() +{ + if (gps.isInitialized()) + { gps.close(); ROS_INFO("Closed connection to %s.", device_.c_str()); } @@ -612,7 +706,8 @@ void UbloxNode::shutdown() { // // U-Blox Firmware (all versions) // -void UbloxFirmware::initializeRosDiagnostics() { +void UbloxFirmware::initializeRosDiagnostics() +{ updater->add("fix", this, &UbloxFirmware::fixDiagnostic); updater->force_update(); } @@ -620,28 +715,32 @@ void UbloxFirmware::initializeRosDiagnostics() { // // U-Blox Firmware Version 6 // -UbloxFirmware6::UbloxFirmware6() {} +UbloxFirmware6::UbloxFirmware6() +{ +} -void UbloxFirmware6::getRosParams() { +void UbloxFirmware6::getRosParams() +{ // Fix Service type, used when publishing fix status messages fix_status_service = sensor_msgs::NavSatStatus::SERVICE_GPS; nh->param("nmea/set", set_nmea_, false); - if (set_nmea_) { + if (set_nmea_) + { bool compat, consider; if (!getRosUint("nmea/version", cfg_nmea_.version)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/version must be set"); + "true, therefore nmea/version must be set"); if (!getRosUint("nmea/num_sv", cfg_nmea_.numSV)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/num_sv must be set"); + "true, therefore nmea/num_sv must be set"); if (!nh->getParam("nmea/compat", compat)) - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/compat must be set"); + throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + + "true, therefore nmea/compat must be set"); if (!nh->getParam("nmea/consider", consider)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/consider must be set"); + "true, therefore nmea/consider must be set"); // set flags cfg_nmea_.flags = compat ? cfg_nmea_.FLAGS_COMPAT : 0; @@ -664,7 +763,8 @@ void UbloxFirmware6::getRosParams() { } } -bool UbloxFirmware6::configureUblox() { +bool UbloxFirmware6::configureUblox() +{ ROS_WARN("ublox_version < 7, ignoring GNSS settings"); if (set_nmea_ && !gps.configure(cfg_nmea_)) @@ -673,7 +773,8 @@ bool UbloxFirmware6::configureUblox() { return true; } -void UbloxFirmware6::subscribe() { +void UbloxFirmware6::subscribe() +{ // Whether or not to publish Nav POS LLH (always subscribes) nh->param("publish/nav/posllh", enabled["nav_posllh"], enabled["nav"]); nh->param("publish/nav/sol", enabled["nav_sol"], enabled["nav"]); @@ -681,56 +782,61 @@ void UbloxFirmware6::subscribe() { // Always subscribes to these messages, but may not publish to ROS topic // Subscribe to Nav POSLLH - gps.subscribe(boost::bind( - &UbloxFirmware6::callbackNavPosLlh, this, _1), kSubscribeRate); + gps.subscribe(boost::bind(&UbloxFirmware6::callbackNavPosLlh, this, _1), kSubscribeRate); // Subscribe to Nav SOL - gps.subscribe(boost::bind( - &UbloxFirmware6::callbackNavSol, this, _1), kSubscribeRate); + gps.subscribe(boost::bind(&UbloxFirmware6::callbackNavSol, this, _1), kSubscribeRate); // Subscribe to Nav VELNED - gps.subscribe(boost::bind( - &UbloxFirmware6::callbackNavVelNed, this, _1), kSubscribeRate); + gps.subscribe(boost::bind(&UbloxFirmware6::callbackNavVelNed, this, _1), kSubscribeRate); // Subscribe to Nav SVINFO nh->param("publish/nav/svinfo", enabled["nav_svinfo"], enabled["nav"]); if (enabled["nav_svinfo"]) - gps.subscribe(boost::bind( - publish, _1, "navsvinfo"), - kNavSvInfoSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "navsvinfo"), + kNavSvInfoSubscribeRate); // Subscribe to Mon HW nh->param("publish/mon_hw", enabled["mon_hw"], enabled["mon"]); if (enabled["mon_hw"]) - gps.subscribe(boost::bind( - publish, _1, "monhw"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "monhw"), kSubscribeRate); } -void UbloxFirmware6::fixDiagnostic( - diagnostic_updater::DiagnosticStatusWrapper& stat) { +void UbloxFirmware6::fixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) +{ // Set the diagnostic level based on the fix status - if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_DEAD_RECKONING_ONLY) { + if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_DEAD_RECKONING_ONLY) + { stat.level = diagnostic_msgs::DiagnosticStatus::WARN; stat.message = "Dead reckoning only"; - } else if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_2D_FIX) { + } + else if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_2D_FIX) + { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "2D fix"; - } else if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_3D_FIX) { + } + else if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_3D_FIX) + { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "3D fix"; - } else if (last_nav_sol_.gpsFix == - ublox_msgs::NavSOL::GPS_GPS_DEAD_RECKONING_COMBINED) { + } + else if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_GPS_DEAD_RECKONING_COMBINED) + { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "GPS and dead reckoning combined"; - } else if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_TIME_ONLY_FIX) { + } + else if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_TIME_ONLY_FIX) + { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "Time fix only"; } // If fix is not ok (within DOP & Accuracy Masks), raise the diagnostic level - if (!(last_nav_sol_.flags & ublox_msgs::NavSOL::FLAGS_GPS_FIX_OK)) { + if (!(last_nav_sol_.flags & ublox_msgs::NavSOL::FLAGS_GPS_FIX_OK)) + { stat.level = diagnostic_msgs::DiagnosticStatus::WARN; stat.message += ", fix not ok"; } // Raise diagnostic level to error if no fix - if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_NO_FIX) { + if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_NO_FIX) + { stat.level = diagnostic_msgs::DiagnosticStatus::ERROR; stat.message = "No fix"; } @@ -746,20 +852,20 @@ void UbloxFirmware6::fixDiagnostic( stat.add("# SVs used", (int)last_nav_sol_.numSV); } -void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m) { - if(enabled["nav_posllh"]) { - static ros::Publisher publisher = - nh->advertise("navposllh", kROSQueueSize); +void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m) +{ + if (enabled["nav_posllh"]) + { + static ros::Publisher publisher = nh->advertise("navposllh", kROSQueueSize); publisher.publish(m); } // Position message - static ros::Publisher fixPublisher = - nh->advertise("fix", kROSQueueSize); + static ros::Publisher fixPublisher = nh->advertise("fix", kROSQueueSize); if (m.iTOW == last_nav_vel_.iTOW) - fix_.header.stamp = velocity_.header.stamp; // use last timestamp + fix_.header.stamp = velocity_.header.stamp; // use last timestamp else - fix_.header.stamp = ros::Time::now(); // new timestamp + fix_.header.stamp = ros::Time::now(); // new timestamp fix_.header.frame_id = frame_id; fix_.latitude = m.lat * 1e-7; @@ -778,8 +884,7 @@ void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m) { fix_.position_covariance[0] = varH; fix_.position_covariance[4] = varH; fix_.position_covariance[8] = varV; - fix_.position_covariance_type = - sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; + fix_.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; fix_.status.service = fix_.status.SERVICE_GPS; fixPublisher.publish(fix_); @@ -789,21 +894,21 @@ void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m) { updater->update(); } -void UbloxFirmware6::callbackNavVelNed(const ublox_msgs::NavVELNED& m) { - if(enabled["nav_velned"]) { - static ros::Publisher publisher = - nh->advertise("navvelned", kROSQueueSize); +void UbloxFirmware6::callbackNavVelNed(const ublox_msgs::NavVELNED& m) +{ + if (enabled["nav_velned"]) + { + static ros::Publisher publisher = nh->advertise("navvelned", kROSQueueSize); publisher.publish(m); } // Example geometry message - static ros::Publisher velocityPublisher = - nh->advertise("fix_velocity", - kROSQueueSize); + static ros::Publisher velocityPublisher = nh->advertise("fix_velocity", + kROSQueueSize); if (m.iTOW == last_nav_pos_.iTOW) - velocity_.header.stamp = fix_.header.stamp; // same time as last navposllh + velocity_.header.stamp = fix_.header.stamp; // same time as last navposllh else - velocity_.header.stamp = ros::Time::now(); // create a new timestamp + velocity_.header.stamp = ros::Time::now(); // create a new timestamp velocity_.header.frame_id = frame_id; // convert to XYZ linear velocity @@ -823,10 +928,11 @@ void UbloxFirmware6::callbackNavVelNed(const ublox_msgs::NavVELNED& m) { last_nav_vel_ = m; } -void UbloxFirmware6::callbackNavSol(const ublox_msgs::NavSOL& m) { - if(enabled["nav_sol"]) { - static ros::Publisher publisher = - nh->advertise("navsol", kROSQueueSize); +void UbloxFirmware6::callbackNavSol(const ublox_msgs::NavSOL& m) +{ + if (enabled["nav_sol"]) + { + static ros::Publisher publisher = nh->advertise("navsol", kROSQueueSize); publisher.publish(m); } last_nav_sol_ = m; @@ -835,9 +941,12 @@ void UbloxFirmware6::callbackNavSol(const ublox_msgs::NavSOL& m) { // // Ublox Firmware Version 7 // -UbloxFirmware7::UbloxFirmware7() {} +UbloxFirmware7::UbloxFirmware7() +{ +} -void UbloxFirmware7::getRosParams() { +void UbloxFirmware7::getRosParams() +{ // // GNSS configuration // @@ -845,53 +954,52 @@ void UbloxFirmware7::getRosParams() { nh->param("gnss/gps", enable_gps_, true); nh->param("gnss/glonass", enable_glonass_, false); nh->param("gnss/qzss", enable_qzss_, false); - getRosUint("gnss/qzss_sig_cfg", qzss_sig_cfg_, - ublox_msgs::CfgGNSS_Block::SIG_CFG_QZSS_L1CA); + getRosUint("gnss/qzss_sig_cfg", qzss_sig_cfg_, ublox_msgs::CfgGNSS_Block::SIG_CFG_QZSS_L1CA); nh->param("gnss/sbas", enable_sbas_, false); - if(enable_gps_ && !supportsGnss("GPS")) + if (enable_gps_ && !supportsGnss("GPS")) ROS_WARN("gnss/gps is true, but GPS GNSS is not supported by this device"); - if(enable_glonass_ && !supportsGnss("GLO")) - ROS_WARN("gnss/glonass is true, but GLONASS is not %s", - "supported by this device"); - if(enable_qzss_ && !supportsGnss("QZSS")) + if (enable_glonass_ && !supportsGnss("GLO")) + ROS_WARN("gnss/glonass is true, but GLONASS is not %s", "supported by this device"); + if (enable_qzss_ && !supportsGnss("QZSS")) ROS_WARN("gnss/qzss is true, but QZSS is not supported by this device"); - if(enable_sbas_ && !supportsGnss("SBAS")) + if (enable_sbas_ && !supportsGnss("SBAS")) ROS_WARN("gnss/sbas is true, but SBAS is not supported by this device"); - if(nh->hasParam("gnss/galileo")) + if (nh->hasParam("gnss/galileo")) ROS_WARN("ublox_version < 8, ignoring Galileo GNSS Settings"); - if(nh->hasParam("gnss/beidou")) + if (nh->hasParam("gnss/beidou")) ROS_WARN("ublox_version < 8, ignoring BeiDou Settings"); - if(nh->hasParam("gnss/imes")) + if (nh->hasParam("gnss/imes")) ROS_WARN("ublox_version < 8, ignoring IMES GNSS Settings"); // Fix Service type, used when publishing fix status messages - fix_status_service = sensor_msgs::NavSatStatus::SERVICE_GPS - + (enable_glonass_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GLONASS; + fix_status_service = sensor_msgs::NavSatStatus::SERVICE_GPS + + (enable_glonass_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GLONASS; // // NMEA Configuration // nh->param("nmea/set", set_nmea_, false); - if (set_nmea_) { + if (set_nmea_) + { bool compat, consider; if (!getRosUint("nmea/version", cfg_nmea_.nmeaVersion)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/version must be set"); + "true, therefore nmea/version must be set"); if (!getRosUint("nmea/num_sv", cfg_nmea_.numSV)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/num_sv must be set"); + "true, therefore nmea/num_sv must be set"); if (!getRosUint("nmea/sv_numbering", cfg_nmea_.svNumbering)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/sv_numbering must be set"); + "true, therefore nmea/sv_numbering must be set"); if (!nh->getParam("nmea/compat", compat)) - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/compat must be set"); + throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + + "true, therefore nmea/compat must be set"); if (!nh->getParam("nmea/consider", consider)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/consider must be set"); + "true, therefore nmea/consider must be set"); // set flags cfg_nmea_.flags = compat ? cfg_nmea_.FLAGS_COMPAT : 0; @@ -925,14 +1033,18 @@ void UbloxFirmware7::getRosParams() { } } -bool UbloxFirmware7::configureUblox() { +bool UbloxFirmware7::configureUblox() +{ /** Configure the GNSS **/ ublox_msgs::CfgGNSS cfgGNSSRead; - if (gps.poll(cfgGNSSRead)) { + if (gps.poll(cfgGNSSRead)) + { ROS_DEBUG("Read GNSS config."); ROS_DEBUG("Num. tracking channels in hardware: %i", cfgGNSSRead.numTrkChHw); ROS_DEBUG("Num. tracking channels to use: %i", cfgGNSSRead.numTrkChUse); - } else { + } + else + { throw std::runtime_error("Failed to read the GNSS config."); } @@ -943,21 +1055,22 @@ bool UbloxFirmware7::configureUblox() { cfgGNSSWrite.msgVer = 0; // configure GLONASS - if(supportsGnss("GLO")) { + if (supportsGnss("GLO")) + { ublox_msgs::CfgGNSS_Block block; block.gnssId = block.GNSS_ID_GLONASS; block.resTrkCh = block.RES_TRK_CH_GLONASS; block.maxTrkCh = block.MAX_TRK_CH_GLONASS; block.flags = enable_glonass_ ? block.SIG_CFG_GLONASS_L1OF : 0; cfgGNSSWrite.blocks.push_back(block); - if (!gps.configure(cfgGNSSWrite)) { - throw std::runtime_error(std::string("Failed to ") + - ((enable_glonass_) ? "enable" : "disable") + - " GLONASS."); + if (!gps.configure(cfgGNSSWrite)) + { + throw std::runtime_error(std::string("Failed to ") + ((enable_glonass_) ? "enable" : "disable") + " GLONASS."); } } - if(supportsGnss("QZSS")) { + if (supportsGnss("QZSS")) + { // configure QZSS ublox_msgs::CfgGNSS_Block block; block.gnssId = block.GNSS_ID_QZSS; @@ -965,14 +1078,14 @@ bool UbloxFirmware7::configureUblox() { block.maxTrkCh = block.MAX_TRK_CH_QZSS; block.flags = enable_qzss_ ? qzss_sig_cfg_ : 0; cfgGNSSWrite.blocks[0] = block; - if (!gps.configure(cfgGNSSWrite)) { - throw std::runtime_error(std::string("Failed to ") + - ((enable_glonass_) ? "enable" : "disable") + - " QZSS."); + if (!gps.configure(cfgGNSSWrite)) + { + throw std::runtime_error(std::string("Failed to ") + ((enable_glonass_) ? "enable" : "disable") + " QZSS."); } } - if(supportsGnss("SBAS")) { + if (supportsGnss("SBAS")) + { // configure SBAS ublox_msgs::CfgGNSS_Block block; block.gnssId = block.GNSS_ID_SBAS; @@ -980,48 +1093,47 @@ bool UbloxFirmware7::configureUblox() { block.maxTrkCh = block.MAX_TRK_CH_SBAS; block.flags = enable_sbas_ ? block.SIG_CFG_SBAS_L1CA : 0; cfgGNSSWrite.blocks[0] = block; - if (!gps.configure(cfgGNSSWrite)) { - throw std::runtime_error(std::string("Failed to ") + - ((enable_sbas_) ? "enable" : "disable") + - " SBAS."); + if (!gps.configure(cfgGNSSWrite)) + { + throw std::runtime_error(std::string("Failed to ") + ((enable_sbas_) ? "enable" : "disable") + " SBAS."); } } - if(set_nmea_ && !gps.configure(cfg_nmea_)) + if (set_nmea_ && !gps.configure(cfg_nmea_)) throw std::runtime_error("Failed to configure NMEA"); return true; } -void UbloxFirmware7::subscribe() { +void UbloxFirmware7::subscribe() +{ // Whether to publish Nav PVT messages to a ROS topic nh->param("publish/nav/pvt", enabled["nav_pvt"], enabled["nav"]); // Subscribe to Nav PVT (always does so since fix information is published // from this) - gps.subscribe(boost::bind( - &UbloxFirmware7Plus::callbackNavPvt, this, _1), - kSubscribeRate); + gps.subscribe(boost::bind(&UbloxFirmware7Plus::callbackNavPvt, this, _1), kSubscribeRate); // Subscribe to Nav SVINFO nh->param("publish/nav/svinfo", enabled["nav_svinfo"], enabled["nav"]); if (enabled["nav_svinfo"]) - gps.subscribe(boost::bind( - publish, _1, "navsvinfo"), - kNavSvInfoSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "navsvinfo"), + kNavSvInfoSubscribeRate); // Subscribe to Mon HW nh->param("publish/mon_hw", enabled["mon_hw"], enabled["mon"]); if (enabled["mon_hw"]) - gps.subscribe(boost::bind( - publish, _1, "monhw"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "monhw"), kSubscribeRate); } // // Ublox Version 8 // -UbloxFirmware8::UbloxFirmware8() {} +UbloxFirmware8::UbloxFirmware8() +{ +} -void UbloxFirmware8::getRosParams() { +void UbloxFirmware8::getRosParams() +{ // UPD SOS configuration nh->param("clear_bbr", clear_bbr_, false); gps.setSaveOnShutdown(nh->param("save_on_shutdown", false)); @@ -1035,59 +1147,54 @@ void UbloxFirmware8::getRosParams() { nh->param("gnss/qzss", enable_qzss_, false); nh->param("gnss/sbas", enable_sbas_, false); // QZSS Signal Configuration - getRosUint("gnss/qzss_sig_cfg", qzss_sig_cfg_, - ublox_msgs::CfgGNSS_Block::SIG_CFG_QZSS_L1CA); + getRosUint("gnss/qzss_sig_cfg", qzss_sig_cfg_, ublox_msgs::CfgGNSS_Block::SIG_CFG_QZSS_L1CA); if (enable_gps_ && !supportsGnss("GPS")) - ROS_WARN("gnss/gps is true, but GPS GNSS is not supported by %s", - "this device"); + ROS_WARN("gnss/gps is true, but GPS GNSS is not supported by %s", "this device"); if (enable_glonass_ && !supportsGnss("GLO")) - ROS_WARN("gnss/glonass is true, but GLONASS is not supported by %s", - "this device"); + ROS_WARN("gnss/glonass is true, but GLONASS is not supported by %s", "this device"); if (enable_galileo_ && !supportsGnss("GAL")) - ROS_WARN("gnss/galileo is true, but Galileo GNSS is not supported %s", - "by this device"); + ROS_WARN("gnss/galileo is true, but Galileo GNSS is not supported %s", "by this device"); if (enable_beidou_ && !supportsGnss("BDS")) - ROS_WARN("gnss/beidou is true, but Beidou GNSS is not supported %s", - "by this device"); + ROS_WARN("gnss/beidou is true, but Beidou GNSS is not supported %s", "by this device"); if (enable_imes_ && !supportsGnss("IMES")) - ROS_WARN("gnss/imes is true, but IMES GNSS is not supported by %s", - "this device"); + ROS_WARN("gnss/imes is true, but IMES GNSS is not supported by %s", "this device"); if (enable_qzss_ && !supportsGnss("QZSS")) ROS_WARN("gnss/qzss is true, but QZSS is not supported by this device"); if (enable_sbas_ && !supportsGnss("SBAS")) ROS_WARN("gnss/sbas is true, but SBAS is not supported by this device"); // Fix Service type, used when publishing fix status messages - fix_status_service = sensor_msgs::NavSatStatus::SERVICE_GPS - + (enable_glonass_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GLONASS - + (enable_beidou_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_COMPASS - + (enable_galileo_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GALILEO; + fix_status_service = sensor_msgs::NavSatStatus::SERVICE_GPS + + (enable_glonass_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GLONASS + + (enable_beidou_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_COMPASS + + (enable_galileo_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GALILEO; // // NMEA Configuration // nh->param("nmea/set", set_nmea_, false); - if (set_nmea_) { + if (set_nmea_) + { bool compat, consider; - cfg_nmea_.version = cfg_nmea_.VERSION; // message version + cfg_nmea_.version = cfg_nmea_.VERSION; // message version // Verify that parameters are set if (!getRosUint("nmea/version", cfg_nmea_.nmeaVersion)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/version must be set"); + "true, therefore nmea/version must be set"); if (!getRosUint("nmea/num_sv", cfg_nmea_.numSV)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/num_sv must be set"); + "true, therefore nmea/num_sv must be set"); if (!getRosUint("nmea/sv_numbering", cfg_nmea_.svNumbering)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/sv_numbering must be set"); + "true, therefore nmea/sv_numbering must be set"); if (!nh->getParam("nmea/compat", compat)) - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/compat must be set"); + throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + + "true, therefore nmea/compat must be set"); if (!nh->getParam("nmea/consider", consider)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/consider must be set"); + "true, therefore nmea/consider must be set"); // set flags cfg_nmea_.flags = compat ? cfg_nmea_.FLAGS_COMPAT : 0; @@ -1132,11 +1239,12 @@ void UbloxFirmware8::getRosParams() { } } - -bool UbloxFirmware8::configureUblox() { - if(clear_bbr_) { +bool UbloxFirmware8::configureUblox() +{ + if (clear_bbr_) + { // clear flash memory - if(!gps.clearBbr()) + if (!gps.clearBbr()) ROS_ERROR("u-blox failed to clear flash memory"); } // @@ -1144,67 +1252,68 @@ bool UbloxFirmware8::configureUblox() { // // First, get the current GNSS configuration ublox_msgs::CfgGNSS cfg_gnss; - if (gps.poll(cfg_gnss)) { + if (gps.poll(cfg_gnss)) + { ROS_DEBUG("Read GNSS config."); ROS_DEBUG("Num. tracking channels in hardware: %i", cfg_gnss.numTrkChHw); ROS_DEBUG("Num. tracking channels to use: %i", cfg_gnss.numTrkChUse); - } else { + } + else + { throw std::runtime_error("Failed to read the GNSS config."); } // Then, check the configuration for each GNSS. If it is different, change it. bool correct = true; - for (int i = 0; i < cfg_gnss.blocks.size(); i++) { + for (int i = 0; i < cfg_gnss.blocks.size(); i++) + { ublox_msgs::CfgGNSS_Block block = cfg_gnss.blocks[i]; - if (block.gnssId == block.GNSS_ID_GPS - && enable_gps_ != (block.flags & block.FLAGS_ENABLE)) { + if (block.gnssId == block.GNSS_ID_GPS && enable_gps_ != (block.flags & block.FLAGS_ENABLE)) + { correct = false; - cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_gps_; + cfg_gnss.blocks[i].flags = (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_gps_; ROS_DEBUG("GPS Configuration is different"); - } else if (block.gnssId == block.GNSS_ID_SBAS - && enable_sbas_ != (block.flags & block.FLAGS_ENABLE)) { + } + else if (block.gnssId == block.GNSS_ID_SBAS && enable_sbas_ != (block.flags & block.FLAGS_ENABLE)) + { correct = false; - cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_sbas_; + cfg_gnss.blocks[i].flags = (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_sbas_; ROS_DEBUG("SBAS Configuration is different"); - } else if (block.gnssId == block.GNSS_ID_GALILEO - && enable_galileo_ != (block.flags & block.FLAGS_ENABLE)) { + } + else if (block.gnssId == block.GNSS_ID_GALILEO && enable_galileo_ != (block.flags & block.FLAGS_ENABLE)) + { correct = false; - cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_galileo_; + cfg_gnss.blocks[i].flags = (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_galileo_; ROS_DEBUG("Galileo GNSS Configuration is different"); - } else if (block.gnssId == block.GNSS_ID_BEIDOU - && enable_beidou_ != (block.flags & block.FLAGS_ENABLE)) { + } + else if (block.gnssId == block.GNSS_ID_BEIDOU && enable_beidou_ != (block.flags & block.FLAGS_ENABLE)) + { correct = false; - cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_beidou_; + cfg_gnss.blocks[i].flags = (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_beidou_; ROS_DEBUG("BeiDou Configuration is different"); - } else if (block.gnssId == block.GNSS_ID_IMES - && enable_imes_ != (block.flags & block.FLAGS_ENABLE)) { + } + else if (block.gnssId == block.GNSS_ID_IMES && enable_imes_ != (block.flags & block.FLAGS_ENABLE)) + { correct = false; - cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_imes_; - } else if (block.gnssId == block.GNSS_ID_QZSS - && (enable_qzss_ != (block.flags & block.FLAGS_ENABLE) - || (enable_qzss_ - && qzss_sig_cfg_ != (block.flags & block.FLAGS_SIG_CFG_MASK)))) { - ROS_DEBUG("QZSS Configuration is different %u, %u", - block.flags & block.FLAGS_ENABLE, - enable_qzss_); + cfg_gnss.blocks[i].flags = (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_imes_; + } + else if (block.gnssId == block.GNSS_ID_QZSS && + (enable_qzss_ != (block.flags & block.FLAGS_ENABLE) || + (enable_qzss_ && qzss_sig_cfg_ != (block.flags & block.FLAGS_SIG_CFG_MASK)))) + { + ROS_DEBUG("QZSS Configuration is different %u, %u", block.flags & block.FLAGS_ENABLE, enable_qzss_); correct = false; ROS_DEBUG("QZSS Configuration: %u", block.flags); - cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_qzss_; + cfg_gnss.blocks[i].flags = (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_qzss_; ROS_DEBUG("QZSS Configuration: %u", cfg_gnss.blocks[i].flags); if (enable_qzss_) // Only change sig cfg if enabling cfg_gnss.blocks[i].flags |= qzss_sig_cfg_; - } else if (block.gnssId == block.GNSS_ID_GLONASS - && enable_glonass_ != (block.flags & block.FLAGS_ENABLE)) { + } + else if (block.gnssId == block.GNSS_ID_GLONASS && enable_glonass_ != (block.flags & block.FLAGS_ENABLE)) + { correct = false; - cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_glonass_; + cfg_gnss.blocks[i].flags = (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_glonass_; ROS_DEBUG("GLONASS Configuration is different"); } } @@ -1214,8 +1323,7 @@ bool UbloxFirmware8::configureUblox() { if (correct) ROS_DEBUG("U-Blox GNSS configuration is correct. GNSS not re-configured."); else if (!gps.configGnss(cfg_gnss, boost::posix_time::seconds(15))) - throw std::runtime_error(std::string("Failed to cold reset device ") + - "after configuring GNSS"); + throw std::runtime_error(std::string("Failed to cold reset device ") + "after configuring GNSS"); // // NMEA config @@ -1226,377 +1334,408 @@ bool UbloxFirmware8::configureUblox() { return true; } -void UbloxFirmware8::subscribe() { +void UbloxFirmware8::subscribe() +{ // Whether to publish Nav PVT messages nh->param("publish/nav/pvt", enabled["nav_pvt"], enabled["nav"]); // Subscribe to Nav PVT - gps.subscribe( - boost::bind(&UbloxFirmware7Plus::callbackNavPvt, this, _1), kSubscribeRate); + gps.subscribe(boost::bind(&UbloxFirmware7Plus::callbackNavPvt, this, _1), kSubscribeRate); // Subscribe to Nav SAT messages nh->param("publish/nav/sat", enabled["nav_sat"], enabled["nav"]); if (enabled["nav_sat"]) - gps.subscribe(boost::bind( - publish, _1, "navsat"), kNavSvInfoSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "navsat"), kNavSvInfoSubscribeRate); // Subscribe to Mon HW nh->param("publish/mon/hw", enabled["mon_hw"], enabled["mon"]); if (enabled["mon_hw"]) - gps.subscribe(boost::bind( - publish, _1, "monhw"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "monhw"), kSubscribeRate); // Subscribe to RTCM messages nh->param("publish/rxm/rtcm", enabled["rxm_rtcm"], enabled["rxm"]); if (enabled["rxm_rtcm"]) - gps.subscribe(boost::bind( - publish, _1, "rxmrtcm"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "rxmrtcm"), kSubscribeRate); } // // Raw Data Products // -void RawDataProduct::subscribe() { +void RawDataProduct::subscribe() +{ // Defaults to true instead of to all nh->param("publish/rxm/all", enabled["rxm"], true); // Subscribe to RXM Raw nh->param("publish/rxm/raw", enabled["rxm_raw"], enabled["rxm"]); if (enabled["rxm_raw"]) - gps.subscribe(boost::bind( - publish, _1, "rxmraw"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "rxmraw"), kSubscribeRate); // Subscribe to RXM SFRB nh->param("publish/rxm/sfrb", enabled["rxm_sfrb"], enabled["rxm"]); if (enabled["rxm_sfrb"]) - gps.subscribe(boost::bind( - publish, _1, "rxmsfrb"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "rxmsfrb"), kSubscribeRate); // Subscribe to RXM EPH nh->param("publish/rxm/eph", enabled["rxm_eph"], enabled["rxm"]); if (enabled["rxm_eph"]) - gps.subscribe(boost::bind( - publish, _1, "rxmeph"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "rxmeph"), kSubscribeRate); // Subscribe to RXM ALM nh->param("publish/rxm/almRaw", enabled["rxm_alm"], enabled["rxm"]); if (enabled["rxm_alm"]) - gps.subscribe(boost::bind( - publish, _1, "rxmalm"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "rxmalm"), kSubscribeRate); } -void RawDataProduct::initializeRosDiagnostics() { +void RawDataProduct::initializeRosDiagnostics() +{ if (enabled["rxm_raw"]) - freq_diagnostics_.push_back(boost::shared_ptr( - new UbloxTopicDiagnostic("rxmraw", kRtcmFreqTol, kRtcmFreqWindow))); + freq_diagnostics_.push_back( + boost::shared_ptr(new UbloxTopicDiagnostic("rxmraw", kRtcmFreqTol, kRtcmFreqWindow))); if (enabled["rxm_sfrb"]) - freq_diagnostics_.push_back(boost::shared_ptr( - new UbloxTopicDiagnostic("rxmsfrb", kRtcmFreqTol, kRtcmFreqWindow))); + freq_diagnostics_.push_back( + boost::shared_ptr(new UbloxTopicDiagnostic("rxmsfrb", kRtcmFreqTol, kRtcmFreqWindow))); if (enabled["rxm_eph"]) - freq_diagnostics_.push_back(boost::shared_ptr( - new UbloxTopicDiagnostic("rxmeph", kRtcmFreqTol, kRtcmFreqWindow))); + freq_diagnostics_.push_back( + boost::shared_ptr(new UbloxTopicDiagnostic("rxmeph", kRtcmFreqTol, kRtcmFreqWindow))); if (enabled["rxm_alm"]) - freq_diagnostics_.push_back(boost::shared_ptr( - new UbloxTopicDiagnostic("rxmalm", kRtcmFreqTol, kRtcmFreqWindow))); + freq_diagnostics_.push_back( + boost::shared_ptr(new UbloxTopicDiagnostic("rxmalm", kRtcmFreqTol, kRtcmFreqWindow))); } -AdrUdrProduct::AdrUdrProduct(float protocol_version) - : protocol_version_(protocol_version) -{} +AdrUdrProduct::AdrUdrProduct(float protocol_version) : protocol_version_(protocol_version) +{ +} // // u-blox ADR devices, partially implemented // -void AdrUdrProduct::getRosParams() { +void AdrUdrProduct::getRosParams() +{ nh->param("use_adr", use_adr_, true); // Check the nav rate float nav_rate_hz = 1000 / (meas_rate * nav_rate); - if(nav_rate_hz != 1) + if (nav_rate_hz != 1) ROS_WARN("Nav Rate recommended to be 1 Hz"); } -bool AdrUdrProduct::configureUblox() { - if(!gps.setUseAdr(use_adr_, protocol_version_)) - throw std::runtime_error(std::string("Failed to ") - + (use_adr_ ? "enable" : "disable") + "use_adr"); +bool AdrUdrProduct::configureUblox() +{ + if (!gps.setUseAdr(use_adr_, protocol_version_)) + throw std::runtime_error(std::string("Failed to ") + (use_adr_ ? "enable" : "disable") + "use_adr"); return true; } -void AdrUdrProduct::subscribe() { +void AdrUdrProduct::subscribe() +{ nh->param("publish/esf/all", enabled["esf"], true); // Subscribe to NAV ATT messages nh->param("publish/nav/att", enabled["nav_att"], enabled["nav"]); if (enabled["nav_att"]) - gps.subscribe(boost::bind( - publish, _1, "navatt"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "navatt"), kSubscribeRate); // Subscribe to ESF INS messages nh->param("publish/esf/ins", enabled["esf_ins"], enabled["esf"]); if (enabled["esf_ins"]) - gps.subscribe(boost::bind( - publish, _1, "esfins"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "esfins"), kSubscribeRate); // Subscribe to ESF Meas messages nh->param("publish/esf/meas", enabled["esf_meas"], enabled["esf"]); if (enabled["esf_meas"]) - gps.subscribe(boost::bind( - publish, _1, "esfmeas"), kSubscribeRate); - // also publish sensor_msgs::Imu - gps.subscribe(boost::bind( - &AdrUdrProduct::callbackEsfMEAS, this, _1), kSubscribeRate); - + gps.subscribe(boost::bind(publish, _1, "esfmeas"), kSubscribeRate); + // also publish sensor_msgs::Imu + gps.subscribe(boost::bind(&AdrUdrProduct::callbackEsfMEAS, this, _1), kSubscribeRate); + // Subscribe to ESF Raw messages nh->param("publish/esf/raw", enabled["esf_raw"], enabled["esf"]); if (enabled["esf_raw"]) - gps.subscribe(boost::bind( - publish, _1, "esfraw"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "esfraw"), kSubscribeRate); // Subscribe to ESF Status messages nh->param("publish/esf/status", enabled["esf_status"], enabled["esf"]); if (enabled["esf_status"]) - gps.subscribe(boost::bind( - publish, _1, "esfstatus"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "esfstatus"), kSubscribeRate); // Subscribe to HNR PVT messages nh->param("publish/hnr/pvt", enabled["hnr_pvt"], true); if (enabled["hnr_pvt"]) - gps.subscribe(boost::bind( - publish, _1, "hnrpvt"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "hnrpvt"), kSubscribeRate); } -void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS &m) { - if (enabled["esf_meas"]) { - static ros::Publisher imu_pub = - nh->advertise("imu_meas", kROSQueueSize); - static ros::Publisher time_ref_pub = - nh->advertise("interrupt_time", kROSQueueSize); - +void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS& m) +{ + if (enabled["esf_meas"]) + { + static ros::Publisher imu_pub = nh->advertise("imu_meas", kROSQueueSize); + static ros::Publisher time_ref_pub = nh->advertise("interrupt_time", kROSQueueSize); + imu_.header.stamp = ros::Time::now(); imu_.header.frame_id = frame_id; - + static const float deg_per_sec = pow(2, -12); static const float m_per_sec_sq = pow(2, -10); static const float deg_c = 1e-2; - + std::vector imu_data = m.data; - for (int i=0; i < imu_data.size(); i++){ - unsigned int data_type = imu_data[i] >> 24; //grab the last six bits of data - int32_t data_value = static_cast(imu_data[i] << 8); //shift to extend sign from 24 to 32 bit integer + for (int i = 0; i < imu_data.size(); i++) + { + unsigned int data_type = imu_data[i] >> 24; // grab the last six bits of data + int32_t data_value = static_cast(imu_data[i] << 8); // shift to extend sign from 24 to 32 bit integer data_value >>= 8; imu_.orientation_covariance[0] = -1; imu_.linear_acceleration_covariance[0] = -1; imu_.angular_velocity_covariance[0] = -1; - if (data_type == 14) { - imu_.angular_velocity.x = data_value * deg_per_sec; - } else if (data_type == 16) { - imu_.linear_acceleration.x = data_value * m_per_sec_sq; - } else if (data_type == 13) { - imu_.angular_velocity.y = data_value * deg_per_sec; - } else if (data_type == 17) { - imu_.linear_acceleration.y = data_value * m_per_sec_sq; - } else if (data_type == 5) { - imu_.angular_velocity.z = data_value * deg_per_sec; - } else if (data_type == 18) { - imu_.linear_acceleration.z = data_value * m_per_sec_sq; - } else if (data_type == 12) { - //ROS_INFO("Temperature in celsius: %f", data_value * deg_c); - } else { + if (data_type == 14) + { + imu_.angular_velocity.x = data_value * deg_per_sec; + } + else if (data_type == 16) + { + imu_.linear_acceleration.x = data_value * m_per_sec_sq; + } + else if (data_type == 13) + { + imu_.angular_velocity.y = data_value * deg_per_sec; + } + else if (data_type == 17) + { + imu_.linear_acceleration.y = data_value * m_per_sec_sq; + } + else if (data_type == 5) + { + imu_.angular_velocity.z = data_value * deg_per_sec; + } + else if (data_type == 18) + { + imu_.linear_acceleration.z = data_value * m_per_sec_sq; + } + else if (data_type == 12) + { + // ROS_INFO("Temperature in celsius: %f", data_value * deg_c); + } + else + { ROS_INFO("data_type: %u", data_type); ROS_INFO("data_value: %u", data_value); } // create time ref message and put in the data - //t_ref_.header.seq = m.risingEdgeCount; - //t_ref_.header.stamp = ros::Time::now(); - //t_ref_.header.frame_id = frame_id; + // t_ref_.header.seq = m.risingEdgeCount; + // t_ref_.header.stamp = ros::Time::now(); + // t_ref_.header.frame_id = frame_id; - //t_ref_.time_ref = ros::Time((m.wnR * 604800 + m.towMsR / 1000), (m.towMsR % 1000) * 1000000 + m.towSubMsR); - - //std::ostringstream src; - //src << "TIM" << int(m.ch); - //t_ref_.source = src.str(); + // t_ref_.time_ref = ros::Time((m.wnR * 604800 + m.towMsR / 1000), (m.towMsR % 1000) * 1000000 + m.towSubMsR); - t_ref_.header.stamp = ros::Time::now(); // create a new timestamp + // std::ostringstream src; + // src << "TIM" << int(m.ch); + // t_ref_.source = src.str(); + + t_ref_.header.stamp = ros::Time::now(); // create a new timestamp t_ref_.header.frame_id = frame_id; - + time_ref_pub.publish(t_ref_); imu_pub.publish(imu_); } } - + updater->force_update(); } // // u-blox High Precision GNSS Reference Station // -void HpgRefProduct::getRosParams() { - if (config_on_startup_flag_) { - if(nav_rate * meas_rate != 1000) +void HpgRefProduct::getRosParams() +{ + if (config_on_startup_flag_) + { + if (nav_rate * meas_rate != 1000) ROS_WARN("For HPG Ref devices, nav_rate should be exactly 1 Hz."); - if(!getRosUint("tmode3", tmode3_)) + if (!getRosUint("tmode3", tmode3_)) throw std::runtime_error("Invalid settings: TMODE3 must be set"); - if(tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_FIXED) { - if(!nh->getParam("arp/position", arp_position_)) - throw std::runtime_error(std::string("Invalid settings: arp/position ") - + "must be set if TMODE3 is fixed"); - if(!getRosInt("arp/position_hp", arp_position_hp_)) - throw std::runtime_error(std::string("Invalid settings: arp/position_hp ") - + "must be set if TMODE3 is fixed"); - if(!nh->getParam("arp/acc", fixed_pos_acc_)) - throw std::runtime_error(std::string("Invalid settings: arp/acc ") - + "must be set if TMODE3 is fixed"); - if(!nh->getParam("arp/lla_flag", lla_flag_)) { - ROS_WARN("arp/lla_flag param not set, assuming ARP coordinates are %s", - "in ECEF"); + if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_FIXED) + { + if (!nh->getParam("arp/position", arp_position_)) + throw std::runtime_error(std::string("Invalid settings: arp/position ") + "must be set if TMODE3 is fixed"); + if (!getRosInt("arp/position_hp", arp_position_hp_)) + throw std::runtime_error(std::string("Invalid settings: arp/position_hp ") + "must be set if TMODE3 is fixed"); + if (!nh->getParam("arp/acc", fixed_pos_acc_)) + throw std::runtime_error(std::string("Invalid settings: arp/acc ") + "must be set if TMODE3 is fixed"); + if (!nh->getParam("arp/lla_flag", lla_flag_)) + { + ROS_WARN("arp/lla_flag param not set, assuming ARP coordinates are %s", "in ECEF"); lla_flag_ = false; } - } else if(tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_SURVEY_IN) { + } + else if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_SURVEY_IN) + { nh->param("sv_in/reset", svin_reset_, true); - if(!getRosUint("sv_in/min_dur", sv_in_min_dur_)) - throw std::runtime_error(std::string("Invalid settings: sv_in/min_dur ") - + "must be set if TMODE3 is survey-in"); - if(!nh->getParam("sv_in/acc_lim", sv_in_acc_lim_)) - throw std::runtime_error(std::string("Invalid settings: sv_in/acc_lim ") - + "must be set if TMODE3 is survey-in"); - } else if(tmode3_ != ublox_msgs::CfgTMODE3::FLAGS_MODE_DISABLED) { - throw std::runtime_error(std::string("tmode3 param invalid. See CfgTMODE3") - + " flag constants for possible values."); + if (!getRosUint("sv_in/min_dur", sv_in_min_dur_)) + throw std::runtime_error(std::string("Invalid settings: sv_in/min_dur ") + + "must be set if TMODE3 is survey-in"); + if (!nh->getParam("sv_in/acc_lim", sv_in_acc_lim_)) + throw std::runtime_error(std::string("Invalid settings: sv_in/acc_lim ") + + "must be set if TMODE3 is survey-in"); + } + else if (tmode3_ != ublox_msgs::CfgTMODE3::FLAGS_MODE_DISABLED) + { + throw std::runtime_error(std::string("tmode3 param invalid. See CfgTMODE3") + + " flag constants for possible values."); } } } -bool HpgRefProduct::configureUblox() { +bool HpgRefProduct::configureUblox() +{ // Configure TMODE3 - if(tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_DISABLED) { - if(!gps.disableTmode3()) + if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_DISABLED) + { + if (!gps.disableTmode3()) throw std::runtime_error("Failed to disable TMODE3."); mode_ = DISABLED; - } else if(tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_FIXED) { - if(!gps.configTmode3Fixed(lla_flag_, arp_position_, arp_position_hp_, - fixed_pos_acc_)) + } + else if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_FIXED) + { + if (!gps.configTmode3Fixed(lla_flag_, arp_position_, arp_position_hp_, fixed_pos_acc_)) throw std::runtime_error("Failed to set TMODE3 to fixed."); - if(!gps.configRtcm(rtcm_ids, rtcm_rates)) + if (!gps.configRtcm(rtcm_ids, rtcm_rates)) throw std::runtime_error("Failed to set RTCM rates"); mode_ = FIXED; - } else if(tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_SURVEY_IN) { - if(!svin_reset_) { + } + else if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_SURVEY_IN) + { + if (!svin_reset_) + { ublox_msgs::NavSVIN nav_svin; - if(!gps.poll(nav_svin)) - throw std::runtime_error(std::string("Failed to poll NavSVIN while") + - " configuring survey-in"); + if (!gps.poll(nav_svin)) + throw std::runtime_error(std::string("Failed to poll NavSVIN while") + " configuring survey-in"); // Don't reset survey-in if it's already active - if(nav_svin.active) { + if (nav_svin.active) + { mode_ = SURVEY_IN; return true; } // Don't reset survey-in if it already has a valid value - if(nav_svin.valid) { + if (nav_svin.valid) + { setTimeMode(); return true; } ublox_msgs::NavPVT nav_pvt; - if(!gps.poll(nav_pvt)) - throw std::runtime_error(std::string("Failed to poll NavPVT while") + - " configuring survey-in"); + if (!gps.poll(nav_pvt)) + throw std::runtime_error(std::string("Failed to poll NavPVT while") + " configuring survey-in"); // Don't reset survey in if in time mode with a good fix - if (nav_pvt.fixType == nav_pvt.FIX_TYPE_TIME_ONLY - && nav_pvt.flags & nav_pvt.FLAGS_GNSS_FIX_OK) { + if (nav_pvt.fixType == nav_pvt.FIX_TYPE_TIME_ONLY && nav_pvt.flags & nav_pvt.FLAGS_GNSS_FIX_OK) + { setTimeMode(); return true; } } // Reset the Survey In // For Survey in, meas rate must be at least 1 Hz - uint16_t meas_rate_temp = meas_rate < 1000 ? meas_rate : 1000; // [ms] + uint16_t meas_rate_temp = meas_rate < 1000 ? meas_rate : 1000; // [ms] // If measurement period isn't a factor of 1000, set to default - if(1000 % meas_rate_temp != 0) + if (1000 % meas_rate_temp != 0) meas_rate_temp = kDefaultMeasPeriod; // Set nav rate to 1 Hz during survey in - if(!gps.configRate(meas_rate_temp, (int) 1000 / meas_rate_temp)) - throw std::runtime_error(std::string("Failed to set nav rate to 1 Hz") + - "before setting TMODE3 to survey-in."); + if (!gps.configRate(meas_rate_temp, (int)1000 / meas_rate_temp)) + throw std::runtime_error(std::string("Failed to set nav rate to 1 Hz") + "before setting TMODE3 to survey-in."); // As recommended in the documentation, first disable, then set to survey in - if(!gps.disableTmode3()) + if (!gps.disableTmode3()) ROS_ERROR("Failed to disable TMODE3 before setting to survey-in."); else mode_ = DISABLED; // Set to Survey in mode - if(!gps.configTmode3SurveyIn(sv_in_min_dur_, sv_in_acc_lim_)) + if (!gps.configTmode3SurveyIn(sv_in_min_dur_, sv_in_acc_lim_)) throw std::runtime_error("Failed to set TMODE3 to survey-in."); mode_ = SURVEY_IN; } return true; } -void HpgRefProduct::subscribe() { +void HpgRefProduct::subscribe() +{ // Whether to publish Nav Survey-In messages nh->param("publish/nav/svin", enabled["nav_svin"], enabled["nav"]); // Subscribe to Nav Survey-In - gps.subscribe(boost::bind( - &HpgRefProduct::callbackNavSvIn, this, _1), kSubscribeRate); + gps.subscribe(boost::bind(&HpgRefProduct::callbackNavSvIn, this, _1), kSubscribeRate); } -void HpgRefProduct::callbackNavSvIn(ublox_msgs::NavSVIN m) { - if(enabled["nav_svin"]) { - static ros::Publisher publisher = - nh->advertise("navsvin", kROSQueueSize); +void HpgRefProduct::callbackNavSvIn(ublox_msgs::NavSVIN m) +{ + if (enabled["nav_svin"]) + { + static ros::Publisher publisher = nh->advertise("navsvin", kROSQueueSize); publisher.publish(m); } last_nav_svin_ = m; - if(!m.active && m.valid && mode_ == SURVEY_IN) { + if (!m.active && m.valid && mode_ == SURVEY_IN) + { setTimeMode(); } updater->update(); } -bool HpgRefProduct::setTimeMode() { +bool HpgRefProduct::setTimeMode() +{ ROS_INFO("Setting mode (internal state) to Time Mode"); mode_ = TIME; // Set the Measurement & nav rate to user config // (survey-in sets nav_rate to 1 Hz regardless of user setting) - if(!gps.configRate(meas_rate, nav_rate)) - ROS_ERROR("Failed to set measurement rate to %d ms %s %d", meas_rate, - "navigation rate to ", nav_rate); + if (!gps.configRate(meas_rate, nav_rate)) + ROS_ERROR("Failed to set measurement rate to %d ms %s %d", meas_rate, "navigation rate to ", nav_rate); // Enable the RTCM out messages - if(!gps.configRtcm(rtcm_ids, rtcm_rates)) { + if (!gps.configRtcm(rtcm_ids, rtcm_rates)) + { ROS_ERROR("Failed to configure RTCM IDs"); return false; } return true; } -void HpgRefProduct::initializeRosDiagnostics() { +void HpgRefProduct::initializeRosDiagnostics() +{ updater->add("TMODE3", this, &HpgRefProduct::tmode3Diagnostics); updater->force_update(); } -void HpgRefProduct::tmode3Diagnostics( - diagnostic_updater::DiagnosticStatusWrapper& stat) { - if (mode_ == INIT) { +void HpgRefProduct::tmode3Diagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat) +{ + if (mode_ == INIT) + { stat.level = diagnostic_msgs::DiagnosticStatus::WARN; stat.message = "Not configured"; - } else if (mode_ == DISABLED){ + } + else if (mode_ == DISABLED) + { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "Disabled"; - } else if (mode_ == SURVEY_IN) { - if (!last_nav_svin_.active && !last_nav_svin_.valid) { + } + else if (mode_ == SURVEY_IN) + { + if (!last_nav_svin_.active && !last_nav_svin_.valid) + { stat.level = diagnostic_msgs::DiagnosticStatus::ERROR; stat.message = "Survey-In inactive and invalid"; - } else if (last_nav_svin_.active && !last_nav_svin_.valid) { + } + else if (last_nav_svin_.active && !last_nav_svin_.valid) + { stat.level = diagnostic_msgs::DiagnosticStatus::WARN; stat.message = "Survey-In active but invalid"; - } else if (!last_nav_svin_.active && last_nav_svin_.valid) { + } + else if (!last_nav_svin_.active && last_nav_svin_.valid) + { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "Survey-In complete"; - } else if (last_nav_svin_.active && last_nav_svin_.valid) { + } + else if (last_nav_svin_.active && last_nav_svin_.valid) + { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "Survey-In active and valid"; } @@ -1611,10 +1750,14 @@ void HpgRefProduct::tmode3Diagnostics( stat.add("Mean Y HP [m]", last_nav_svin_.meanYHP * 1e-4); stat.add("Mean Z HP [m]", last_nav_svin_.meanZHP * 1e-4); stat.add("Mean Accuracy [m]", last_nav_svin_.meanAcc * 1e-4); - } else if(mode_ == FIXED) { + } + else if (mode_ == FIXED) + { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "Fixed Position"; - } else if(mode_ == TIME) { + } + else if (mode_ == TIME) + { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "Time"; } @@ -1623,61 +1766,62 @@ void HpgRefProduct::tmode3Diagnostics( // // U-Blox High Precision GNSS Rover // -void HpgRovProduct::getRosParams() { +void HpgRovProduct::getRosParams() +{ // default to float, see CfgDGNSS message for details - getRosUint("dgnss_mode", dgnss_mode_, - ublox_msgs::CfgDGNSS::DGNSS_MODE_RTK_FIXED); + getRosUint("dgnss_mode", dgnss_mode_, ublox_msgs::CfgDGNSS::DGNSS_MODE_RTK_FIXED); } -bool HpgRovProduct::configureUblox() { +bool HpgRovProduct::configureUblox() +{ // Configure the DGNSS - if(!gps.setDgnss(dgnss_mode_)) + if (!gps.setDgnss(dgnss_mode_)) throw std::runtime_error(std::string("Failed to Configure DGNSS")); return true; } -void HpgRovProduct::subscribe() { +void HpgRovProduct::subscribe() +{ // Whether to publish Nav Relative Position NED nh->param("publish/nav/relposned", enabled["nav_relposned"], enabled["nav"]); // Subscribe to Nav Relative Position NED messages (also updates diagnostics) - gps.subscribe(boost::bind( - &HpgRovProduct::callbackNavRelPosNed, this, _1), kSubscribeRate); + gps.subscribe(boost::bind(&HpgRovProduct::callbackNavRelPosNed, this, _1), kSubscribeRate); } -void HpgRovProduct::initializeRosDiagnostics() { - freq_rtcm_ = UbloxTopicDiagnostic(std::string("rxmrtcm"), - kRtcmFreqMin, kRtcmFreqMax, - kRtcmFreqTol, kRtcmFreqWindow); - updater->add("Carrier Phase Solution", this, - &HpgRovProduct::carrierPhaseDiagnostics); +void HpgRovProduct::initializeRosDiagnostics() +{ + freq_rtcm_ = UbloxTopicDiagnostic(std::string("rxmrtcm"), kRtcmFreqMin, kRtcmFreqMax, kRtcmFreqTol, kRtcmFreqWindow); + updater->add("Carrier Phase Solution", this, &HpgRovProduct::carrierPhaseDiagnostics); updater->force_update(); } -void HpgRovProduct::carrierPhaseDiagnostics( - diagnostic_updater::DiagnosticStatusWrapper& stat) { +void HpgRovProduct::carrierPhaseDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat) +{ uint32_t carr_soln = last_rel_pos_.flags & last_rel_pos_.FLAGS_CARR_SOLN_MASK; stat.add("iTOW", last_rel_pos_.iTOW); if (carr_soln & last_rel_pos_.FLAGS_CARR_SOLN_NONE || - !(last_rel_pos_.flags & last_rel_pos_.FLAGS_DIFF_SOLN && - last_rel_pos_.flags & last_rel_pos_.FLAGS_REL_POS_VALID)) { + !(last_rel_pos_.flags & last_rel_pos_.FLAGS_DIFF_SOLN && last_rel_pos_.flags & last_rel_pos_.FLAGS_REL_POS_VALID)) + { stat.level = diagnostic_msgs::DiagnosticStatus::ERROR; stat.message = "None"; - } else { - if (carr_soln & last_rel_pos_.FLAGS_CARR_SOLN_FLOAT) { + } + else + { + if (carr_soln & last_rel_pos_.FLAGS_CARR_SOLN_FLOAT) + { stat.level = diagnostic_msgs::DiagnosticStatus::WARN; stat.message = "Float"; - } else if (carr_soln & last_rel_pos_.FLAGS_CARR_SOLN_FIXED) { + } + else if (carr_soln & last_rel_pos_.FLAGS_CARR_SOLN_FIXED) + { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "Fixed"; } stat.add("Ref Station ID", last_rel_pos_.refStationId); - double rel_pos_n = (last_rel_pos_.relPosN - + (last_rel_pos_.relPosHPN * 1e-2)) * 1e-2; - double rel_pos_e = (last_rel_pos_.relPosE - + (last_rel_pos_.relPosHPE * 1e-2)) * 1e-2; - double rel_pos_d = (last_rel_pos_.relPosD - + (last_rel_pos_.relPosHPD * 1e-2)) * 1e-2; + double rel_pos_n = (last_rel_pos_.relPosN + (last_rel_pos_.relPosHPN * 1e-2)) * 1e-2; + double rel_pos_e = (last_rel_pos_.relPosE + (last_rel_pos_.relPosHPE * 1e-2)) * 1e-2; + double rel_pos_d = (last_rel_pos_.relPosD + (last_rel_pos_.relPosHPD * 1e-2)) * 1e-2; stat.add("Relative Position N [m]", rel_pos_n); stat.add("Relative Accuracy N [m]", last_rel_pos_.accN * 1e-4); stat.add("Relative Position E [m]", rel_pos_e); @@ -1687,10 +1831,11 @@ void HpgRovProduct::carrierPhaseDiagnostics( } } -void HpgRovProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED &m) { - if (enabled["nav_relposned"]) { - static ros::Publisher publisher = - nh->advertise("navrelposned", kROSQueueSize); +void HpgRovProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED& m) +{ + if (enabled["nav_relposned"]) + { + static ros::Publisher publisher = nh->advertise("navrelposned", kROSQueueSize); publisher.publish(m); } @@ -1701,12 +1846,13 @@ void HpgRovProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED &m) { // // U-Blox High Precision Positioning Receiver // -void HpPosRecProduct::subscribe() { +void HpPosRecProduct::subscribe() +{ // Subscribe to Nav High Precision Position ECEF nh->param("publish/nav/hpposecef", enabled["nav_hpposecef"], enabled["nav"]); if (enabled["nav_hpposecef"]) - gps.subscribe(boost::bind( - publish, _1, "navhpposecef"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "navhpposecef"), + kSubscribeRate); // Whether to publish the NavSatFix info from Nav High Precision Position LLH nh->param("publish/nav/hp_fix", enabled["nav_hpfix"], enabled["nav"]); @@ -1716,30 +1862,31 @@ void HpPosRecProduct::subscribe() { // Subscribe to Nav High Precision Position LLH if (enabled["nav_hpposllh"] || enabled["nav_hpfix"]) - gps.subscribe(boost::bind( - &HpPosRecProduct::callbackNavHpPosLlh, this, _1), kSubscribeRate); + gps.subscribe(boost::bind(&HpPosRecProduct::callbackNavHpPosLlh, this, _1), + kSubscribeRate); // Whether to publish Nav Relative Position NED nh->param("publish/nav/relposned", enabled["nav_relposned"], enabled["nav"]); // Subscribe to Nav Relative Position NED messages (also updates diagnostics) - gps.subscribe(boost::bind( - &HpPosRecProduct::callbackNavRelPosNed, this, _1), kSubscribeRate); + gps.subscribe(boost::bind(&HpPosRecProduct::callbackNavRelPosNed, this, _1), + kSubscribeRate); // Whether to publish the Heading info from Nav Relative Position NED nh->param("publish/nav/heading", enabled["nav_heading"], enabled["nav"]); } -void HpPosRecProduct::callbackNavHpPosLlh(const ublox_msgs::NavHPPOSLLH& m) { - if (enabled["nav_hpposllh"]) { - static ros::Publisher publisher = - nh->advertise("navhpposllh", kROSQueueSize); +void HpPosRecProduct::callbackNavHpPosLlh(const ublox_msgs::NavHPPOSLLH& m) +{ + if (enabled["nav_hpposllh"]) + { + static ros::Publisher publisher = nh->advertise("navhpposllh", kROSQueueSize); publisher.publish(m); } - if (enabled["nav_hpfix"]) { + if (enabled["nav_hpfix"]) + { sensor_msgs::NavSatFix fix_msg; - static ros::Publisher fixPublisher = - nh->advertise("hp_fix", kROSQueueSize); + static ros::Publisher fixPublisher = nh->advertise("hp_fix", kROSQueueSize); fix_msg.header.stamp = ros::Time::now(); fix_msg.header.frame_id = frame_id; @@ -1747,9 +1894,12 @@ void HpPosRecProduct::callbackNavHpPosLlh(const ublox_msgs::NavHPPOSLLH& m) { fix_msg.longitude = m.lon * 1e-7 + m.lonHp * 1e-9; fix_msg.altitude = m.height * 1e-3 + m.heightHp * 1e-4; - if (m.invalid_llh) { + if (m.invalid_llh) + { fix_msg.status.status = fix_msg.status.STATUS_NO_FIX; - } else { + } + else + { fix_msg.status.status = fix_msg.status.STATUS_FIX; } @@ -1760,24 +1910,24 @@ void HpPosRecProduct::callbackNavHpPosLlh(const ublox_msgs::NavHPPOSLLH& m) { fix_msg.position_covariance[0] = varH; fix_msg.position_covariance[4] = varH; fix_msg.position_covariance[8] = varV; - fix_msg.position_covariance_type = - sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; + fix_msg.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; fix_msg.status.service = fix_msg.status.SERVICE_GPS; fixPublisher.publish(fix_msg); } } -void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m) { - if (enabled["nav_relposned"]) { - static ros::Publisher publisher = - nh->advertise("navrelposned", kROSQueueSize); +void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9& m) +{ + if (enabled["nav_relposned"]) + { + static ros::Publisher publisher = nh->advertise("navrelposned", kROSQueueSize); publisher.publish(m); } - if (enabled["nav_heading"]) { - static ros::Publisher imu_pub = - nh->advertise("navheading", kROSQueueSize); + if (enabled["nav_heading"]) + { + static ros::Publisher imu_pub = nh->advertise("navheading", kROSQueueSize); imu_.header.stamp = ros::Time::now(); imu_.header.frame_id = frame_id; @@ -1787,7 +1937,7 @@ void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m) { // Transform angle since ublox is representing heading as NED but ROS uses ENU as convention (REP-103). // Also convert the base-to-rover angle to a robot-to-base angle (consistent with frame_id) - double heading = - (static_cast(m.relPosHeading) * 1e-5 / 180.0 * M_PI) - M_PI_2; + double heading = -(static_cast(m.relPosHeading) * 1e-5 / 180.0 * M_PI) - M_PI_2; tf::Quaternion orientation; orientation.setRPY(0, 0, heading); imu_.orientation.x = orientation[0]; @@ -1813,97 +1963,99 @@ void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m) { // // U-Blox Time Sync Products, partially implemented. // -void TimProduct::getRosParams() { +void TimProduct::getRosParams() +{ } -bool TimProduct::configureUblox() { +bool TimProduct::configureUblox() +{ uint8_t r = 1; // Configure the reciever - if(!gps.setUTCtime()) + if (!gps.setUTCtime()) throw std::runtime_error(std::string("Failed to Configure TIM Product to UTC Time")); - - if(!gps.setTimtm2(r)) + + if (!gps.setTimtm2(r)) throw std::runtime_error(std::string("Failed to Configure TIM Product")); return true; } -void TimProduct::subscribe() { +void TimProduct::subscribe() +{ ROS_INFO("TIM is Enabled: %u", enabled["tim"]); ROS_INFO("TIM-TM2 is Enabled: %u", enabled["tim_tm2"]); // Subscribe to TIM-TM2 messages (Time mark messages) nh->param("publish/tim/tm2", enabled["tim_tm2"], enabled["tim"]); - gps.subscribe(boost::bind( - &TimProduct::callbackTimTM2, this, _1), kSubscribeRate); - + gps.subscribe(boost::bind(&TimProduct::callbackTimTM2, this, _1), kSubscribeRate); + ROS_INFO("Subscribed to TIM-TM2 messages on topic tim/tm2"); - + // Subscribe to SFRBX messages nh->param("publish/rxm/sfrb", enabled["rxm_sfrb"], enabled["rxm"]); if (enabled["rxm_sfrb"]) - gps.subscribe(boost::bind( - publish, _1, "rxmsfrb"), kSubscribeRate); - - // Subscribe to RawX messages - nh->param("publish/rxm/raw", enabled["rxm_raw"], enabled["rxm"]); - if (enabled["rxm_raw"]) - gps.subscribe(boost::bind( - publish, _1, "rxmraw"), kSubscribeRate); -} - -void TimProduct::callbackTimTM2(const ublox_msgs::TimTM2 &m) { - - if (enabled["tim_tm2"]) { - static ros::Publisher publisher = - nh->advertise("timtm2", kROSQueueSize); - static ros::Publisher time_ref_pub = - nh->advertise("interrupt_time", kROSQueueSize); - + gps.subscribe(boost::bind(publish, _1, "rxmsfrb"), kSubscribeRate); + + // Subscribe to RawX messages + nh->param("publish/rxm/raw", enabled["rxm_raw"], enabled["rxm"]); + if (enabled["rxm_raw"]) + gps.subscribe(boost::bind(publish, _1, "rxmraw"), kSubscribeRate); +} + +void TimProduct::callbackTimTM2(const ublox_msgs::TimTM2& m) +{ + if (enabled["tim_tm2"]) + { + static ros::Publisher publisher = nh->advertise("timtm2", kROSQueueSize); + static ros::Publisher time_ref_pub = nh->advertise("interrupt_time", kROSQueueSize); + // create time ref message and put in the data t_ref_.header.seq = m.risingEdgeCount; t_ref_.header.stamp = ros::Time::now(); t_ref_.header.frame_id = frame_id; - t_ref_.time_ref = ros::Time((m.wnR * 604800 + m.towMsR / 1000), (m.towMsR % 1000) * 1000000 + m.towSubMsR); - + t_ref_.time_ref = ros::Time((m.wnR * 604800 + m.towMsR / 1000), (m.towMsR % 1000) * 1000000 + m.towSubMsR); + std::ostringstream src; - src << "TIM" << int(m.ch); + src << "TIM" << int(m.ch); t_ref_.source = src.str(); - t_ref_.header.stamp = ros::Time::now(); // create a new timestamp + t_ref_.header.stamp = ros::Time::now(); // create a new timestamp t_ref_.header.frame_id = frame_id; - + publisher.publish(m); time_ref_pub.publish(t_ref_); } - + updater->force_update(); } -void TimProduct::initializeRosDiagnostics() { +void TimProduct::initializeRosDiagnostics() +{ updater->force_update(); } -void rtcmCallback(const rtcm_msgs::Message::ConstPtr &msg) { +void rtcmCallback(const rtcm_msgs::Message::ConstPtr& msg) +{ gps.sendRtcm(msg->message); } -void spartnCallback(const std_msgs::UInt8MultiArray::ConstPtr &msg) { +void spartnCallback(const std_msgs::UInt8MultiArray::ConstPtr& msg) +{ gps.sendSpartn(msg->data); } -int main(int argc, char** argv) { +int main(int argc, char** argv) +{ ros::init(argc, argv, "ublox_gps"); nh.reset(new ros::NodeHandle("~")); ros::Subscriber subRtcm = nh->subscribe("/rtcm", 10, rtcmCallback); ros::Subscriber subSpartn = nh->subscribe("/spartn", 10, spartnCallback); nh->param("debug", ublox_gps::debug, 1); - if(ublox_gps::debug) { - if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, - ros::console::levels::Debug)) - ros::console::notifyLoggerLevelsChanged(); - + if (ublox_gps::debug) + { + if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) + ros::console::notifyLoggerLevelsChanged(); } UbloxNode node; return 0; diff --git a/ublox_gps/src/raw_data_pa.cpp b/ublox_gps/src/raw_data_pa.cpp index 872fca9a..29973def 100644 --- a/ublox_gps/src/raw_data_pa.cpp +++ b/ublox_gps/src/raw_data_pa.cpp @@ -47,146 +47,169 @@ using namespace ublox_node; // ublox_node namespace // -RawDataStreamPa::RawDataStreamPa(bool is_ros_subscriber) : - pnh_(ros::NodeHandle("~")), - flag_publish_(false), - is_ros_subscriber_(is_ros_subscriber) { - +RawDataStreamPa::RawDataStreamPa(bool is_ros_subscriber) + : pnh_(ros::NodeHandle("~")), flag_publish_(false), is_ros_subscriber_(is_ros_subscriber) +{ } -void RawDataStreamPa::getRosParams() { - - if (is_ros_subscriber_) { - pnh_.param("dir", file_dir_, ""); - } else { - pnh_.param("raw_data_stream/dir", file_dir_, ""); - pnh_.param("raw_data_stream/publish", flag_publish_, false); - } +void RawDataStreamPa::getRosParams() +{ + if (is_ros_subscriber_) + { + pnh_.param("dir", file_dir_, ""); + } + else + { + pnh_.param("raw_data_stream/dir", file_dir_, ""); + pnh_.param("raw_data_stream/publish", flag_publish_, false); + } } -bool RawDataStreamPa::isEnabled() { - - if (is_ros_subscriber_) { - return !file_dir_.empty(); - } else { - return flag_publish_ || (!file_dir_.empty()); - } +bool RawDataStreamPa::isEnabled() +{ + if (is_ros_subscriber_) + { + return !file_dir_.empty(); + } + else + { + return flag_publish_ || (!file_dir_.empty()); + } } - -void RawDataStreamPa::initialize() { - - if (is_ros_subscriber_) { - ROS_INFO("Subscribing to raw data stream."); - static ros::Subscriber subscriber = - nh_.subscribe ("raw_data_stream", 100, - &RawDataStreamPa::msgCallback, this); - } else if (flag_publish_) { - ROS_INFO("Publishing raw data stream."); - RawDataStreamPa::publishMsg(std::string()); +void RawDataStreamPa::initialize() +{ + if (is_ros_subscriber_) + { + ROS_INFO("Subscribing to raw data stream."); + static ros::Subscriber subscriber = nh_.subscribe("raw_data_stream", 100, &RawDataStreamPa::msgCallback, this); + } + else if (flag_publish_) + { + ROS_INFO("Publishing raw data stream."); + RawDataStreamPa::publishMsg(std::string()); + } + + if (!file_dir_.empty()) + { + struct stat stat_info; + if (stat(file_dir_.c_str(), &stat_info) != 0) + { + ROS_ERROR("Can't log raw data to file. " + "Directory \"%s\" does not exist.", + file_dir_.c_str()); } - - if (!file_dir_.empty()) { - struct stat stat_info; - if (stat(file_dir_.c_str(), &stat_info ) != 0) { - ROS_ERROR("Can't log raw data to file. " - "Directory \"%s\" does not exist.", file_dir_.c_str()); - - } else if ((stat_info.st_mode & S_IFDIR) != S_IFDIR) { - ROS_ERROR("Can't log raw data to file. " - "\"%s\" exists, but is not a directory.", file_dir_.c_str()); - - } else { - if (file_dir_.back() != '/') { - file_dir_ += '/'; - } - - time_t t = time(NULL); - struct tm time_struct = *localtime(&t); - - std::stringstream filename; - filename.width(4); filename.fill('0'); - filename << time_struct.tm_year + 1900; - filename.width(0); filename << '_'; - filename.width(2); filename.fill('0'); - filename << time_struct.tm_mon + 1; - filename.width(0); filename << '_'; - filename.width(2); filename.fill('0'); - filename << time_struct.tm_mday; - filename.width(0); filename << '_'; - filename.width(2); filename.fill('0'); - filename << time_struct.tm_hour; - filename.width(2); filename.fill('0'); - filename << time_struct.tm_min ; - filename.width(0); filename << ".log"; - file_name_ = file_dir_ + filename.str(); - - try { - file_handle_.open(file_name_); - ROS_INFO("Logging raw data to file \"%s\"", + else if ((stat_info.st_mode & S_IFDIR) != S_IFDIR) + { + ROS_ERROR("Can't log raw data to file. " + "\"%s\" exists, but is not a directory.", + file_dir_.c_str()); + } + else + { + if (file_dir_.back() != '/') + { + file_dir_ += '/'; + } + + time_t t = time(NULL); + struct tm time_struct = *localtime(&t); + + std::stringstream filename; + filename.width(4); + filename.fill('0'); + filename << time_struct.tm_year + 1900; + filename.width(0); + filename << '_'; + filename.width(2); + filename.fill('0'); + filename << time_struct.tm_mon + 1; + filename.width(0); + filename << '_'; + filename.width(2); + filename.fill('0'); + filename << time_struct.tm_mday; + filename.width(0); + filename << '_'; + filename.width(2); + filename.fill('0'); + filename << time_struct.tm_hour; + filename.width(2); + filename.fill('0'); + filename << time_struct.tm_min; + filename.width(0); + filename << ".log"; + file_name_ = file_dir_ + filename.str(); + + try + { + file_handle_.open(file_name_); + ROS_INFO("Logging raw data to file \"%s\"", file_name_.c_str()); + } + catch (const std::exception& e) + { + ROS_ERROR("Can't log raw data to file. " + "Can't create file \"%s\".", file_name_.c_str()); - } catch(const std::exception& e) { - ROS_ERROR("Can't log raw data to file. " - "Can't create file \"%s\".", file_name_.c_str()); - } - } + } } + } } -void RawDataStreamPa::ubloxCallback(const unsigned char* data, - const std::size_t size) { +void RawDataStreamPa::ubloxCallback(const unsigned char* data, const std::size_t size) +{ + std::string str((const char*)data, size); - std::string str((const char*) data, size); + if (flag_publish_) + { + publishMsg(str); + } - if (flag_publish_) { - publishMsg(str); - } - - saveToFile(str); + saveToFile(str); } -void RawDataStreamPa::msgCallback( - const std_msgs::UInt8MultiArray::ConstPtr& msg) { - - std::string str(msg->data.size(), ' '); - std::copy(msg->data.begin(), msg->data.end(), str.begin()); - saveToFile(str); +void RawDataStreamPa::msgCallback(const std_msgs::UInt8MultiArray::ConstPtr& msg) +{ + std::string str(msg->data.size(), ' '); + std::copy(msg->data.begin(), msg->data.end(), str.begin()); + saveToFile(str); } -std_msgs::UInt8MultiArray RawDataStreamPa::str2uint8( - const std::string str) { - - std_msgs::UInt8MultiArray msg; +std_msgs::UInt8MultiArray RawDataStreamPa::str2uint8(const std::string str) +{ + std_msgs::UInt8MultiArray msg; - msg.layout.data_offset = 0; - msg.layout.dim.push_back(std_msgs::MultiArrayDimension()); - msg.layout.dim[0].size = str.length(); - msg.layout.dim[0].stride = 1; - msg.layout.dim[0].label = "raw_data_stream"; + msg.layout.data_offset = 0; + msg.layout.dim.push_back(std_msgs::MultiArrayDimension()); + msg.layout.dim[0].size = str.length(); + msg.layout.dim[0].stride = 1; + msg.layout.dim[0].label = "raw_data_stream"; - msg.data.resize(str.length()); - std::copy(str.begin(), str.end(), msg.data.begin()); + msg.data.resize(str.length()); + std::copy(str.begin(), str.end(), msg.data.begin()); - return msg; + return msg; } -void RawDataStreamPa::publishMsg(const std::string str) { +void RawDataStreamPa::publishMsg(const std::string str) +{ + static ros::Publisher publisher = pnh_.advertise("raw_data_stream", 100); - static ros::Publisher publisher = - pnh_.advertise("raw_data_stream", 100); - - publisher.publish(RawDataStreamPa::str2uint8(str)); + publisher.publish(RawDataStreamPa::str2uint8(str)); } -void RawDataStreamPa::saveToFile(const std::string str) { - - if (file_handle_.is_open()) { - try { - file_handle_ << str; - // file_handle_.flush(); - } catch(const std::exception& e) { - ROS_WARN("Error writing to file \"%s\"", file_name_.c_str()); - } +void RawDataStreamPa::saveToFile(const std::string str) +{ + if (file_handle_.is_open()) + { + try + { + file_handle_ << str; + // file_handle_.flush(); } + catch (const std::exception& e) + { + ROS_WARN("Error writing to file \"%s\"", file_name_.c_str()); + } + } } - diff --git a/ublox_msg_filters/include/ublox_msg_filters/exact_time.h b/ublox_msg_filters/include/ublox_msg_filters/exact_time.h index f11859b4..61d133f6 100644 --- a/ublox_msg_filters/include/ublox_msg_filters/exact_time.h +++ b/ublox_msg_filters/include/ublox_msg_filters/exact_time.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2009, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * 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. -* * Neither the name of the Willow Garage 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 OWNER 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + * * Neither the name of the Willow Garage 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 OWNER 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. + *********************************************************************/ #ifndef UBLOX_MSG_FILTERS_EXACT_TIME_H #define UBLOX_MSG_FILTERS_EXACT_TIME_H @@ -62,33 +62,52 @@ namespace ublox_msg_filters { - using NullType = message_filters::NullType; using Connection = message_filters::Connection; -template +template using PolicyBase = message_filters::PolicyBase; -template +template using Synchronizer = message_filters::Synchronizer; namespace mpl = boost::mpl; -template +template struct iTOW { - static u_int32_t value(const M& m) { return m.iTOW; } + static u_int32_t value(const M& m) + { + return m.iTOW; + } }; -template<> +template <> struct iTOW { - static u_int32_t value(const NullType& m) { return 0; } + static u_int32_t value(const NullType& m) + { + return 0; + } }; -template +template struct ExactTime : public PolicyBase { typedef Synchronizer Sync; @@ -108,11 +127,7 @@ struct ExactTime : public PolicyBase typedef typename Super::M8Event M8Event; typedef boost::tuple Tuple; - ExactTime(uint32_t queue_size) - : parent_(0) - , queue_size_(queue_size) - , enable_reset_(false) - , last_stamp_(0) + ExactTime(uint32_t queue_size) : parent_(0), queue_size_(queue_size), enable_reset_(false), last_stamp_(0) { } @@ -137,7 +152,7 @@ struct ExactTime : public PolicyBase parent_ = parent; } - template + template void add(const typename mpl::at_c::type& evt) { ROS_ASSERT(parent_); @@ -152,44 +167,44 @@ struct ExactTime : public PolicyBase checkTuple(t); } - template + template Connection registerDropCallback(const C& callback) { - #ifndef _WIN32 +#ifndef _WIN32 return drop_signal_.template addCallback(callback); - #else +#else return drop_signal_.addCallback(callback); - #endif +#endif } - template + template Connection registerDropCallback(C& callback) { - #ifndef _WIN32 +#ifndef _WIN32 return drop_signal_.template addCallback(callback); - #else +#else return drop_signal_.addCallback(callback); - #endif +#endif } - template + template Connection registerDropCallback(const C& callback, T* t) { - #ifndef _WIN32 +#ifndef _WIN32 return drop_signal_.template addCallback(callback, t); - #else +#else return drop_signal_.addCallback(callback, t); - #endif +#endif } - template + template Connection registerDropCallback(C& callback, T* t) { - #ifndef _WIN32 +#ifndef _WIN32 return drop_signal_.template addCallback(callback, t); - #else +#else return drop_signal_.addCallback(callback, t); - #endif +#endif } void setReset(const bool reset) @@ -198,7 +213,6 @@ struct ExactTime : public PolicyBase } private: - // assumes mutex_ is already locked void checkTuple(Tuple& t) { @@ -217,9 +231,15 @@ struct ExactTime : public PolicyBase if (full) { - parent_->signal(boost::get<0>(t), boost::get<1>(t), boost::get<2>(t), - boost::get<3>(t), boost::get<4>(t), boost::get<5>(t), - boost::get<6>(t), boost::get<7>(t), boost::get<8>(t)); + parent_->signal(boost::get<0>(t), + boost::get<1>(t), + boost::get<2>(t), + boost::get<3>(t), + boost::get<4>(t), + boost::get<5>(t), + boost::get<6>(t), + boost::get<7>(t), + boost::get<8>(t)); last_signal_time_ = iTOW::value(*boost::get<0>(t).getMessage()); @@ -233,9 +253,15 @@ struct ExactTime : public PolicyBase while (tuples_.size() > queue_size_) { Tuple& t2 = tuples_.begin()->second; - drop_signal_.call(boost::get<0>(t2), boost::get<1>(t2), boost::get<2>(t2), - boost::get<3>(t2), boost::get<4>(t2), boost::get<5>(t2), - boost::get<6>(t2), boost::get<7>(t2), boost::get<8>(t2)); + drop_signal_.call(boost::get<0>(t2), + boost::get<1>(t2), + boost::get<2>(t2), + boost::get<3>(t2), + boost::get<4>(t2), + boost::get<5>(t2), + boost::get<6>(t2), + boost::get<7>(t2), + boost::get<8>(t2)); tuples_.erase(tuples_.begin()); } } @@ -254,9 +280,15 @@ struct ExactTime : public PolicyBase ++it; Tuple& t = old->second; - drop_signal_.call(boost::get<0>(t), boost::get<1>(t), boost::get<2>(t), - boost::get<3>(t), boost::get<4>(t), boost::get<5>(t), - boost::get<6>(t), boost::get<7>(t), boost::get<8>(t)); + drop_signal_.call(boost::get<0>(t), + boost::get<1>(t), + boost::get<2>(t), + boost::get<3>(t), + boost::get<4>(t), + boost::get<5>(t), + boost::get<6>(t), + boost::get<7>(t), + boost::get<8>(t)); tuples_.erase(old); } else @@ -282,6 +314,6 @@ struct ExactTime : public PolicyBase boost::mutex mutex_; }; -} // namespace ublox_msg_filters +} // namespace ublox_msg_filters -#endif // UBLOX_MSG_FILTERS_EXACT_TIME_H +#endif // UBLOX_MSG_FILTERS_EXACT_TIME_H diff --git a/ublox_msg_filters/src/example.cpp b/ublox_msg_filters/src/example.cpp index ba0bf707..67ec9a6f 100644 --- a/ublox_msg_filters/src/example.cpp +++ b/ublox_msg_filters/src/example.cpp @@ -5,13 +5,15 @@ #include #include -void callback(const ublox_msgs::NavHPPOSLLHConstPtr &msg1, - const ublox_msgs::NavRELPOSNED9ConstPtr &msg2, - const ublox_msgs::NavVELNEDConstPtr &msg3) { +void callback(const ublox_msgs::NavHPPOSLLHConstPtr& msg1, + const ublox_msgs::NavRELPOSNED9ConstPtr& msg2, + const ublox_msgs::NavVELNEDConstPtr& msg3) +{ ROS_INFO("RX %u %u %u", msg1->iTOW, msg2->iTOW, msg3->iTOW); } -int main(int argc, char **argv) { +int main(int argc, char** argv) +{ ros::init(argc, argv, "ublox_sync"); ros::NodeHandle nh; @@ -19,7 +21,8 @@ int main(int argc, char **argv) { message_filters::Subscriber sub2(nh, "msg2", 1); message_filters::Subscriber sub3(nh, "msg3", 1); - typedef ublox_msg_filters::ExactTime MySyncPolicy; + typedef ublox_msg_filters::ExactTime + MySyncPolicy; message_filters::Synchronizer sync(MySyncPolicy(10), sub1, sub2, sub3); sync.registerCallback(boost::bind(callback, _1, _2, _3)); diff --git a/ublox_msg_filters/tests/test.cpp b/ublox_msg_filters/tests/test.cpp index 553dbcdf..5258ee0b 100644 --- a/ublox_msg_filters/tests/test.cpp +++ b/ublox_msg_filters/tests/test.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * 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. -* * Neither the name of the Willow Garage 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 OWNER 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + * * Neither the name of the Willow Garage 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 OWNER 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. + *********************************************************************/ #include @@ -50,14 +50,16 @@ typedef boost::shared_ptr MsgConstPtr; class Helper { public: - Helper() {} + Helper() + { + } void cb() { ++count_; } - void cb3(const MsgConstPtr &msg1, const MsgConstPtr &msg2, const MsgConstPtr &msg3) + void cb3(const MsgConstPtr& msg1, const MsgConstPtr& msg2, const MsgConstPtr& msg3) { ++count_; stamp1_ = msg1->iTOW; @@ -154,7 +156,8 @@ TEST(ExactTime, dropCallback) ASSERT_EQ(h.drop_count_, 1); } -int main(int argc, char **argv){ +int main(int argc, char** argv) +{ testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "blah"); diff --git a/ublox_msgs/CMakeLists.txt b/ublox_msgs/CMakeLists.txt index 5c677b74..39586cd2 100644 --- a/ublox_msgs/CMakeLists.txt +++ b/ublox_msgs/CMakeLists.txt @@ -3,6 +3,11 @@ project(ublox_msgs) find_package(catkin REQUIRED COMPONENTS message_generation ublox_serialization std_msgs sensor_msgs) +# Generate services in the 'srv' folder +add_service_files( + FILES + GetVersionInfo.srv +) add_message_files(DIRECTORY msg) generate_messages(DEPENDENCIES std_msgs sensor_msgs) diff --git a/ublox_msgs/include/ublox/serialization/ublox_msgs.h b/ublox_msgs/include/ublox/serialization/ublox_msgs.h index fc7b2c41..b180117d 100644 --- a/ublox_msgs/include/ublox/serialization/ublox_msgs.h +++ b/ublox_msgs/include/ublox/serialization/ublox_msgs.h @@ -14,9 +14,9 @@ // 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 +// 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 BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; @@ -26,7 +26,6 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //============================================================================== - #ifndef UBLOX_SERIALIZATION_UBLOX_MSGS_H #define UBLOX_SERIALIZATION_UBLOX_MSGS_H @@ -35,24 +34,24 @@ #include /// -/// This file declares custom serializers for u-blox messages with dynamic +/// This file declares custom serializers for u-blox messages with dynamic /// lengths and messages where the get/set messages have different sizes, but /// share the same parameters, such as CfgDAT. /// -namespace ublox { - +namespace ublox +{ /// -/// @brief Serializes the CfgDAT message which has a different length for +/// @brief Serializes the CfgDAT message which has a different length for /// get/set. /// template -struct Serializer > { - typedef boost::call_traits > - CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); +struct Serializer > +{ + typedef boost::call_traits > CallTraits; + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.datumNum); stream.next(m.datumName); stream.next(m.majA); @@ -66,14 +65,15 @@ struct Serializer > { stream.next(m.scale); } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { // this is the size of CfgDAT set messages // serializedLength is only used for writes so this is ok return 44; } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { ros::serialization::OStream stream(data, size); // ignores datumNum & datumName stream.next(m.majA); @@ -92,38 +92,40 @@ struct Serializer > { /// @brief Serializes the CfgGNSS message which has a repeated block. /// template -struct Serializer > { +struct Serializer > +{ typedef ublox_msgs::CfgGNSS_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.msgVer); stream.next(m.numTrkChHw); stream.next(m.numTrkChUse); stream.next(m.numConfigBlocks); m.blocks.resize(m.numConfigBlocks); - for(std::size_t i = 0; i < m.blocks.size(); ++i) + for (std::size_t i = 0; i < m.blocks.size(); ++i) ros::serialization::deserialize(stream, m.blocks[i]); } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 4 + 8 * m.numConfigBlocks; } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.blocks.size() != m.numConfigBlocks) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { + if (m.blocks.size() != m.numConfigBlocks) + { ROS_ERROR("CfgGNSS numConfigBlocks must equal blocks size"); } ros::serialization::OStream stream(data, size); stream.next(m.msgVer); stream.next(m.numTrkChHw); stream.next(m.numTrkChUse); - stream.next( - static_cast(m.blocks.size())); - for(std::size_t i = 0; i < m.blocks.size(); ++i) + stream.next(static_cast(m.blocks.size())); + for (std::size_t i = 0; i < m.blocks.size(); ++i) ros::serialization::serialize(stream, m.blocks[i]); } }; @@ -132,27 +134,28 @@ struct Serializer > { /// @brief Serializes the CfgInf message which has a repeated block. /// template -struct Serializer > { - typedef boost::call_traits > - CallTraits; +struct Serializer > +{ + typedef boost::call_traits > CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); int num_blocks = count / 10; m.blocks.resize(num_blocks); - for(std::size_t i = 0; i < num_blocks; ++i) + for (std::size_t i = 0; i < num_blocks; ++i) ros::serialization::deserialize(stream, m.blocks[i]); } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 10 * m.blocks.size(); } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { ros::serialization::OStream stream(data, size); - for(std::size_t i = 0; i < m.blocks.size(); ++i) + for (std::size_t i = 0; i < m.blocks.size(); ++i) ros::serialization::serialize(stream, m.blocks[i]); } }; @@ -161,25 +164,27 @@ struct Serializer > { /// @brief Serializes the Inf message which has a dynamic length string. /// template -struct Serializer > { +struct Serializer > +{ typedef boost::call_traits > CallTraits; - - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); m.str.resize(count); for (int i = 0; i < count; ++i) ros::serialization::deserialize(stream, m.str[i]); } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return m.str.size(); } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { ros::serialization::OStream stream(data, size); - for(std::size_t i = 0; i < m.str.size(); ++i) + for (std::size_t i = 0; i < m.str.size(); ++i) ros::serialization::serialize(stream, m.str[i]); } }; @@ -188,13 +193,14 @@ struct Serializer > { /// @brief Serializes the MonVER message which has a repeated block. /// template -struct Serializer > { +struct Serializer > +{ typedef ublox_msgs::MonVER_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.swVersion); stream.next(m.hwVersion); @@ -202,23 +208,25 @@ struct Serializer > { int N = (count - 40) / 30; m.extension.reserve(N); typename Msg::_extension_type::value_type ext; - for (int i = 0; i < N; i++) { + for (int i = 0; i < N; i++) + { // Read each extension string stream.next(ext); m.extension.push_back(ext); } } - static uint32_t serializedLength(typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 40 + (30 * m.extension.size()); } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { ros::serialization::OStream stream(data, size); stream.next(m.swVersion); stream.next(m.hwVersion); - for(std::size_t i = 0; i < m.extension.size(); ++i) + for (std::size_t i = 0; i < m.extension.size(); ++i) ros::serialization::serialize(stream, m.extension[i]); } }; @@ -227,13 +235,14 @@ struct Serializer > { /// @brief Serializes the NavDGPS message which has a repeated block. /// template -struct Serializer > { +struct Serializer > +{ typedef ublox_msgs::NavDGPS_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.iTOW); stream.next(m.age); stream.next(m.baseId); @@ -242,17 +251,19 @@ struct Serializer > { stream.next(m.status); stream.next(m.reserved1); m.sv.resize(m.numCh); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::deserialize(stream, m.sv[i]); } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 16 + 12 * m.numCh; } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.sv.size() != m.numCh) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { + if (m.sv.size() != m.numCh) + { ROS_ERROR("NavDGPS numCh must equal sv size"); } ros::serialization::OStream stream(data, size); @@ -263,23 +274,23 @@ struct Serializer > { stream.next(static_cast(m.sv.size())); stream.next(m.status); stream.next(m.reserved1); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::serialize(stream, m.sv[i]); } }; - /// /// @brief Serializes the NavSBAS message which has a repeated block. /// template -struct Serializer > { +struct Serializer > +{ typedef ublox_msgs::NavSBAS_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.iTOW); stream.next(m.geo); stream.next(m.mode); @@ -288,17 +299,19 @@ struct Serializer > { stream.next(m.cnt); stream.next(m.reserved0); m.sv.resize(m.cnt); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::deserialize(stream, m.sv[i]); } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 12 + 12 * m.cnt; } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.sv.size() != m.cnt) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { + if (m.sv.size() != m.cnt) + { ROS_ERROR("NavSBAS cnt must equal sv size"); } ros::serialization::OStream stream(data, size); @@ -309,7 +322,7 @@ struct Serializer > { stream.next(m.service); stream.next(static_cast(m.sv.size())); stream.next(m.reserved0); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::serialize(stream, m.sv[i]); } }; @@ -318,29 +331,32 @@ struct Serializer > { /// @brief Serializes the NavSAT message which has a repeated block. /// template -struct Serializer > { +struct Serializer > +{ typedef ublox_msgs::NavSAT_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.iTOW); stream.next(m.version); stream.next(m.numSvs); stream.next(m.reserved0); m.sv.resize(m.numSvs); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::deserialize(stream, m.sv[i]); } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 8 + 12 * m.numSvs; } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.sv.size() != m.numSvs) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { + if (m.sv.size() != m.numSvs) + { ROS_ERROR("NavSAT numSvs must equal sv size"); } ros::serialization::OStream stream(data, size); @@ -348,7 +364,7 @@ struct Serializer > { stream.next(m.version); stream.next(static_cast(m.sv.size())); stream.next(m.reserved0); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::serialize(stream, m.sv[i]); } }; @@ -357,29 +373,32 @@ struct Serializer > { /// @brief Serializes the NavDGPS message which has a repeated block. /// template -struct Serializer > { +struct Serializer > +{ typedef ublox_msgs::NavSVINFO_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.iTOW); stream.next(m.numCh); stream.next(m.globalFlags); stream.next(m.reserved2); m.sv.resize(m.numCh); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::deserialize(stream, m.sv[i]); } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 8 + 12 * m.numCh; } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.sv.size() != m.numCh) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { + if (m.sv.size() != m.numCh) + { ROS_ERROR("NavSVINFO numCh must equal sv size"); } ros::serialization::OStream stream(data, size); @@ -387,7 +406,7 @@ struct Serializer > { stream.next(static_cast(m.sv.size())); stream.next(m.globalFlags); stream.next(m.reserved2); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::serialize(stream, m.sv[i]); } }; @@ -396,29 +415,32 @@ struct Serializer > { /// @brief Serializes the RxmRAW message which has a repeated block. /// template -struct Serializer > { +struct Serializer > +{ typedef ublox_msgs::RxmRAW_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.rcvTOW); stream.next(m.week); stream.next(m.numSV); stream.next(m.reserved1); m.sv.resize(m.numSV); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::deserialize(stream, m.sv[i]); } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 8 + 24 * m.numSV; } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.sv.size() != m.numSV) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { + if (m.sv.size() != m.numSV) + { ROS_ERROR("RxmRAW numSV must equal sv size"); } ros::serialization::OStream stream(data, size); @@ -426,7 +448,7 @@ struct Serializer > { stream.next(m.week); stream.next(static_cast(m.sv.size())); stream.next(m.reserved1); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::serialize(stream, m.sv[i]); } }; @@ -435,13 +457,14 @@ struct Serializer > { /// @brief Serializes the RxmRAWX message which has a repeated block. /// template -struct Serializer > { +struct Serializer > +{ typedef ublox_msgs::RxmRAWX_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.rcvTOW); stream.next(m.week); stream.next(m.leapS); @@ -450,17 +473,19 @@ struct Serializer > { stream.next(m.version); stream.next(m.reserved1); m.meas.resize(m.numMeas); - for(std::size_t i = 0; i < m.meas.size(); ++i) + for (std::size_t i = 0; i < m.meas.size(); ++i) ros::serialization::deserialize(stream, m.meas[i]); } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 16 + 32 * m.numMeas; } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.meas.size() != m.numMeas) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { + if (m.meas.size() != m.numMeas) + { ROS_ERROR("RxmRAWX numMeas must equal meas size"); } ros::serialization::OStream stream(data, size); @@ -471,7 +496,7 @@ struct Serializer > { stream.next(m.recStat); stream.next(m.version); stream.next(m.reserved1); - for(std::size_t i = 0; i < m.meas.size(); ++i) + for (std::size_t i = 0; i < m.meas.size(); ++i) ros::serialization::serialize(stream, m.meas[i]); } }; @@ -480,13 +505,14 @@ struct Serializer > { /// @brief Serializes the RxmSFRBX message which has a repeated block. /// template -struct Serializer > { +struct Serializer > +{ typedef ublox_msgs::RxmSFRBX_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.gnssId); stream.next(m.svId); stream.next(m.reserved0); @@ -496,17 +522,19 @@ struct Serializer > { stream.next(m.version); stream.next(m.reserved1); m.dwrd.resize(m.numWords); - for(std::size_t i = 0; i < m.dwrd.size(); ++i) + for (std::size_t i = 0; i < m.dwrd.size(); ++i) ros::serialization::deserialize(stream, m.dwrd[i]); } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 8 + 4 * m.numWords; } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.dwrd.size() != m.numWords) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { + if (m.dwrd.size() != m.numWords) + { ROS_ERROR("RxmSFRBX numWords must equal dwrd size"); } ros::serialization::OStream stream(data, size); @@ -518,7 +546,7 @@ struct Serializer > { stream.next(m.chn); stream.next(m.version); stream.next(m.reserved1); - for(std::size_t i = 0; i < m.dwrd.size(); ++i) + for (std::size_t i = 0; i < m.dwrd.size(); ++i) ros::serialization::serialize(stream, m.dwrd[i]); } }; @@ -527,29 +555,32 @@ struct Serializer > { /// @brief Serializes the RxmSVSI message which has a repeated block. /// template -struct Serializer > { +struct Serializer > +{ typedef ublox_msgs::RxmSVSI_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.iTOW); stream.next(m.week); stream.next(m.numVis); stream.next(m.numSV); m.sv.resize(m.numSV); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::deserialize(stream, m.sv[i]); } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 8 + 6 * m.numSV; } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.sv.size() != m.numSV) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { + if (m.sv.size() != m.numSV) + { ROS_ERROR("RxmSVSI numSV must equal sv size"); } ros::serialization::OStream stream(data, size); @@ -557,7 +588,7 @@ struct Serializer > { stream.next(m.week); stream.next(m.numVis); stream.next(static_cast(m.sv.size())); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::serialize(stream, m.sv[i]); } }; @@ -566,37 +597,41 @@ struct Serializer > { /// @brief Serializes the RxmALM message which has a repeated block. /// template -struct Serializer > { +struct Serializer > +{ typedef ublox_msgs::RxmALM_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.svid); stream.next(m.week); m.dwrd.clear(); - if(count == 40) { + if (count == 40) + { typename Msg::_dwrd_type::value_type temp; m.dwrd.resize(8); - for(std::size_t i = 0; i < 8; ++i) { + for (std::size_t i = 0; i < 8; ++i) + { stream.next(temp); m.dwrd.push_back(temp); } } } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 8 + (4 * m.dwrd.size()); } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { ros::serialization::OStream stream(data, size); stream.next(m.svid); stream.next(m.week); - for(std::size_t i = 0; i < m.dwrd.size(); ++i) + for (std::size_t i = 0; i < m.dwrd.size(); ++i) ros::serialization::serialize(stream, m.dwrd[i]); } }; @@ -610,52 +645,57 @@ struct Serializer > typedef ublox_msgs::RxmEPH_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.svid); stream.next(m.how); m.sf1d.clear(); m.sf2d.clear(); m.sf3d.clear(); - if (count == 104) { + if (count == 104) + { typename Msg::_sf1d_type::value_type temp1; typename Msg::_sf2d_type::value_type temp2; typename Msg::_sf3d_type::value_type temp3; m.sf1d.resize(8); - for(std::size_t i = 0; i < 8; ++i) { + for (std::size_t i = 0; i < 8; ++i) + { stream.next(temp1); m.sf1d.push_back(temp1); } m.sf2d.resize(8); - for(std::size_t i = 0; i < 8; ++i) { + for (std::size_t i = 0; i < 8; ++i) + { stream.next(temp2); m.sf2d.push_back(temp2); } m.sf3d.resize(8); - for(std::size_t i = 0; i < 8; ++i) { + for (std::size_t i = 0; i < 8; ++i) + { stream.next(temp3); m.sf3d.push_back(temp3); } } } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 8 + (4 * m.sf1d.size()) + (4 * m.sf2d.size()) + (4 * m.sf3d.size()); } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { ros::serialization::OStream stream(data, size); stream.next(m.svid); stream.next(m.how); - for(std::size_t i = 0; i < m.sf1d.size(); ++i) + for (std::size_t i = 0; i < m.sf1d.size(); ++i) ros::serialization::serialize(stream, m.sf1d[i]); - for(std::size_t i = 0; i < m.sf2d.size(); ++i) + for (std::size_t i = 0; i < m.sf2d.size(); ++i) ros::serialization::serialize(stream, m.sf2d[i]); - for(std::size_t i = 0; i < m.sf3d.size(); ++i) + for (std::size_t i = 0; i < m.sf3d.size(); ++i) ros::serialization::serialize(stream, m.sf3d[i]); } }; @@ -664,37 +704,41 @@ struct Serializer > /// @brief Serializes the AidALM message which has a repeated block. /// template -struct Serializer > { +struct Serializer > +{ typedef ublox_msgs::AidALM_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.svid); stream.next(m.week); m.dwrd.clear(); - if (count == 40) { + if (count == 40) + { typename Msg::_dwrd_type::value_type temp; m.dwrd.resize(8); - for(std::size_t i = 0; i < 8; ++i) { + for (std::size_t i = 0; i < 8; ++i) + { stream.next(temp); m.dwrd.push_back(temp); } - } + } } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 8 + (4 * m.dwrd.size()); } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { ros::serialization::OStream stream(data, size); stream.next(m.svid); stream.next(m.week); - for(std::size_t i = 0; i < m.dwrd.size(); ++i) + for (std::size_t i = 0; i < m.dwrd.size(); ++i) ros::serialization::serialize(stream, m.dwrd[i]); } }; @@ -708,51 +752,56 @@ struct Serializer > typedef ublox_msgs::AidEPH_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.svid); stream.next(m.how); m.sf1d.clear(); m.sf2d.clear(); m.sf3d.clear(); - if (count == 104) { + if (count == 104) + { typename Msg::_sf1d_type::value_type temp1; typename Msg::_sf2d_type::value_type temp2; typename Msg::_sf3d_type::value_type temp3; m.sf1d.resize(8); - for(std::size_t i = 0; i < 8; ++i) { + for (std::size_t i = 0; i < 8; ++i) + { stream.next(temp1); m.sf1d.push_back(temp1); } m.sf2d.resize(8); - for(std::size_t i = 0; i < 8; ++i) { + for (std::size_t i = 0; i < 8; ++i) + { stream.next(temp2); m.sf2d.push_back(temp2); } m.sf3d.resize(8); - for(std::size_t i = 0; i < 8; ++i) { + for (std::size_t i = 0; i < 8; ++i) + { stream.next(temp3); m.sf3d.push_back(temp3); } } } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 8 + (4 * m.sf1d.size()) + (4 * m.sf2d.size()) + (4 * m.sf3d.size()); } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { ros::serialization::OStream stream(data, size); stream.next(m.svid); stream.next(m.how); - for(std::size_t i = 0; i < m.sf1d.size(); ++i) + for (std::size_t i = 0; i < m.sf1d.size(); ++i) ros::serialization::serialize(stream, m.sf1d[i]); - for(std::size_t i = 0; i < m.sf2d.size(); ++i) + for (std::size_t i = 0; i < m.sf2d.size(); ++i) ros::serialization::serialize(stream, m.sf2d[i]); - for(std::size_t i = 0; i < m.sf3d.size(); ++i) + for (std::size_t i = 0; i < m.sf3d.size(); ++i) ros::serialization::serialize(stream, m.sf3d[i]); } }; @@ -762,13 +811,13 @@ struct Serializer > /// optional block. /// template -struct Serializer > { - typedef boost::call_traits > - CallTraits; +struct Serializer > +{ + typedef boost::call_traits > CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.timeTag); stream.next(m.flags); stream.next(m.id); @@ -777,28 +826,30 @@ struct Serializer > { int data_size = (count - (calib_valid ? 12 : 8)) / 4; // Repeating block m.data.resize(data_size); - for(std::size_t i = 0; i < data_size; ++i) + for (std::size_t i = 0; i < data_size; ++i) ros::serialization::deserialize(stream, m.data[i]); // Optional block - if(calib_valid) { + if (calib_valid) + { m.calibTtag.resize(1); ros::serialization::deserialize(stream, m.calibTtag[0]); } } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 4 + 8 * m.data.size() + 4 * m.calibTtag.size(); } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { ros::serialization::OStream stream(data, size); stream.next(m.timeTag); stream.next(m.flags); stream.next(m.id); - for(std::size_t i = 0; i < m.data.size(); ++i) + for (std::size_t i = 0; i < m.data.size(); ++i) ros::serialization::serialize(stream, m.data[i]); - for(std::size_t i = 0; i < m.calibTtag.size(); ++i) + for (std::size_t i = 0; i < m.calibTtag.size(); ++i) ros::serialization::serialize(stream, m.calibTtag[i]); } }; @@ -807,31 +858,31 @@ struct Serializer > { /// @brief Serializes the EsfRAW message which has a repeated block. /// template -struct Serializer > { - typedef boost::call_traits > - CallTraits; +struct Serializer > +{ + typedef boost::call_traits > CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.reserved0); m.blocks.clear(); int num_blocks = (count - 4) / 8; m.blocks.resize(num_blocks); - for(std::size_t i = 0; i < num_blocks; ++i) + for (std::size_t i = 0; i < num_blocks; ++i) ros::serialization::deserialize(stream, m.blocks[i]); } - - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 4 + 8 * m.blocks.size(); } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { ros::serialization::OStream stream(data, size); stream.next(m.reserved0); - for(std::size_t i = 0; i < m.blocks.size(); ++i) + for (std::size_t i = 0; i < m.blocks.size(); ++i) ros::serialization::serialize(stream, m.blocks[i]); } }; @@ -840,30 +891,33 @@ struct Serializer > { /// @brief Serializes the EsfSTATUS message which has a repeated block. /// template -struct Serializer > { +struct Serializer > +{ typedef ublox_msgs::EsfSTATUS_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.iTOW); stream.next(m.version); stream.next(m.fusionMode); stream.next(m.reserved2); stream.next(m.numSens); m.sens.resize(m.numSens); - for(std::size_t i = 0; i < m.sens.size(); ++i) + for (std::size_t i = 0; i < m.sens.size(); ++i) ros::serialization::deserialize(stream, m.sens[i]); } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 16 + 4 * m.numSens; } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.sens.size() != m.numSens) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { + if (m.sens.size() != m.numSens) + { ROS_ERROR("Writing EsfSTATUS message: numSens must equal size of sens"); } ros::serialization::OStream stream(data, size); @@ -872,12 +926,11 @@ struct Serializer > { stream.next(m.fusionMode); stream.next(m.reserved2); stream.next(static_cast(m.sens.size())); - for(std::size_t i = 0; i < m.sens.size(); ++i) + for (std::size_t i = 0; i < m.sens.size(); ++i) ros::serialization::serialize(stream, m.sens[i]); } }; +} // namespace ublox -} // namespace ublox - -#endif // UBLOX_SERIALIZATION_UBLOX_MSGS_H +#endif // UBLOX_SERIALIZATION_UBLOX_MSGS_H diff --git a/ublox_msgs/include/ublox_msgs/ublox_msgs.h b/ublox_msgs/include/ublox_msgs/ublox_msgs.h index d640e56a..006dc0b5 100644 --- a/ublox_msgs/include/ublox_msgs/ublox_msgs.h +++ b/ublox_msgs/include/ublox_msgs/ublox_msgs.h @@ -14,9 +14,9 @@ // 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 +// 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 BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; @@ -111,153 +111,167 @@ #include -namespace ublox_msgs { - -namespace Class { - static const uint8_t NAV = 0x01; //!< Navigation Result Messages: Position, - //!< Speed, Time, Acceleration, Heading, +namespace ublox_msgs +{ +namespace Class +{ +static const uint8_t NAV = 0x01; //!< Navigation Result Messages: Position, + //!< Speed, Time, Acceleration, Heading, //!< DOP, SVs used - static const uint8_t RXM = 0x02; //!< Receiver Manager Messages: +static const uint8_t RXM = 0x02; //!< Receiver Manager Messages: //!< Satellite Status, RTC Status - static const uint8_t INF = 0x04; //!< Information Messages: +static const uint8_t INF = 0x04; //!< Information Messages: //!< Printf-Style Messages, with IDs such as //!< Error, Warning, Notice - static const uint8_t ACK = 0x05; //!< Ack/Nack Messages: Acknowledge or Reject +static const uint8_t ACK = 0x05; //!< Ack/Nack Messages: Acknowledge or Reject //!< messages to CFG input messages - static const uint8_t CFG = 0x06; //!< Configuration Input Messages: Set - //!< Dynamic Model, Set DOP Mask, Set Baud +static const uint8_t CFG = 0x06; //!< Configuration Input Messages: Set + //!< Dynamic Model, Set DOP Mask, Set Baud //!< Rate, etc. - static const uint8_t UPD = 0x09; //!< Firmware Update Messages: i.e. - //!< Memory/Flash erase/write, Reboot, Flash +static const uint8_t UPD = 0x09; //!< Firmware Update Messages: i.e. + //!< Memory/Flash erase/write, Reboot, Flash //!< identification, etc. - //!< Used to update the firmware and identify + //!< Used to update the firmware and identify //!< any attached flash device - static const uint8_t MON = 0x0A; //!< Monitoring Messages: Communication - //!< Status, CPU Load, Stack Usage, +static const uint8_t MON = 0x0A; //!< Monitoring Messages: Communication + //!< Status, CPU Load, Stack Usage, //!< Task Status - static const uint8_t AID = 0x0B; //!< AssistNow Aiding Messages: Ephemeris, +static const uint8_t AID = 0x0B; //!< AssistNow Aiding Messages: Ephemeris, //!< Almanac, other A-GPS data input - static const uint8_t TIM = 0x0D; //!< Timing Messages: Timepulse Output, +static const uint8_t TIM = 0x0D; //!< Timing Messages: Timepulse Output, //!< Timemark Results - static const uint8_t ESF = 0x10; //!< External Sensor Fusion Messages: - //!< External sensor measurements and status +static const uint8_t ESF = 0x10; //!< External Sensor Fusion Messages: + //!< External sensor measurements and status //!< information - static const uint8_t MGA = 0x13; //!< Multiple GNSS Assistance Messages: +static const uint8_t MGA = 0x13; //!< Multiple GNSS Assistance Messages: //!< Assistance data for various GNSS - static const uint8_t LOG = 0x21; //!< Logging Messages: Log creation, +static const uint8_t LOG = 0x21; //!< Logging Messages: Log creation, //!< deletion, info and retrieval - static const uint8_t SEC = 0x27; //!< Security Feature Messages - static const uint8_t HNR = 0x28; //!< High Rate Navigation Results Messages: +static const uint8_t SEC = 0x27; //!< Security Feature Messages +static const uint8_t HNR = 0x28; //!< High Rate Navigation Results Messages: //!< High rate time, position, speed, heading - static const uint8_t RTCM = 0xF5; //!< RTCM Configuration Messages -} +static const uint8_t RTCM = 0xF5; //!< RTCM Configuration Messages +} // namespace Class -namespace Message { - namespace NAV { - static const uint8_t ATT = NavATT::MESSAGE_ID; - static const uint8_t CLOCK = NavCLOCK::MESSAGE_ID; - static const uint8_t DGPS = NavDGPS::MESSAGE_ID; - static const uint8_t DOP = NavDOP::MESSAGE_ID; - static const uint8_t HPPOSECEF = NavHPPOSECEF::MESSAGE_ID; - static const uint8_t HPPOSLLH = NavHPPOSLLH::MESSAGE_ID; - static const uint8_t POSECEF = NavPOSECEF::MESSAGE_ID; - static const uint8_t POSLLH = NavPOSLLH::MESSAGE_ID; - static const uint8_t RELPOSNED = NavRELPOSNED::MESSAGE_ID; - static const uint8_t RELPOSNED9 = NavRELPOSNED9::MESSAGE_ID; - static const uint8_t SBAS = NavSBAS::MESSAGE_ID; - static const uint8_t SOL = NavSOL::MESSAGE_ID; - static const uint8_t PVT = NavPVT::MESSAGE_ID; - static const uint8_t SAT = NavSAT::MESSAGE_ID; - static const uint8_t STATUS = NavSTATUS::MESSAGE_ID; - static const uint8_t SVINFO = NavSVINFO::MESSAGE_ID; - static const uint8_t SVIN = NavSVIN::MESSAGE_ID; - static const uint8_t TIMEGPS = NavTIMEGPS::MESSAGE_ID; - static const uint8_t TIMEUTC = NavTIMEUTC::MESSAGE_ID; - static const uint8_t VELECEF = NavVELECEF::MESSAGE_ID; - static const uint8_t VELNED = NavVELNED::MESSAGE_ID; - } +namespace Message +{ +namespace NAV +{ +static const uint8_t ATT = NavATT::MESSAGE_ID; +static const uint8_t CLOCK = NavCLOCK::MESSAGE_ID; +static const uint8_t DGPS = NavDGPS::MESSAGE_ID; +static const uint8_t DOP = NavDOP::MESSAGE_ID; +static const uint8_t HPPOSECEF = NavHPPOSECEF::MESSAGE_ID; +static const uint8_t HPPOSLLH = NavHPPOSLLH::MESSAGE_ID; +static const uint8_t POSECEF = NavPOSECEF::MESSAGE_ID; +static const uint8_t POSLLH = NavPOSLLH::MESSAGE_ID; +static const uint8_t RELPOSNED = NavRELPOSNED::MESSAGE_ID; +static const uint8_t RELPOSNED9 = NavRELPOSNED9::MESSAGE_ID; +static const uint8_t SBAS = NavSBAS::MESSAGE_ID; +static const uint8_t SOL = NavSOL::MESSAGE_ID; +static const uint8_t PVT = NavPVT::MESSAGE_ID; +static const uint8_t SAT = NavSAT::MESSAGE_ID; +static const uint8_t STATUS = NavSTATUS::MESSAGE_ID; +static const uint8_t SVINFO = NavSVINFO::MESSAGE_ID; +static const uint8_t SVIN = NavSVIN::MESSAGE_ID; +static const uint8_t TIMEGPS = NavTIMEGPS::MESSAGE_ID; +static const uint8_t TIMEUTC = NavTIMEUTC::MESSAGE_ID; +static const uint8_t VELECEF = NavVELECEF::MESSAGE_ID; +static const uint8_t VELNED = NavVELNED::MESSAGE_ID; +} // namespace NAV - namespace RXM { - static const uint8_t ALM = RxmALM::MESSAGE_ID; - static const uint8_t EPH = RxmEPH::MESSAGE_ID; - static const uint8_t RAW = RxmRAW::MESSAGE_ID; - static const uint8_t RAWX = RxmRAWX::MESSAGE_ID; - static const uint8_t RTCM = RxmRTCM::MESSAGE_ID; - static const uint8_t SFRB = RxmSFRB::MESSAGE_ID; - static const uint8_t SFRBX = RxmSFRBX::MESSAGE_ID; - static const uint8_t SVSI = RxmSVSI::MESSAGE_ID; - } +namespace RXM +{ +static const uint8_t ALM = RxmALM::MESSAGE_ID; +static const uint8_t EPH = RxmEPH::MESSAGE_ID; +static const uint8_t RAW = RxmRAW::MESSAGE_ID; +static const uint8_t RAWX = RxmRAWX::MESSAGE_ID; +static const uint8_t RTCM = RxmRTCM::MESSAGE_ID; +static const uint8_t SFRB = RxmSFRB::MESSAGE_ID; +static const uint8_t SFRBX = RxmSFRBX::MESSAGE_ID; +static const uint8_t SVSI = RxmSVSI::MESSAGE_ID; +} // namespace RXM - namespace INF { - static const uint8_t ERROR = 0x00; - static const uint8_t WARNING = 0x01; - static const uint8_t NOTICE = 0x02; - static const uint8_t TEST = 0x03; - static const uint8_t DEBUG = 0x04; - } +namespace INF +{ +static const uint8_t ERROR = 0x00; +static const uint8_t WARNING = 0x01; +static const uint8_t NOTICE = 0x02; +static const uint8_t TEST = 0x03; +static const uint8_t DEBUG = 0x04; +} // namespace INF - namespace ACK { - static const uint8_t NACK = 0x00; - static const uint8_t ACK = 0x01; - } +namespace ACK +{ +static const uint8_t NACK = 0x00; +static const uint8_t ACK = 0x01; +} // namespace ACK - namespace AID { - static const uint8_t ALM = AidALM::MESSAGE_ID; - static const uint8_t EPH = AidEPH::MESSAGE_ID; - static const uint8_t HUI = AidHUI::MESSAGE_ID; - } +namespace AID +{ +static const uint8_t ALM = AidALM::MESSAGE_ID; +static const uint8_t EPH = AidEPH::MESSAGE_ID; +static const uint8_t HUI = AidHUI::MESSAGE_ID; +} // namespace AID - namespace CFG { - static const uint8_t ANT = CfgANT::MESSAGE_ID; - static const uint8_t CFG = CfgCFG::MESSAGE_ID; - static const uint8_t DAT = CfgDAT::MESSAGE_ID; - static const uint8_t GNSS = CfgGNSS::MESSAGE_ID; - static const uint8_t HNR = CfgHNR::MESSAGE_ID; - static const uint8_t INF = CfgINF::MESSAGE_ID; - static const uint8_t DGNSS = CfgDGNSS::MESSAGE_ID; - static const uint8_t MSG = CfgMSG::MESSAGE_ID; - static const uint8_t NAV5 = CfgNAV5::MESSAGE_ID; - static const uint8_t NAVX5 = CfgNAVX5::MESSAGE_ID; - static const uint8_t NMEA = CfgNMEA::MESSAGE_ID; - static const uint8_t PRT = CfgPRT::MESSAGE_ID; - static const uint8_t RATE = CfgRATE::MESSAGE_ID; - static const uint8_t RST = CfgRST::MESSAGE_ID; - static const uint8_t SBAS = CfgSBAS::MESSAGE_ID; - static const uint8_t TMODE3 = CfgTMODE3::MESSAGE_ID; - static const uint8_t USB = CfgUSB::MESSAGE_ID; - } +namespace CFG +{ +static const uint8_t ANT = CfgANT::MESSAGE_ID; +static const uint8_t CFG = CfgCFG::MESSAGE_ID; +static const uint8_t DAT = CfgDAT::MESSAGE_ID; +static const uint8_t GNSS = CfgGNSS::MESSAGE_ID; +static const uint8_t HNR = CfgHNR::MESSAGE_ID; +static const uint8_t INF = CfgINF::MESSAGE_ID; +static const uint8_t DGNSS = CfgDGNSS::MESSAGE_ID; +static const uint8_t MSG = CfgMSG::MESSAGE_ID; +static const uint8_t NAV5 = CfgNAV5::MESSAGE_ID; +static const uint8_t NAVX5 = CfgNAVX5::MESSAGE_ID; +static const uint8_t NMEA = CfgNMEA::MESSAGE_ID; +static const uint8_t PRT = CfgPRT::MESSAGE_ID; +static const uint8_t RATE = CfgRATE::MESSAGE_ID; +static const uint8_t RST = CfgRST::MESSAGE_ID; +static const uint8_t SBAS = CfgSBAS::MESSAGE_ID; +static const uint8_t TMODE3 = CfgTMODE3::MESSAGE_ID; +static const uint8_t USB = CfgUSB::MESSAGE_ID; +} // namespace CFG - namespace UPD { - //! SOS and SOS_Ack have the same message ID, but different lengths - static const uint8_t SOS = UpdSOS::MESSAGE_ID; - } - - namespace MON { - static const uint8_t GNSS = MonGNSS::MESSAGE_ID; - static const uint8_t HW = MonHW::MESSAGE_ID; - static const uint8_t VER = MonVER::MESSAGE_ID; - } +namespace UPD +{ +//! SOS and SOS_Ack have the same message ID, but different lengths +static const uint8_t SOS = UpdSOS::MESSAGE_ID; +} // namespace UPD - namespace ESF { - static const uint8_t INS = EsfINS::MESSAGE_ID; - static const uint8_t MEAS = EsfMEAS::MESSAGE_ID; - static const uint8_t RAW = EsfRAW::MESSAGE_ID; - static const uint8_t STATUS = EsfSTATUS::MESSAGE_ID; - } +namespace MON +{ +static const uint8_t GNSS = MonGNSS::MESSAGE_ID; +static const uint8_t HW = MonHW::MESSAGE_ID; +static const uint8_t VER = MonVER::MESSAGE_ID; +} // namespace MON - namespace MGA { - static const uint8_t GAL = MgaGAL::MESSAGE_ID; - } +namespace ESF +{ +static const uint8_t INS = EsfINS::MESSAGE_ID; +static const uint8_t MEAS = EsfMEAS::MESSAGE_ID; +static const uint8_t RAW = EsfRAW::MESSAGE_ID; +static const uint8_t STATUS = EsfSTATUS::MESSAGE_ID; +} // namespace ESF - namespace HNR { - static const uint8_t PVT = HnrPVT::MESSAGE_ID; - } +namespace MGA +{ +static const uint8_t GAL = MgaGAL::MESSAGE_ID; +} + +namespace HNR +{ +static const uint8_t PVT = HnrPVT::MESSAGE_ID; +} - namespace TIM { - static const uint8_t TM2 = TimTM2::MESSAGE_ID; - } +namespace TIM +{ +static const uint8_t TM2 = TimTM2::MESSAGE_ID; } +} // namespace Message -} //!< namespace ublox_msgs +} // namespace ublox_msgs -#endif //!< UBLOX_MSGS_H +#endif //!< UBLOX_MSGS_H diff --git a/ublox_msgs/src/ublox_msgs.cpp b/ublox_msgs/src/ublox_msgs.cpp index 7a6b90d3..ca2abd3a 100644 --- a/ublox_msgs/src/ublox_msgs.cpp +++ b/ublox_msgs/src/ublox_msgs.cpp @@ -14,9 +14,9 @@ // 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 +// 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 BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; @@ -29,176 +29,95 @@ #include template -std::vector > ublox::Message::keys_; - -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::ATT, - ublox_msgs, NavATT); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::CLOCK, - ublox_msgs, NavCLOCK); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::DGPS, - ublox_msgs, NavDGPS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::DOP, - ublox_msgs, NavDOP); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::HPPOSECEF, - ublox_msgs, NavHPPOSECEF); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::HPPOSLLH, - ublox_msgs, NavHPPOSLLH); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::POSECEF, - ublox_msgs, NavPOSECEF); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::POSLLH, - ublox_msgs, NavPOSLLH); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, - ublox_msgs::Message::NAV::RELPOSNED, - ublox_msgs, - NavRELPOSNED); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, - ublox_msgs::Message::NAV::RELPOSNED9, - ublox_msgs, - NavRELPOSNED9); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SBAS, - ublox_msgs, NavSBAS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SOL, - ublox_msgs, NavSOL); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::PVT, - ublox_msgs, NavPVT); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::PVT, - ublox_msgs, NavPVT7); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SAT, - ublox_msgs, NavSAT); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::STATUS, - ublox_msgs, NavSTATUS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SVIN, - ublox_msgs, NavSVIN); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SVINFO, - ublox_msgs, NavSVINFO); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::TIMEGPS, - ublox_msgs, NavTIMEGPS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::TIMEUTC, - ublox_msgs, NavTIMEUTC); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::VELECEF, - ublox_msgs, NavVELECEF); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::VELNED, - ublox_msgs, NavVELNED); - -// ACK messages are declared differently because they both have the same +std::vector > ublox::Message::keys_; + +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::ATT, ublox_msgs, NavATT); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::CLOCK, ublox_msgs, NavCLOCK); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::DGPS, ublox_msgs, NavDGPS); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::DOP, ublox_msgs, NavDOP); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::HPPOSECEF, ublox_msgs, NavHPPOSECEF); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::HPPOSLLH, ublox_msgs, NavHPPOSLLH); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::POSECEF, ublox_msgs, NavPOSECEF); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::POSLLH, ublox_msgs, NavPOSLLH); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::RELPOSNED, ublox_msgs, NavRELPOSNED); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::RELPOSNED9, ublox_msgs, NavRELPOSNED9); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SBAS, ublox_msgs, NavSBAS); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SOL, ublox_msgs, NavSOL); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::PVT, ublox_msgs, NavPVT); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::PVT, ublox_msgs, NavPVT7); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SAT, ublox_msgs, NavSAT); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::STATUS, ublox_msgs, NavSTATUS); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SVIN, ublox_msgs, NavSVIN); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SVINFO, ublox_msgs, NavSVINFO); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::TIMEGPS, ublox_msgs, NavTIMEGPS); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::TIMEUTC, ublox_msgs, NavTIMEUTC); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::VELECEF, ublox_msgs, NavVELECEF); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::VELNED, ublox_msgs, NavVELNED); + +// ACK messages are declared differently because they both have the same // protocol, so only 1 ROS message is used -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ACK, ublox_msgs::Message::ACK::NACK, - ublox_msgs, Ack); -DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::ACK, ublox_msgs::Message::ACK::ACK, - ublox_msgs, Ack, ACK); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ACK, ublox_msgs::Message::ACK::NACK, ublox_msgs, Ack); +DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::ACK, ublox_msgs::Message::ACK::ACK, ublox_msgs, Ack, ACK); -// INF messages are declared differently because they all have the same +// INF messages are declared differently because they all have the same // protocol, so only 1 ROS message is used. DECLARE_UBLOX_MESSAGE can only // be called once, and DECLARE_UBLOX_MESSAGE_ID is called for the following // messages -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::INF, ublox_msgs::Message::INF::ERROR, - ublox_msgs, Inf); -DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, - ublox_msgs::Message::INF::WARNING, - ublox_msgs, Inf, WARNING); -DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, - ublox_msgs::Message::INF::NOTICE, - ublox_msgs, Inf, NOTICE); -DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, - ublox_msgs::Message::INF::TEST, - ublox_msgs, Inf, TEST); -DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, - ublox_msgs::Message::INF::DEBUG, - ublox_msgs, Inf, DEBUG); - -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::ALM, - ublox_msgs, RxmALM); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::EPH, - ublox_msgs, RxmEPH); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::RAW, - ublox_msgs, RxmRAW); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::RAWX, - ublox_msgs, RxmRAWX); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::RTCM, - ublox_msgs, RxmRTCM); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::SFRB, - ublox_msgs, RxmSFRB); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::SFRBX, - ublox_msgs, RxmSFRBX); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::SVSI, - ublox_msgs, RxmSVSI); - -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::ANT, - ublox_msgs, CfgANT); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::CFG, - ublox_msgs, CfgCFG); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::DAT, - ublox_msgs, CfgDAT); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::DGNSS, - ublox_msgs, CfgDGNSS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::GNSS, - ublox_msgs, CfgGNSS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::HNR, - ublox_msgs, CfgHNR); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::INF, - ublox_msgs, CfgINF); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::MSG, - ublox_msgs, CfgMSG); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NAV5, - ublox_msgs, CfgNAV5); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NAVX5, - ublox_msgs, CfgNAVX5); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NMEA, - ublox_msgs, CfgNMEA); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NMEA, - ublox_msgs, CfgNMEA6); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NMEA, - ublox_msgs, CfgNMEA7); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::PRT, - ublox_msgs, CfgPRT); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::RATE, - ublox_msgs, CfgRATE); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::RST, - ublox_msgs, CfgRST); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::TMODE3, - ublox_msgs, CfgTMODE3); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::USB, - ublox_msgs, CfgUSB); - -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::UPD, ublox_msgs::Message::UPD::SOS, - ublox_msgs, UpdSOS); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::INF, ublox_msgs::Message::INF::ERROR, ublox_msgs, Inf); +DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, ublox_msgs::Message::INF::WARNING, ublox_msgs, Inf, WARNING); +DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, ublox_msgs::Message::INF::NOTICE, ublox_msgs, Inf, NOTICE); +DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, ublox_msgs::Message::INF::TEST, ublox_msgs, Inf, TEST); +DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, ublox_msgs::Message::INF::DEBUG, ublox_msgs, Inf, DEBUG); + +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::ALM, ublox_msgs, RxmALM); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::EPH, ublox_msgs, RxmEPH); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::RAW, ublox_msgs, RxmRAW); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::RAWX, ublox_msgs, RxmRAWX); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::RTCM, ublox_msgs, RxmRTCM); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::SFRB, ublox_msgs, RxmSFRB); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::SFRBX, ublox_msgs, RxmSFRBX); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::SVSI, ublox_msgs, RxmSVSI); + +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::ANT, ublox_msgs, CfgANT); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::CFG, ublox_msgs, CfgCFG); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::DAT, ublox_msgs, CfgDAT); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::DGNSS, ublox_msgs, CfgDGNSS); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::GNSS, ublox_msgs, CfgGNSS); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::HNR, ublox_msgs, CfgHNR); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::INF, ublox_msgs, CfgINF); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::MSG, ublox_msgs, CfgMSG); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NAV5, ublox_msgs, CfgNAV5); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NAVX5, ublox_msgs, CfgNAVX5); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NMEA, ublox_msgs, CfgNMEA); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NMEA, ublox_msgs, CfgNMEA6); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NMEA, ublox_msgs, CfgNMEA7); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::PRT, ublox_msgs, CfgPRT); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::RATE, ublox_msgs, CfgRATE); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::RST, ublox_msgs, CfgRST); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::TMODE3, ublox_msgs, CfgTMODE3); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::USB, ublox_msgs, CfgUSB); + +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::UPD, ublox_msgs::Message::UPD::SOS, ublox_msgs, UpdSOS); // SOS and SOS_Ack have the same message ID, but different lengths -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::UPD, ublox_msgs::Message::UPD::SOS, - ublox_msgs, UpdSOS_Ack); - -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::GNSS, - ublox_msgs, MonGNSS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::HW, - ublox_msgs, MonHW); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::HW, - ublox_msgs, MonHW6); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::VER, - ublox_msgs, MonVER); - -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::ALM, - ublox_msgs, AidALM); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::EPH, - ublox_msgs, AidEPH); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::HUI, - ublox_msgs, AidHUI); - -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::INS, - ublox_msgs, EsfINS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::MEAS, - ublox_msgs, EsfMEAS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::RAW, - ublox_msgs, EsfRAW); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::STATUS, - ublox_msgs, EsfSTATUS); - - -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MGA, ublox_msgs::Message::MGA::GAL, - ublox_msgs, MgaGAL); - -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::HNR, ublox_msgs::Message::HNR::PVT, - ublox_msgs, HnrPVT); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::UPD, ublox_msgs::Message::UPD::SOS, ublox_msgs, UpdSOS_Ack); -// TIM messages -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::TIM, ublox_msgs::Message::TIM::TM2, - ublox_msgs, TimTM2); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::GNSS, ublox_msgs, MonGNSS); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::HW, ublox_msgs, MonHW); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::HW, ublox_msgs, MonHW6); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::VER, ublox_msgs, MonVER); + +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::ALM, ublox_msgs, AidALM); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::EPH, ublox_msgs, AidEPH); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::HUI, ublox_msgs, AidHUI); + +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::INS, ublox_msgs, EsfINS); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::MEAS, ublox_msgs, EsfMEAS); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::RAW, ublox_msgs, EsfRAW); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::STATUS, ublox_msgs, EsfSTATUS); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MGA, ublox_msgs::Message::MGA::GAL, ublox_msgs, MgaGAL); + +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::HNR, ublox_msgs::Message::HNR::PVT, ublox_msgs, HnrPVT); + +// TIM messages +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::TIM, ublox_msgs::Message::TIM::TM2, ublox_msgs, TimTM2); diff --git a/ublox_msgs/srv/GetVersionInfo.srv b/ublox_msgs/srv/GetVersionInfo.srv new file mode 100644 index 00000000..b0336a66 --- /dev/null +++ b/ublox_msgs/srv/GetVersionInfo.srv @@ -0,0 +1,7 @@ + +--- +string hardware +string software +string firmware +string protocol +string device_model \ No newline at end of file diff --git a/ublox_serialization/include/ublox/checksum.h b/ublox_serialization/include/ublox/checksum.h index 38041390..5b271129 100644 --- a/ublox_serialization/include/ublox/checksum.h +++ b/ublox_serialization/include/ublox/checksum.h @@ -14,9 +14,9 @@ // 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 +// 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 BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; @@ -31,21 +31,20 @@ #include -namespace ublox { - +namespace ublox +{ /** * @brief calculate the checksum of a u-blox_message - * @param data the start of the u-blox message + * @param data the start of the u-blox message * @param data the size of the u-blox message * @param ck_a the checksum a output * @param ck_b the checksum b output */ -static inline void calculateChecksum(const uint8_t *data, - uint32_t size, - uint8_t &ck_a, - uint8_t &ck_b) { - ck_a = 0; ck_b = 0; - for(uint32_t i = 0; i < size; ++i) +static inline void calculateChecksum(const uint8_t* data, uint32_t size, uint8_t& ck_a, uint8_t& ck_b) +{ + ck_a = 0; + ck_b = 0; + for (uint32_t i = 0; i < size; ++i) { ck_a = ck_a + data[i]; ck_b = ck_b + ck_a; @@ -54,19 +53,18 @@ static inline void calculateChecksum(const uint8_t *data, /** * @brief calculate the checksum of a u-blox_message. - * @param data the start of the u-blox message + * @param data the start of the u-blox message * @param data the size of the u-blox message * @param checksum the checksum output * @return the checksum */ -static inline uint16_t calculateChecksum(const uint8_t *data, - uint32_t size, - uint16_t &checksum) { - uint8_t *byte = reinterpret_cast(&checksum); +static inline uint16_t calculateChecksum(const uint8_t* data, uint32_t size, uint16_t& checksum) +{ + uint8_t* byte = reinterpret_cast(&checksum); calculateChecksum(data, size, byte[0], byte[1]); return checksum; } -} // namespace ublox +} // namespace ublox -#endif // UBLOX_MSGS_CHECKSUM_H +#endif // UBLOX_MSGS_CHECKSUM_H diff --git a/ublox_serialization/include/ublox/serialization.h b/ublox_serialization/include/ublox/serialization.h index d664d626..30ffd914 100644 --- a/ublox_serialization/include/ublox/serialization.h +++ b/ublox_serialization/include/ublox/serialization.h @@ -14,9 +14,9 @@ // 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 +// 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 BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; @@ -39,62 +39,59 @@ /// /// This file defines the Serializer template class which encodes and decodes -/// specific message types. -/// The Reader class decodes messages and from a buffer and the Writer class +/// specific message types. +/// The Reader class decodes messages and from a buffer and the Writer class /// encodes messages and writes them to a buffer. /// It also declares macros for declaring Messages. The Message class /// maps ROS messages types to class and message ID(s). /// - /** * @namespace ublox * This namespace is for u-blox message serialization. */ -namespace ublox { - +namespace ublox +{ //! u-blox message Sync A char -static const uint8_t DEFAULT_SYNC_A = 0xB5; +static const uint8_t DEFAULT_SYNC_A = 0xB5; //! u-blox message Sync B char -static const uint8_t DEFAULT_SYNC_B = 0x62; +static const uint8_t DEFAULT_SYNC_B = 0x62; //! Maximum payload length static const uint32_t kMaxPayloadLength = 8184; // == (buffer size - header length - checksum length) //! Number of bytes in a message header (Sync chars + class ID + message ID) -static const uint8_t kHeaderLength = 6; +static const uint8_t kHeaderLength = 6; //! Number of checksum bytes in the u-blox message -static const uint8_t kChecksumLength = 2; +static const uint8_t kChecksumLength = 2; /** * @brief Encodes and decodes messages. */ template -struct Serializer { +struct Serializer +{ /** * @brief Decode the message payload from the data buffer. * @param data a pointer to the start of the message payload * @param count the number of bytes in the message payload * @param message the output message */ - static void read(const uint8_t *data, uint32_t count, - typename boost::call_traits::reference message); + static void read(const uint8_t* data, uint32_t count, typename boost::call_traits::reference message); /** * @brief Get the length of the message payload in bytes. - * + * * @details The payload does not include the header or checksum. * @param message the message to get the length of * @return the length of the message in bytes. */ - static uint32_t serializedLength( - typename boost::call_traits::param_type message); - + static uint32_t serializedLength(typename boost::call_traits::param_type message); + /** * @brief Encode the message payload as a byte array. * @param data a buffer to fill with the message payload bytes * @param size the length of the buffer * @param message the output message */ - static void write(uint8_t *data, uint32_t size, - typename boost::call_traits::param_type message); + static void write(uint8_t* data, uint32_t size, typename boost::call_traits::param_type message); }; /** @@ -102,86 +99,98 @@ struct Serializer { * message type. */ template -class Message { - public: +class Message +{ +public: /** * @brief Can this message type decode a u-blox message with the given ID? * @param class_id the class ID of the u-blox message * @param message_id the message ID of the u-blox message * @return whether or not this message type decode the u-blox message */ - static bool canDecode(uint8_t class_id, uint8_t message_id) { - return std::find(keys_.begin(), keys_.end(), - std::make_pair(class_id, message_id)) != keys_.end(); + static bool canDecode(uint8_t class_id, uint8_t message_id) + { + return std::find(keys_.begin(), keys_.end(), std::make_pair(class_id, message_id)) != keys_.end(); } - + /** - * @brief Indicate that this message type can decode u-blox messages with the + * @brief Indicate that this message type can decode u-blox messages with the * given ID * @param class_id the class ID of the u-blox message * @param message_id the message ID of the u-blox message */ - static void addKey(uint8_t class_id, uint8_t message_id) { + static void addKey(uint8_t class_id, uint8_t message_id) + { keys_.push_back(std::make_pair(class_id, message_id)); } struct StaticKeyInitializer { - StaticKeyInitializer(uint8_t class_id, uint8_t message_id) { - Message::addKey(class_id, message_id); + StaticKeyInitializer(uint8_t class_id, uint8_t message_id) + { + Message::addKey(class_id, message_id); } }; - private: - static std::vector > keys_; +private: + static std::vector > keys_; }; /** * @brief Options for the Reader and Writer for encoding and decoding messages. */ -struct Options { +struct Options +{ /** * The default options for a u-blox message. */ - Options() : sync_a(DEFAULT_SYNC_A), sync_b(DEFAULT_SYNC_B), - max_payload_length(kMaxPayloadLength), - header_length(kHeaderLength), checksum_length(kChecksumLength) {} + Options() + : sync_a(DEFAULT_SYNC_A) + , sync_b(DEFAULT_SYNC_B) + , max_payload_length(kMaxPayloadLength) + , header_length(kHeaderLength) + , checksum_length(kChecksumLength) + { + } //! The sync_a byte value identifying the start of a message - uint8_t sync_a; + uint8_t sync_a; //! The sync_b byte value identifying the start of a message - uint8_t sync_b; + uint8_t sync_b; //! The maximum payload length. uint32_t max_payload_length; //! The length of the message header in bytes (everything before the payload) - uint8_t header_length; + uint8_t header_length; //! The length of the checksum in bytes - uint8_t checksum_length; - + uint8_t checksum_length; + /** * @brief Get the number of bytes in the header and footer. * @return the number of bytes in the header and footer */ - int wrapper_length() { - return header_length + checksum_length; + int wrapper_length() + { + return header_length + checksum_length; } }; -/** +/** * @brief Decodes byte messages into u-blox ROS messages. */ -class Reader { - public: +class Reader +{ +public: /** * @param data a buffer containing u-blox messages * @param count the size of the buffer - * @param options A struct containing the parameters sync_a and sync_b + * @param options A struct containing the parameters sync_a and sync_b * which represent the sync bytes indicating the beginning of the message */ - Reader(const uint8_t *data, uint32_t count, - const Options &options = Options()) : - data_(data), count_(count), found_(false), options_(options) {} + Reader(const uint8_t* data, uint32_t count, const Options& options = Options()) + : data_(data), count_(count), found_(false), options_(options) + { + } - typedef const uint8_t *iterator; + typedef const uint8_t* iterator; /** * @brief Search the buffer for the beginning of the next u-blox message @@ -189,19 +198,24 @@ class Reader { */ iterator search() { - if (found_) next(); + if (found_) + next(); // Search for a message header - for( ; count_ > 0; --count_, ++data_) { - if (data_[0] == options_.sync_a && - (count_ == 1 || data_[1] == options_.sync_b)) { + for (; count_ > 0; --count_, ++data_) + { + if (data_[0] == options_.sync_a && (count_ == 1 || data_[1] == options_.sync_b)) + { // Ignore messages which exceed the maximum payload length - if (length() > options_.max_payload_length) { + if (length() > options_.max_payload_length) + { // Message exceeds maximum payload length ROS_ERROR("U-Blox message exceeds maximum payload length %u: " - "0x%02x / 0x%02x", options_.max_payload_length, - classId(), messageId()); - continue; + "0x%02x / 0x%02x", + options_.max_payload_length, + classId(), + messageId()); + continue; } break; } @@ -216,31 +230,37 @@ class Reader { */ bool found() { - if (found_) return true; + if (found_) + return true; // Verify message is long enough to have sync chars, id, length & checksum - if (count_ < options_.wrapper_length()) return false; + if (count_ < options_.wrapper_length()) + return false; // Verify the header bits - if (data_[0] != options_.sync_a || data_[1] != options_.sync_b) + if (data_[0] != options_.sync_a || data_[1] != options_.sync_b) return false; // Verify that the buffer length is long enough based on the received // message length - if (count_ < length() + options_.wrapper_length()) return false; + if (count_ < length() + options_.wrapper_length()) + return false; found_ = true; return true; } /** - * @brief Go to the start of the next message based on the received message + * @brief Go to the start of the next message based on the received message * length. * - * @details Warning: Does not go to the correct byte location if the received + * @details Warning: Does not go to the correct byte location if the received * message length is incorrect. If this is the case, search must be called. */ - iterator next() { - if (found()) { + iterator next() + { + if (found()) + { uint32_t size = length() + options_.wrapper_length(); - data_ += size; count_ -= size; + data_ += size; + count_ -= size; } found_ = false; return data_; @@ -250,16 +270,24 @@ class Reader { * @brief Get the current position in the read buffer. * @return the current position of the read buffer */ - iterator pos() { + iterator pos() + { return data_; } - iterator end() { + iterator end() + { return data_ + count_; } - uint8_t classId() { return data_[2]; } - uint8_t messageId() { return data_[3]; } + uint8_t classId() + { + return data_[2]; + } + uint8_t messageId() + { + return data_[3]; + } /** * @brief Get the length of the u-blox message payload. @@ -268,20 +296,25 @@ class Reader { * Determines the length from the header of the u-blox message. * @return the length of the message payload */ - uint32_t length() { - if (count_ < 6) return 0u; + uint32_t length() + { + if (count_ < 6) + return 0u; return (data_[5] << 8) + data_[4]; } - const uint8_t *data() { return data_ + options_.header_length; } - + const uint8_t* data() + { + return data_ + options_.header_length; + } + /** * @brief Get the checksum of the u-blox message. * * @return the checksum of the u-blox message */ - uint16_t checksum() { - return *reinterpret_cast(data_ + options_.header_length + - length()); + uint16_t checksum() + { + return *reinterpret_cast(data_ + options_.header_length + length()); } /** @@ -290,17 +323,20 @@ class Reader { * @param search whether or not to skip to the next message in the buffer */ template - bool read(typename boost::call_traits::reference message, - bool search = false) { - if (search) this->search(); - if (!found()) return false; - if (!Message::canDecode(classId(), messageId())) return false; + bool read(typename boost::call_traits::reference message, bool search = false) + { + if (search) + this->search(); + if (!found()) + return false; + if (!Message::canDecode(classId(), messageId())) + return false; uint16_t chk; - if (calculateChecksum(data_ + 2, length() + 4, chk) != this->checksum()) { + if (calculateChecksum(data_ + 2, length() + 4, chk) != this->checksum()) + { // checksum error - ROS_DEBUG("U-Blox read checksum error: 0x%02x / 0x%02x", classId(), - messageId()); + ROS_DEBUG("U-Blox read checksum error: 0x%02x / 0x%02x", classId(), messageId()); return false; } @@ -310,42 +346,47 @@ class Reader { /** * @brief Can the given message type decode the current message in the buffer? - * @return whether the given message type can decode the current message in + * @return whether the given message type can decode the current message in * the buffer */ - template - bool hasType() { - if (!found()) return false; + template + bool hasType() + { + if (!found()) + return false; return Message::canDecode(classId(), messageId()); } /** * @brief Does the u-blox message have the given class and message ID? - * @return Whether or not the u-blox message has the given class and message + * @return Whether or not the u-blox message has the given class and message * ID */ - bool isMessage(uint8_t class_id, uint8_t message_id) { - if (!found()) return false; + bool isMessage(uint8_t class_id, uint8_t message_id) + { + if (!found()) + return false; return (classId() == class_id && messageId() == message_id); } - private: +private: //! The buffer of message bytes - const uint8_t *data_; + const uint8_t* data_; //! the number of bytes in the buffer, //! decrement as the buffer is read - uint32_t count_; + uint32_t count_; //! Whether or not a message has been found - bool found_; + bool found_; //! Options representing the sync char values, etc. - Options options_; + Options options_; }; -/** +/** * @brief Encodes a u-blox ROS message as a byte array. */ -class Writer { - public: - typedef uint8_t *iterator; +class Writer +{ +public: + typedef uint8_t* iterator; /** * @brief Construct a Writer with the given buffer. @@ -353,8 +394,9 @@ class Writer { * @param size the size of the buffer * @param options options representing the message sync chars, etc. */ - Writer(uint8_t *data, uint32_t size, const Options &options = Options()) : - data_(data), size_(size), options_(options) {} + Writer(uint8_t* data, uint32_t size, const Options& options = Options()) : data_(data), size_(size), options_(options) + { + } /** * @brief Encode the u-blox message. @@ -363,19 +405,18 @@ class Writer { * @param message_id the u-blox message ID, defaults to the message MESSAGE_ID * @return true if the message was encoded correctly, false otherwise */ - template bool write(const T& message, - uint8_t class_id = T::CLASS_ID, - uint8_t message_id = T::MESSAGE_ID) { + template + bool write(const T& message, uint8_t class_id = T::CLASS_ID, uint8_t message_id = T::MESSAGE_ID) + { // Check for buffer overflow uint32_t length = Serializer::serializedLength(message); - if (size_ < length + options_.wrapper_length()) { - ROS_ERROR("u-blox write buffer overflow. Message %u / %u not written", - class_id, message_id); + if (size_ < length + options_.wrapper_length()) + { + ROS_ERROR("u-blox write buffer overflow. Message %u / %u not written", class_id, message_id); return false; } // Encode the message and add it to the buffer - Serializer::write(data_ + options_.header_length, - size_ - options_.header_length, message); + Serializer::write(data_ + options_.header_length, size_ - options_.header_length, message); return write(0, length, class_id, message_id); } @@ -388,11 +429,11 @@ class Writer { * @param message_id the u-blox message ID * @return true if the message was encoded correctly, false otherwise */ - bool write(const uint8_t* message, uint32_t length, uint8_t class_id, - uint8_t message_id) { - if (size_ < length + options_.wrapper_length()) { - ROS_ERROR("u-blox write buffer overflow. Message %u / %u not written", - class_id, message_id); + bool write(const uint8_t* message, uint32_t length, uint8_t class_id, uint8_t message_id) + { + if (size_ < length + options_.wrapper_length()) + { + ROS_ERROR("u-blox write buffer overflow. Message %u / %u not written", class_id, message_id); return false; } iterator start = data_; @@ -407,7 +448,8 @@ class Writer { size_ -= options_.header_length; // write message - if (message) std::copy(message, message + length, data_); + if (message) + std::copy(message, message + length, data_); data_ += length; size_ -= length; @@ -421,40 +463,48 @@ class Writer { return true; } - iterator end() { + iterator end() + { return data_; } - private: +private: //! The buffer of message bytes - iterator data_; + iterator data_; //! The number of remaining bytes in the buffer /*! Decrements as messages are written to the buffer */ - uint32_t size_; + uint32_t size_; //! Options representing the sync char values, etc. - Options options_; + Options options_; }; -} // namespace ublox +} // namespace ublox // Use to declare u-blox messages and message serializers -#define DECLARE_UBLOX_MESSAGE(class_id, message_id, package, message) \ - template class ublox::Serializer; \ - template class ublox::Message; \ - namespace package { namespace { \ - static const ublox::Message::StaticKeyInitializer static_key_initializer_##message(class_id, message_id); \ - } } \ +#define DECLARE_UBLOX_MESSAGE(class_id, message_id, package, message) \ + template class ublox::Serializer; \ + template class ublox::Message; \ + namespace package \ + { \ + namespace \ + { \ + static const ublox::Message::StaticKeyInitializer static_key_initializer_##message(class_id, message_id); \ + } \ + } // Use for messages which have the same structure but different IDs, e.g. INF // Call DECLARE_UBLOX_MESSAGE for the first message and DECLARE_UBLOX_MESSAGE_ID // for following declarations -#define DECLARE_UBLOX_MESSAGE_ID(class_id, message_id, package, message, name) \ - namespace package { namespace { \ - static const ublox::Message::StaticKeyInitializer static_key_initializer_##name(class_id, message_id); \ - } } \ - +#define DECLARE_UBLOX_MESSAGE_ID(class_id, message_id, package, message, name) \ + namespace package \ + { \ + namespace \ + { \ + static const ublox::Message::StaticKeyInitializer static_key_initializer_##name(class_id, message_id); \ + } \ + } // use implementation of class Serializer in "serialization_ros.h" #include "serialization_ros.h" -#endif // UBLOX_SERIALIZATION_H +#endif // UBLOX_SERIALIZATION_H diff --git a/ublox_serialization/include/ublox/serialization_ros.h b/ublox_serialization/include/ublox/serialization_ros.h index 9236ff46..2e3b3c0d 100644 --- a/ublox_serialization/include/ublox/serialization_ros.h +++ b/ublox_serialization/include/ublox/serialization_ros.h @@ -14,9 +14,9 @@ // 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 +// 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 BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; @@ -34,28 +34,28 @@ #include -namespace ublox { - +namespace ublox +{ template -void Serializer::read(const uint8_t *data, uint32_t count, - typename boost::call_traits::reference message) { - ros::serialization::IStream stream(const_cast(data), count); +void Serializer::read(const uint8_t* data, uint32_t count, typename boost::call_traits::reference message) +{ + ros::serialization::IStream stream(const_cast(data), count); ros::serialization::Serializer::read(stream, message); } template -uint32_t Serializer::serializedLength( - typename boost::call_traits::param_type message) { +uint32_t Serializer::serializedLength(typename boost::call_traits::param_type message) +{ return ros::serialization::Serializer::serializedLength(message); } template -void Serializer::write(uint8_t *data, uint32_t size, - typename boost::call_traits::param_type message) { +void Serializer::write(uint8_t* data, uint32_t size, typename boost::call_traits::param_type message) +{ ros::serialization::OStream stream(data, size); ros::serialization::Serializer::write(stream, message); } -} // namespace ublox +} // namespace ublox -#endif // UBLOX_SERIALIZATION_ROS_H +#endif // UBLOX_SERIALIZATION_ROS_H From ec4a04b72d569e97c5ab784b7e4407d0afe1a534 Mon Sep 17 00:00:00 2001 From: Jonathan Sanabria Date: Wed, 4 May 2022 16:41:00 -0400 Subject: [PATCH 06/17] clang formatting of files. --- ublox_gps/include/ublox_gps/async_worker.h | 179 ++- ublox_gps/include/ublox_gps/callback.h | 138 +- ublox_gps/include/ublox_gps/gps.h | 153 +- ublox_gps/include/ublox_gps/mkgmtime.h | 8 +- ublox_gps/include/ublox_gps/node.h | 419 ++--- ublox_gps/include/ublox_gps/raw_data_pa.h | 165 +- ublox_gps/include/ublox_gps/utils.h | 19 +- ublox_gps/include/ublox_gps/worker.h | 21 +- ublox_gps/src/gps.cpp | 416 ++--- ublox_gps/src/logger_node_pa.cpp | 16 +- ublox_gps/src/node.cpp | 1362 +++++++++-------- ublox_gps/src/raw_data_pa.cpp | 255 +-- .../include/ublox_msg_filters/exact_time.h | 184 ++- ublox_msg_filters/src/example.cpp | 13 +- ublox_msg_filters/tests/test.cpp | 73 +- .../include/ublox/serialization/ublox_msgs.h | 501 +++--- ublox_msgs/include/ublox_msgs/ublox_msgs.h | 262 ++-- ublox_msgs/src/ublox_msgs.cpp | 255 ++- ublox_serialization/include/ublox/checksum.h | 36 +- .../include/ublox/serialization.h | 326 ++-- .../include/ublox/serialization_ros.h | 28 +- 21 files changed, 2572 insertions(+), 2257 deletions(-) diff --git a/ublox_gps/include/ublox_gps/async_worker.h b/ublox_gps/include/ublox_gps/async_worker.h index 19c18e29..ae741c4f 100644 --- a/ublox_gps/include/ublox_gps/async_worker.h +++ b/ublox_gps/include/ublox_gps/async_worker.h @@ -37,19 +37,19 @@ #include #include - #include "worker.h" -namespace ublox_gps { - -int debug; //!< Used to determine which debug messages to display +namespace ublox_gps +{ +int debug; //!< Used to determine which debug messages to display /** * @brief Handles Asynchronous I/O reading and writing. */ template -class AsyncWorker : public Worker { - public: +class AsyncWorker : public Worker +{ +public: typedef boost::mutex Mutex; typedef boost::mutex::scoped_lock ScopedLock; @@ -68,13 +68,19 @@ class AsyncWorker : public Worker { * @brief Set the callback function which handles input messages. * @param callback the read callback which handles received messages */ - void setCallback(const Callback& callback) { read_callback_ = callback; } + void setCallback(const Callback& callback) + { + read_callback_ = callback; + } /** * @brief Set the callback function which handles raw data. * @param callback the write callback which handles raw data */ - void setRawDataCallback(const Callback& callback) { write_callback_ = callback; } + void setRawDataCallback(const Callback& callback) + { + write_callback_ = callback; + } /** * @brief Send the data bytes via the I/O stream. @@ -88,9 +94,12 @@ class AsyncWorker : public Worker { */ void wait(const boost::posix_time::time_duration& timeout); - bool isOpen() const { return stream_->is_open(); } + bool isOpen() const + { + return stream_->is_open(); + } - protected: +protected: /** * @brief Read the input stream. */ @@ -113,32 +122,33 @@ class AsyncWorker : public Worker { */ void doClose(); - boost::shared_ptr stream_; //!< The I/O stream - boost::shared_ptr io_service_; //!< The I/O service + boost::shared_ptr stream_; //!< The I/O stream + boost::shared_ptr io_service_; //!< The I/O service - Mutex read_mutex_; //!< Lock for the input buffer + Mutex read_mutex_; //!< Lock for the input buffer boost::condition read_condition_; - std::vector in_; //!< The input buffer - std::size_t in_buffer_size_; //!< number of bytes currently in the input - //!< buffer + std::vector in_; //!< The input buffer + std::size_t in_buffer_size_; //!< number of bytes currently in the input + //!< buffer - Mutex write_mutex_; //!< Lock for the output buffer + Mutex write_mutex_; //!< Lock for the output buffer boost::condition write_condition_; - std::vector out_; //!< The output buffer + std::vector out_; //!< The output buffer - boost::shared_ptr background_thread_; //!< thread for the I/O - //!< service - Callback read_callback_; //!< Callback function to handle received messages - Callback write_callback_; //!< Callback function to handle raw data + boost::shared_ptr background_thread_; //!< thread for the I/O + //!< service + Callback read_callback_; //!< Callback function to handle received messages + Callback write_callback_; //!< Callback function to handle raw data - bool stopping_; //!< Whether or not the I/O service is closed + bool stopping_; //!< Whether or not the I/O service is closed }; template AsyncWorker::AsyncWorker(boost::shared_ptr stream, - boost::shared_ptr io_service, - std::size_t buffer_size) - : stopping_(false) { + boost::shared_ptr io_service, + std::size_t buffer_size) + : stopping_(false) +{ stream_ = stream; io_service_ = io_service; in_.resize(buffer_size); @@ -147,27 +157,29 @@ AsyncWorker::AsyncWorker(boost::shared_ptr stream, out_.reserve(buffer_size); io_service_->post(boost::bind(&AsyncWorker::doRead, this)); - background_thread_.reset(new boost::thread( - boost::bind(&boost::asio::io_service::run, io_service_))); + background_thread_.reset(new boost::thread(boost::bind(&boost::asio::io_service::run, io_service_))); } template -AsyncWorker::~AsyncWorker() { +AsyncWorker::~AsyncWorker() +{ io_service_->post(boost::bind(&AsyncWorker::doClose, this)); background_thread_->join(); - //io_service_->reset(); + // io_service_->reset(); } template -bool AsyncWorker::send(const unsigned char* data, - const unsigned int size) { +bool AsyncWorker::send(const unsigned char* data, const unsigned int size) +{ ScopedLock lock(write_mutex_); - if(size == 0) { + if (size == 0) + { ROS_ERROR("Ublox AsyncWorker::send: Size of message to send is 0"); return true; } - if (out_.capacity() - out_.size() < size) { + if (out_.capacity() - out_.size() < size) + { ROS_ERROR("Ublox AsyncWorker::send: Output buffer too full to send message"); return false; } @@ -178,20 +190,22 @@ bool AsyncWorker::send(const unsigned char* data, } template -void AsyncWorker::doWrite() { +void AsyncWorker::doWrite() +{ ScopedLock lock(write_mutex_); // Do nothing if out buffer is empty - if (out_.size() == 0) { + if (out_.size() == 0) + { return; } // Write all the data in the out buffer boost::asio::write(*stream_, boost::asio::buffer(out_.data(), out_.size())); - if (debug >= 2) { + if (debug >= 2) + { // Print the data that was sent std::ostringstream oss; - for (std::vector::iterator it = out_.begin(); - it != out_.end(); ++it) + for (std::vector::iterator it = out_.begin(); it != out_.end(); ++it) oss << boost::format("%02x") % static_cast(*it) << " "; ROS_DEBUG("U-Blox sent %li bytes: \n%s", out_.size(), oss.str().c_str()); } @@ -200,20 +214,22 @@ void AsyncWorker::doWrite() { write_condition_.notify_all(); } template <> -inline void AsyncWorker::doWrite() { +inline void AsyncWorker::doWrite() +{ ScopedLock lock(write_mutex_); // Do nothing if out buffer is empty - if (out_.size() == 0) { + if (out_.size() == 0) + { return; } // Write all the data in the out buffer stream_->send(boost::asio::buffer(out_.data(), out_.size())); - if (debug >= 2) { + if (debug >= 2) + { // Print the data that was sent std::ostringstream oss; - for (std::vector::iterator it = out_.begin(); - it != out_.end(); ++it) + for (std::vector::iterator it = out_.begin(); it != out_.end(); ++it) oss << boost::format("%02x") % static_cast(*it) << " "; ROS_DEBUG("U-Blox sent %li bytes: \n%s", out_.size(), oss.str().c_str()); } @@ -223,51 +239,52 @@ inline void AsyncWorker::doWrite() { } template -void AsyncWorker::doRead() { +void AsyncWorker::doRead() +{ ScopedLock lock(read_mutex_); - stream_->async_read_some( - boost::asio::buffer(in_.data() + in_buffer_size_, - in_.size() - in_buffer_size_), - boost::bind(&AsyncWorker::readEnd, this, - boost::asio::placeholders::error, - boost::asio::placeholders::bytes_transferred)); + stream_->async_read_some(boost::asio::buffer(in_.data() + in_buffer_size_, in_.size() - in_buffer_size_), + boost::bind(&AsyncWorker::readEnd, + this, + boost::asio::placeholders::error, + boost::asio::placeholders::bytes_transferred)); } template <> -inline void AsyncWorker::doRead() { +inline void AsyncWorker::doRead() +{ ScopedLock lock(read_mutex_); - stream_->async_receive( - boost::asio::buffer(in_.data() + in_buffer_size_, - in_.size() - in_buffer_size_), - boost::bind(&AsyncWorker::readEnd, this, - boost::asio::placeholders::error, - boost::asio::placeholders::bytes_transferred)); + stream_->async_receive(boost::asio::buffer(in_.data() + in_buffer_size_, in_.size() - in_buffer_size_), + boost::bind(&AsyncWorker::readEnd, + this, + boost::asio::placeholders::error, + boost::asio::placeholders::bytes_transferred)); } template -void AsyncWorker::readEnd(const boost::system::error_code& error, - std::size_t bytes_transfered) { +void AsyncWorker::readEnd(const boost::system::error_code& error, std::size_t bytes_transfered) +{ ScopedLock lock(read_mutex_); - if (error) { - ROS_ERROR("U-Blox ASIO input buffer read error: %s, %li", - error.message().c_str(), - bytes_transfered); - } else if (bytes_transfered > 0) { + if (error) + { + ROS_ERROR("U-Blox ASIO input buffer read error: %s, %li", error.message().c_str(), bytes_transfered); + } + else if (bytes_transfered > 0) + { in_buffer_size_ += bytes_transfered; - unsigned char *pRawDataStart = &(*(in_.begin() + (in_buffer_size_ - bytes_transfered))); + unsigned char* pRawDataStart = &(*(in_.begin() + (in_buffer_size_ - bytes_transfered))); std::size_t raw_data_stream_size = bytes_transfered; if (write_callback_) write_callback_(pRawDataStart, raw_data_stream_size); - if (debug >= 4) { + if (debug >= 4) + { std::ostringstream oss; - for (std::vector::iterator it = - in_.begin() + in_buffer_size_ - bytes_transfered; - it != in_.begin() + in_buffer_size_; ++it) + for (std::vector::iterator it = in_.begin() + in_buffer_size_ - bytes_transfered; + it != in_.begin() + in_buffer_size_; + ++it) oss << boost::format("%02x") % static_cast(*it) << " "; - ROS_DEBUG("U-Blox received %li bytes \n%s", bytes_transfered, - oss.str().c_str()); + ROS_DEBUG("U-Blox received %li bytes \n%s", bytes_transfered, oss.str().c_str()); } if (read_callback_) @@ -280,9 +297,9 @@ void AsyncWorker::readEnd(const boost::system::error_code& error, // The remaining buffer size in_.size() - in_buffer_size_ must be at least // one byte. Otherwise readEnd() and doRead() start to busy-wait without // a chance to recover. - if (in_buffer_size_ >= in_.size()) { - ROS_ERROR("U-Blox ASIO input buffer overflow, dropping %zu bytes", - in_buffer_size_); + if (in_buffer_size_ >= in_.size()) + { + ROS_ERROR("U-Blox ASIO input buffer overflow, dropping %zu bytes", in_buffer_size_); in_buffer_size_ = 0; } @@ -291,19 +308,19 @@ void AsyncWorker::readEnd(const boost::system::error_code& error, } template -void AsyncWorker::doClose() { +void AsyncWorker::doClose() +{ ScopedLock lock(read_mutex_); stopping_ = true; boost::system::error_code error; stream_->close(error); - if(error) - ROS_ERROR_STREAM( - "Error while closing the AsyncWorker stream: " << error.message()); + if (error) + ROS_ERROR_STREAM("Error while closing the AsyncWorker stream: " << error.message()); } template -void AsyncWorker::wait( - const boost::posix_time::time_duration& timeout) { +void AsyncWorker::wait(const boost::posix_time::time_duration& timeout) +{ ScopedLock lock(read_mutex_); read_condition_.timed_wait(lock, timeout); } diff --git a/ublox_gps/include/ublox_gps/callback.h b/ublox_gps/include/ublox_gps/callback.h index bbf51f2d..809b6f82 100644 --- a/ublox_gps/include/ublox_gps/callback.h +++ b/ublox_gps/include/ublox_gps/callback.h @@ -14,9 +14,9 @@ // 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 +// 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 BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; @@ -35,13 +35,14 @@ #include #include -namespace ublox_gps { - +namespace ublox_gps +{ /** * @brief A callback handler for a u-blox message. */ -class CallbackHandler { - public: +class CallbackHandler +{ +public: /** * @brief Decode the u-blox message. */ @@ -50,14 +51,15 @@ class CallbackHandler { /** * @brief Wait for on the condition. */ - bool wait(const boost::posix_time::time_duration& timeout) { + bool wait(const boost::posix_time::time_duration& timeout) + { boost::mutex::scoped_lock lock(mutex_); return condition_.timed_wait(lock, timeout); } - protected: - boost::mutex mutex_; //!< Lock for the handler - boost::condition_variable condition_; //!< Condition for the handler lock +protected: + boost::mutex mutex_; //!< Lock for the handler + boost::condition_variable condition_; //!< Condition for the handler lock }; /** @@ -65,40 +67,51 @@ class CallbackHandler { * @typedef T the message type */ template -class CallbackHandler_ : public CallbackHandler { - public: - typedef boost::function Callback; //!< A callback function +class CallbackHandler_ : public CallbackHandler +{ +public: + typedef boost::function Callback; //!< A callback function - /** + /** * @brief Initialize the Callback Handler with a callback function * @param func a callback function for the message, defaults to none */ - CallbackHandler_(const Callback& func = Callback()) : func_(func) {} - + CallbackHandler_(const Callback& func = Callback()) : func_(func) + { + } + /** * @brief Get the last received message. */ - virtual const T& get() { return message_; } + virtual const T& get() + { + return message_; + } /** * @brief Decode the U-Blox message & call the callback function if it exists. * @param reader a reader to decode the message buffer */ - void handle(ublox::Reader& reader) { + void handle(ublox::Reader& reader) + { boost::mutex::scoped_lock lock(mutex_); - try { - if (!reader.read(message_)) { - ROS_DEBUG_COND(debug >= 2, - "U-Blox Decoder error for 0x%02x / 0x%02x (%d bytes)", + try + { + if (!reader.read(message_)) + { + ROS_DEBUG_COND(debug >= 2, + "U-Blox Decoder error for 0x%02x / 0x%02x (%d bytes)", static_cast(reader.classId()), static_cast(reader.messageId()), reader.length()); condition_.notify_all(); return; } - } catch (std::runtime_error& e) { - ROS_DEBUG_COND(debug >= 2, - "U-Blox Decoder error for 0x%02x / 0x%02x (%d bytes)", + } + catch (std::runtime_error& e) + { + ROS_DEBUG_COND(debug >= 2, + "U-Blox Decoder error for 0x%02x / 0x%02x (%d bytes)", static_cast(reader.classId()), static_cast(reader.messageId()), reader.length()); @@ -106,36 +119,38 @@ class CallbackHandler_ : public CallbackHandler { return; } - if (func_) func_(message_); + if (func_) + func_(message_); condition_.notify_all(); } - - private: - Callback func_; //!< the callback function to handle the message - T message_; //!< The last received message + +private: + Callback func_; //!< the callback function to handle the message + T message_; //!< The last received message }; /** * @brief Callback handlers for incoming u-blox messages. */ -class CallbackHandlers { - public: +class CallbackHandlers +{ +public: /** * @brief Add a callback handler for the given message type. * @param callback the callback handler for the message * @typedef.a ublox_msgs message with CLASS_ID and MESSAGE_ID constants */ template - void insert(typename CallbackHandler_::Callback callback) { + void insert(typename CallbackHandler_::Callback callback) + { boost::mutex::scoped_lock lock(callback_mutex_); CallbackHandler_* handler = new CallbackHandler_(callback); callbacks_.insert( - std::make_pair(std::make_pair(T::CLASS_ID, T::MESSAGE_ID), - boost::shared_ptr(handler))); + std::make_pair(std::make_pair(T::CLASS_ID, T::MESSAGE_ID), boost::shared_ptr(handler))); } /** - * @brief Add a callback handler for the given message type and ID. This is + * @brief Add a callback handler for the given message type and ID. This is * used for messages in which have the same structure (and therefore msg file) * and same class ID but different message IDs. (e.g. INF, ACK) * @param callback the callback handler for the message @@ -143,27 +158,25 @@ class CallbackHandlers { * @typedef.a ublox_msgs message with a CLASS_ID constant */ template - void insert( - typename CallbackHandler_::Callback callback, - unsigned int message_id) { + void insert(typename CallbackHandler_::Callback callback, unsigned int message_id) + { boost::mutex::scoped_lock lock(callback_mutex_); CallbackHandler_* handler = new CallbackHandler_(callback); callbacks_.insert( - std::make_pair(std::make_pair(T::CLASS_ID, message_id), - boost::shared_ptr(handler))); + std::make_pair(std::make_pair(T::CLASS_ID, message_id), boost::shared_ptr(handler))); } /** * @brief Calls the callback handler for the message in the reader. * @param reader a reader containing a u-blox message */ - void handle(ublox::Reader& reader) { + void handle(ublox::Reader& reader) + { // Find the callback handlers for the message & decode it boost::mutex::scoped_lock lock(callback_mutex_); - Callbacks::key_type key = - std::make_pair(reader.classId(), reader.messageId()); - for (Callbacks::iterator callback = callbacks_.lower_bound(key); - callback != callbacks_.upper_bound(key); ++callback) + Callbacks::key_type key = std::make_pair(reader.classId(), reader.messageId()); + for (Callbacks::iterator callback = callbacks_.lower_bound(key); callback != callbacks_.upper_bound(key); + ++callback) callback->second->handle(reader); } @@ -173,22 +186,23 @@ class CallbackHandlers { * @param timeout the amount of time to wait for the desired message */ template - bool read(T& message, const boost::posix_time::time_duration& timeout) { + bool read(T& message, const boost::posix_time::time_duration& timeout) + { bool result = false; // Create a callback handler for this message callback_mutex_.lock(); CallbackHandler_* handler = new CallbackHandler_(); Callbacks::iterator callback = callbacks_.insert( - (std::make_pair(std::make_pair(T::CLASS_ID, T::MESSAGE_ID), - boost::shared_ptr(handler)))); + (std::make_pair(std::make_pair(T::CLASS_ID, T::MESSAGE_ID), boost::shared_ptr(handler)))); callback_mutex_.unlock(); // Wait for the message - if (handler->wait(timeout)) { + if (handler->wait(timeout)) + { message = handler->get(); result = true; } - + // Remove the callback handler callback_mutex_.lock(); callbacks_.erase(callback); @@ -202,18 +216,19 @@ class CallbackHandlers { * @param data the buffer of u-blox messages to process * @param size the size of the buffer */ - void readCallback(unsigned char* data, std::size_t& size) { + void readCallback(unsigned char* data, std::size_t& size) + { ublox::Reader reader(data, size); // Read all U-Blox messages in buffer - while (reader.search() != reader.end() && reader.found()) { - if (debug >= 3) { + while (reader.search() != reader.end() && reader.found()) + { + if (debug >= 3) + { // Print the received bytes std::ostringstream oss; - for (ublox::Reader::iterator it = reader.pos(); - it != reader.pos() + reader.length() + 8; ++it) + for (ublox::Reader::iterator it = reader.pos(); it != reader.pos() + reader.length() + 8; ++it) oss << boost::format("%02x") % static_cast(*it) << " "; - ROS_DEBUG("U-blox: reading %d bytes\n%s", reader.length() + 8, - oss.str().c_str()); + ROS_DEBUG("U-blox: reading %d bytes\n%s", reader.length() + 8, oss.str().c_str()); } handle(reader); @@ -224,9 +239,8 @@ class CallbackHandlers { size -= reader.pos() - data; } - private: - typedef std::multimap, - boost::shared_ptr > Callbacks; +private: + typedef std::multimap, boost::shared_ptr > Callbacks; // Call back handlers for u-blox messages Callbacks callbacks_; diff --git a/ublox_gps/include/ublox_gps/gps.h b/ublox_gps/include/ublox_gps/gps.h index 504c2a2a..b91be5f4 100644 --- a/ublox_gps/include/ublox_gps/gps.h +++ b/ublox_gps/include/ublox_gps/gps.h @@ -53,21 +53,16 @@ * This namespace is for I/O communication with the u-blox device, including * read callbacks. */ -namespace ublox_gps { +namespace ublox_gps +{ //! Possible baudrates for u-blox devices -constexpr static unsigned int kBaudrates[] = { 4800, - 9600, - 19200, - 38400, - 57600, - 115200, - 230400, - 460800 }; +constexpr static unsigned int kBaudrates[] = { 4800, 9600, 19200, 38400, 57600, 115200, 230400, 460800 }; /** * @brief Handles communication with and configuration of the u-blox device */ -class Gps { - public: +class Gps +{ +public: //! Default timeout for ACK messages in seconds constexpr static double kDefaultAckTimeout = 1.0; //! Size of write buffer for output messages @@ -80,7 +75,8 @@ class Gps { * @brief If called, when the node shuts down, it will send a command to * save the flash memory. */ - void setSaveOnShutdown(bool save_on_shutdown) { + void setSaveOnShutdown(bool save_on_shutdown) + { save_on_shutdown_ = save_on_shutdown; } @@ -88,7 +84,10 @@ class Gps { * @brief Set the internal flag for enabling or disabling the initial configurations. * @param config_on_startup boolean flag */ - void setConfigOnStartup(const bool config_on_startup) { config_on_startup_flag_ = config_on_startup; } + void setConfigOnStartup(const bool config_on_startup) + { + config_on_startup_flag_ = config_on_startup; + } /** * @brief Initialize TCP I/O. @@ -111,8 +110,7 @@ class Gps { * @param uart_in the UART In protocol, see CfgPRT for options * @param uart_out the UART Out protocol, see CfgPRT for options */ - void initializeSerial(std::string port, unsigned int baudrate, - uint16_t uart_in, uint16_t uart_out); + void initializeSerial(std::string port, unsigned int baudrate, uint16_t uart_in, uint16_t uart_out); /** * @brief Reset the Serial I/O port after u-blox reset. @@ -125,13 +123,13 @@ class Gps { /** * @brief Send rtcm correction message. - */ - bool sendRtcm(const std::vector &message); + */ + bool sendRtcm(const std::vector& message); /** * @brief Send SPARTN correction message. - */ - bool sendSpartn(const std::vector &message); + */ + bool sendSpartn(const std::vector& message); /** * @brief Closes the I/O port, and initiates save on shutdown procedure @@ -160,8 +158,7 @@ class Gps { * @return true if the GNSS was configured, the device was reset, and the * I/O reset successfully */ - bool configGnss(ublox_msgs::CfgGNSS gnss, - const boost::posix_time::time_duration& wait); + bool configGnss(ublox_msgs::CfgGNSS gnss, const boost::posix_time::time_duration& wait); /** * @brief Send a message to the receiver to delete the BBR data stored in @@ -177,8 +174,7 @@ class Gps { * @param out_proto_mask the out protocol mask, see CfgPRT message * @return true on ACK, false on other conditions. */ - bool configUart1(unsigned int baudrate, uint16_t in_proto_mask, - uint16_t out_proto_mask); + bool configUart1(unsigned int baudrate, uint16_t in_proto_mask, uint16_t out_proto_mask); /** * @brief Disable the UART Port. Sets in/out protocol masks to 0. Does not @@ -196,8 +192,7 @@ class Gps { * @param out_proto_mask the out protocol mask, see CfgPRT message * @return true on ACK, false on other conditions. */ - bool configUsb(uint16_t tx_ready, uint16_t in_proto_mask, - uint16_t out_proto_mask); + bool configUsb(uint16_t tx_ready, uint16_t in_proto_mask, uint16_t out_proto_mask); /** * @brief Configure the device navigation and measurement rate settings. @@ -315,7 +310,7 @@ class Gps { bool setUseAdr(bool enable, float protocol_version); /** - * @brief Configure the U-Blox to UTC time + * @brief Configure the U-Blox to UTC time * @return true on ACK, false on other conditions. * * @note This is part of the expert settings. It is recommended you check @@ -332,7 +327,7 @@ class Gps { * the ublox manual first. */ bool setTimtm2(uint8_t rate); - + /** * @brief Configure the U-Blox send rate of the message & subscribe to the * given message @@ -340,8 +335,7 @@ class Gps { * @param rate the rate in Hz of the message */ template - void subscribe(typename CallbackHandler_::Callback callback, - unsigned int rate); + void subscribe(typename CallbackHandler_::Callback callback, unsigned int rate); /** * @brief Subscribe to the given Ublox message. * @param the callback handler for the message @@ -357,8 +351,7 @@ class Gps { * @param message_id the U-Blox message ID */ template - void subscribeId(typename CallbackHandler_::Callback callback, - unsigned int message_id); + void subscribeId(typename CallbackHandler_::Callback callback, unsigned int message_id); /** * Read a u-blox message of the given type. @@ -366,12 +359,20 @@ class Gps { * @param timeout the amount of time to wait for the desired message */ template - bool read(T& message, - const boost::posix_time::time_duration& timeout = default_timeout_); + bool read(T& message, const boost::posix_time::time_duration& timeout = default_timeout_); - bool isInitialized() const { return worker_ != 0; } - bool isConfigured() const { return isInitialized() && configured_; } - bool isOpen() const { return worker_->isOpen(); } + bool isInitialized() const + { + return worker_ != 0; + } + bool isConfigured() const + { + return isInitialized() && configured_; + } + bool isOpen() const + { + return worker_->isOpen(); + } /** * Poll a u-blox message of the given type. @@ -392,8 +393,7 @@ class Gps { * defaults to empty * @param timeout the amount of time to wait for the desired message */ - bool poll(uint8_t class_id, uint8_t message_id, - const std::vector& payload = std::vector()); + bool poll(uint8_t class_id, uint8_t message_id, const std::vector& payload = std::vector()); /** * @brief Send the given configuration message. @@ -412,8 +412,7 @@ class Gps { * @param msg_id the expected message ID of the ACK * @return true if expected ACK received, false otherwise */ - bool waitForAcknowledge(const boost::posix_time::time_duration& timeout, - uint8_t class_id, uint8_t msg_id); + bool waitForAcknowledge(const boost::posix_time::time_duration& timeout, uint8_t class_id, uint8_t msg_id); /** * @brief Set the callback function which handles raw data. @@ -421,19 +420,21 @@ class Gps { */ void setRawDataCallback(const Worker::Callback& callback); - private: +private: //! Types for ACK/NACK messages, WAIT is used when waiting for an ACK - enum AckType { - NACK, //! Not acknowledged - ACK, //! Acknowledge - WAIT //! Waiting for ACK + enum AckType + { + NACK, //! Not acknowledged + ACK, //! Acknowledge + WAIT //! Waiting for ACK }; //! Stores ACK/NACK messages - struct Ack { - AckType type; //!< The ACK type - uint8_t class_id; //!< The class ID of the ACK - uint8_t msg_id; //!< The message ID of the ACK + struct Ack + { + AckType type; //!< The ACK type + uint8_t class_id; //!< The class ID of the ACK + uint8_t msg_id; //!< The message ID of the ACK }; /** @@ -451,19 +452,19 @@ class Gps { * @brief Callback handler for UBX-ACK message. * @param m the message to process */ - void processAck(const ublox_msgs::Ack &m); + void processAck(const ublox_msgs::Ack& m); /** * @brief Callback handler for UBX-NACK message. * @param m the message to process */ - void processNack(const ublox_msgs::Ack &m); + void processNack(const ublox_msgs::Ack& m); /** * @brief Callback handler for UBX-UPD-SOS-ACK message. * @param m the message to process */ - void processUpdSosAck(const ublox_msgs::UpdSOS_Ack &m); + void processUpdSosAck(const ublox_msgs::UpdSOS_Ack& m); /** * @brief Execute save on shutdown procedure. @@ -486,7 +487,6 @@ class Gps { //!< Whether or not initial configuration to the hardware is done bool config_on_startup_flag_; - //! The default timeout for ACK messages static const boost::posix_time::time_duration default_timeout_; //! Stores last received ACK accessed by multiple threads @@ -499,40 +499,46 @@ class Gps { }; template -void Gps::subscribe( - typename CallbackHandler_::Callback callback, unsigned int rate) { - if (!setRate(T::CLASS_ID, T::MESSAGE_ID, rate)) return; +void Gps::subscribe(typename CallbackHandler_::Callback callback, unsigned int rate) +{ + if (!setRate(T::CLASS_ID, T::MESSAGE_ID, rate)) + return; subscribe(callback); } template -void Gps::subscribe(typename CallbackHandler_::Callback callback) { +void Gps::subscribe(typename CallbackHandler_::Callback callback) +{ callbacks_.insert(callback); } template -void Gps::subscribeId(typename CallbackHandler_::Callback callback, - unsigned int message_id) { +void Gps::subscribeId(typename CallbackHandler_::Callback callback, unsigned int message_id) +{ callbacks_.insert(callback, message_id); } template -bool Gps::poll(ConfigT& message, - const std::vector& payload, - const boost::posix_time::time_duration& timeout) { - if (!poll(ConfigT::CLASS_ID, ConfigT::MESSAGE_ID, payload)) return false; +bool Gps::poll(ConfigT& message, const std::vector& payload, const boost::posix_time::time_duration& timeout) +{ + if (!poll(ConfigT::CLASS_ID, ConfigT::MESSAGE_ID, payload)) + return false; return read(message, timeout); } template -bool Gps::read(T& message, const boost::posix_time::time_duration& timeout) { - if (!worker_) return false; +bool Gps::read(T& message, const boost::posix_time::time_duration& timeout) +{ + if (!worker_) + return false; return callbacks_.read(message, timeout); } template -bool Gps::configure(const ConfigT& message, bool wait) { - if (!worker_) return false; +bool Gps::configure(const ConfigT& message, bool wait) +{ + if (!worker_) + return false; // Reset ack Ack ack; @@ -542,20 +548,19 @@ bool Gps::configure(const ConfigT& message, bool wait) { // Encode the message std::vector out(kWriterSize); ublox::Writer writer(out.data(), out.size()); - if (!writer.write(message)) { - ROS_ERROR("Failed to encode config message 0x%02x / 0x%02x", - message.CLASS_ID, message.MESSAGE_ID); + if (!writer.write(message)) + { + ROS_ERROR("Failed to encode config message 0x%02x / 0x%02x", message.CLASS_ID, message.MESSAGE_ID); return false; } // Send the message to the device worker_->send(out.data(), writer.end() - out.data()); - if (!wait) return true; + if (!wait) + return true; // Wait for an acknowledgment and return whether or not it was received - return waitForAcknowledge(default_timeout_, - message.CLASS_ID, - message.MESSAGE_ID); + return waitForAcknowledge(default_timeout_, message.CLASS_ID, message.MESSAGE_ID); } } // namespace ublox_gps diff --git a/ublox_gps/include/ublox_gps/mkgmtime.h b/ublox_gps/include/ublox_gps/mkgmtime.h index 8537379c..e4ce8029 100644 --- a/ublox_gps/include/ublox_gps/mkgmtime.h +++ b/ublox_gps/include/ublox_gps/mkgmtime.h @@ -1,6 +1,6 @@ /* mkgmtime.h -- make a time_t from a gmtime struct tm $Id: mkgmtime.h,v 1.5 2003/02/13 20:15:41 rjs3 Exp $ - + * Copyright (c) 1998-2003 Carnegie Mellon University. All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -8,7 +8,7 @@ * are met: * * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. + * 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 @@ -18,7 +18,7 @@ * 3. The name "Carnegie Mellon University" must not be used to * endorse or promote products derived from this software without * prior written permission. For permission or any other legal - * details, please contact + * details, please contact * Office of Technology Transfer * Carnegie Mellon University * 5000 Forbes Avenue @@ -50,6 +50,6 @@ * @brief Get the UTC time in seconds and nano-seconds from a time struct in * GM time. */ -extern time_t mkgmtime(struct tm * const tmp); +extern time_t mkgmtime(struct tm* const tmp); #endif /* INCLUDED_MKGMTIME_H */ \ No newline at end of file diff --git a/ublox_gps/include/ublox_gps/node.h b/ublox_gps/include/ublox_gps/node.h index 8163b773..b0fbc66e 100644 --- a/ublox_gps/include/ublox_gps/node.h +++ b/ublox_gps/include/ublox_gps/node.h @@ -85,8 +85,8 @@ * This namespace is for the ROS u-blox node and handles anything regarding * ROS parameters, message passing, diagnostics, etc. */ -namespace ublox_node { - +namespace ublox_node +{ //! Queue size for ROS publishers constexpr static uint32_t kROSQueueSize = 1; //! Default measurement period for HPG devices @@ -128,10 +128,12 @@ std::vector rtcm_rates; //! Flag for enabling configuration on startup bool config_on_startup_flag_; - //! Topic diagnostics for u-blox messages -struct UbloxTopicDiagnostic { - UbloxTopicDiagnostic() {} +struct UbloxTopicDiagnostic +{ + UbloxTopicDiagnostic() + { + } // Must not copy this struct (would confuse FrequencyStatusParam pointers) UbloxTopicDiagnostic(const UbloxTopicDiagnostic&) = delete; @@ -144,15 +146,13 @@ struct UbloxTopicDiagnostic { * @param freq_tol the tolerance [%] for the topic frequency * @param freq_window the number of messages to use for diagnostic statistics */ - UbloxTopicDiagnostic (std::string topic, double freq_tol, int freq_window) { - const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate); // Hz + UbloxTopicDiagnostic(std::string topic, double freq_tol, int freq_window) + { + const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate); // Hz min_freq = target_freq; max_freq = target_freq; - diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, - freq_tol, freq_window); - diagnostic = new diagnostic_updater::HeaderlessTopicDiagnostic(topic, - *updater, - freq_param); + diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, freq_tol, freq_window); + diagnostic = new diagnostic_updater::HeaderlessTopicDiagnostic(topic, *updater, freq_param); } /** @@ -165,19 +165,16 @@ struct UbloxTopicDiagnostic { * @param freq_tol the tolerance [%] for the topic frequency * @param freq_window the number of messages to use for diagnostic statistics */ - UbloxTopicDiagnostic (std::string topic, double freq_min, double freq_max, - double freq_tol, int freq_window) { + UbloxTopicDiagnostic(std::string topic, double freq_min, double freq_max, double freq_tol, int freq_window) + { min_freq = freq_min; max_freq = freq_max; - diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, - freq_tol, freq_window); - diagnostic = new diagnostic_updater::HeaderlessTopicDiagnostic(topic, - *updater, - freq_param); + diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, freq_tol, freq_window); + diagnostic = new diagnostic_updater::HeaderlessTopicDiagnostic(topic, *updater, freq_param); } //! Topic frequency diagnostic updater - diagnostic_updater::HeaderlessTopicDiagnostic *diagnostic; + diagnostic_updater::HeaderlessTopicDiagnostic* diagnostic; //! Minimum allow frequency of topic double min_freq; //! Maximum allow frequency of topic @@ -185,8 +182,11 @@ struct UbloxTopicDiagnostic { }; //! Topic diagnostics for fix / fix_velocity messages -struct FixDiagnostic { - FixDiagnostic() {} +struct FixDiagnostic +{ + FixDiagnostic() + { + } // Must not copy this struct (would confuse FrequencyStatusParam pointers) FixDiagnostic(const FixDiagnostic&) = delete; @@ -200,23 +200,19 @@ struct FixDiagnostic { * @param freq_window the number of messages to use for diagnostic statistics * @param stamp_min the minimum allowed time delay */ - FixDiagnostic (std::string name, double freq_tol, int freq_window, - double stamp_min) { - const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate); // Hz + FixDiagnostic(std::string name, double freq_tol, int freq_window, double stamp_min) + { + const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate); // Hz min_freq = target_freq; max_freq = target_freq; - diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, - freq_tol, freq_window); + diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, freq_tol, freq_window); double stamp_max = meas_rate * 1e-3 * (1 + freq_tol); diagnostic_updater::TimeStampStatusParam time_param(stamp_min, stamp_max); - diagnostic = new diagnostic_updater::TopicDiagnostic(name, - *updater, - freq_param, - time_param); + diagnostic = new diagnostic_updater::TopicDiagnostic(name, *updater, freq_param, time_param); } //! Topic frequency diagnostic updater - diagnostic_updater::TopicDiagnostic *diagnostic; + diagnostic_updater::TopicDiagnostic* diagnostic; //! Minimum allow frequency of topic double min_freq; //! Maximum allow frequency of topic @@ -262,8 +258,10 @@ uint8_t fixModeFromString(const std::string& mode); * @throws std::runtime_error if it is below the minimum */ template -void checkMin(V val, T min, std::string name) { - if(val < min) { +void checkMin(V val, T min, std::string name) +{ + if (val < min) + { std::stringstream oss; oss << "Invalid settings: " << name << " must be > " << min; throw std::runtime_error(oss.str()); @@ -279,11 +277,12 @@ void checkMin(V val, T min, std::string name) { * @throws std::runtime_error if it is out of bounds */ template -void checkRange(V val, T min, T max, std::string name) { - if(val < min || val > max) { +void checkRange(V val, T min, T max, std::string name) +{ + if (val < min || val > max) + { std::stringstream oss; - oss << "Invalid settings: " << name << " must be in range [" << min << - ", " << max << "]."; + oss << "Invalid settings: " << name << " must be in range [" << min << ", " << max << "]."; throw std::runtime_error(oss.str()); } } @@ -297,8 +296,10 @@ void checkRange(V val, T min, T max, std::string name) { * @throws std::runtime_error value it is out of bounds */ template -void checkRange(std::vector val, T min, T max, std::string name) { - for(size_t i = 0; i < val.size(); i++) { +void checkRange(std::vector val, T min, T max, std::string name) +{ + for (size_t i = 0; i < val.size(); i++) + { std::stringstream oss; oss << name << "[" << i << "]"; checkRange(val[i], min, max, oss.str()); @@ -313,15 +314,17 @@ void checkRange(std::vector val, T min, T max, std::string name) { * @return true if found, false if not found. */ template -bool getRosUint(const std::string& key, U &u) { +bool getRosUint(const std::string& key, U& u) +{ int param; - if (!nh->getParam(key, param)) return false; + if (!nh->getParam(key, param)) + return false; // Check the bounds U min = std::numeric_limits::lowest(); U max = std::numeric_limits::max(); checkRange(param, min, max, key); // set the output - u = (U) param; + u = (U)param; return true; } @@ -334,8 +337,9 @@ bool getRosUint(const std::string& key, U &u) { * @return true if found, false if not found. */ template -void getRosUint(const std::string& key, U &u, V default_val) { - if(!getRosUint(key, u)) +void getRosUint(const std::string& key, U& u, V default_val) +{ + if (!getRosUint(key, u)) u = default_val; } @@ -345,9 +349,11 @@ void getRosUint(const std::string& key, U &u, V default_val) { * @return true if found, false if not found. */ template -bool getRosUint(const std::string& key, std::vector &u) { +bool getRosUint(const std::string& key, std::vector& u) +{ std::vector param; - if (!nh->getParam(key, param)) return false; + if (!nh->getParam(key, param)) + return false; // Check the bounds U min = std::numeric_limits::lowest(); @@ -367,15 +373,17 @@ bool getRosUint(const std::string& key, std::vector &u) { * @return true if found, false if not found. */ template -bool getRosInt(const std::string& key, I &u) { +bool getRosInt(const std::string& key, I& u) +{ int param; - if (!nh->getParam(key, param)) return false; + if (!nh->getParam(key, param)) + return false; // Check the bounds I min = std::numeric_limits::lowest(); I max = std::numeric_limits::max(); checkRange(param, min, max, key); // set the output - u = (I) param; + u = (I)param; return true; } @@ -388,8 +396,9 @@ bool getRosInt(const std::string& key, I &u) { * @return true if found, false if not found. */ template -void getRosInt(const std::string& key, U &u, V default_val) { - if(!getRosInt(key, u)) +void getRosInt(const std::string& key, U& u, V default_val) +{ + if (!getRosInt(key, u)) u = default_val; } @@ -399,9 +408,11 @@ void getRosInt(const std::string& key, U &u, V default_val) { * @return true if found, false if not found. */ template -bool getRosInt(const std::string& key, std::vector &i) { +bool getRosInt(const std::string& key, std::vector& i) +{ std::vector param; - if (!nh->getParam(key, param)) return false; + if (!nh->getParam(key, param)) + return false; // Check the bounds I min = std::numeric_limits::lowest(); @@ -422,9 +433,9 @@ bool getRosInt(const std::string& key, std::vector &i) { * @param topic the topic to publish the message on */ template -void publish(const MessageT& m, const std::string& topic) { - static ros::Publisher publisher = nh->advertise(topic, - kROSQueueSize); +void publish(const MessageT& m, const std::string& topic) +{ + static ros::Publisher publisher = nh->advertise(topic, kROSQueueSize); publisher.publish(m); } @@ -433,7 +444,8 @@ void publish(const MessageT& m, const std::string& topic) { * i.e. GPS, GLO, GAL, BDS, QZSS, SBAS, IMES * @return true if the device supports the given GNSS */ -bool supportsGnss(std::string gnss) { +bool supportsGnss(std::string gnss) +{ return supported.count(gnss) > 0; } @@ -443,8 +455,9 @@ bool supportsGnss(std::string gnss) { * @details This interface is generic and can be implemented for other features * besides the main node, hardware versions, and firmware versions. */ -class ComponentInterface { - public: +class ComponentInterface +{ +public: /** * @brief Get the ROS parameters. * @throws std::runtime_error if a parameter is invalid or required @@ -488,8 +501,9 @@ typedef boost::shared_ptr ComponentPtr; * The UbloxNode calls the public methods of ComponentInterface for each * element in the components vector. */ -class UbloxNode : public virtual ComponentInterface { - public: +class UbloxNode : public virtual ComponentInterface +{ +public: //! How often (in seconds) to send keep-alive message constexpr static double kKeepAlivePeriod = 10.0; //! How often (in seconds) to call poll messages @@ -533,10 +547,9 @@ class UbloxNode : public virtual ComponentInterface { /** * @brief Print an INF message to the ROS console. */ - void printInf(const ublox_msgs::Inf &m, uint8_t id); - - private: + void printInf(const ublox_msgs::Inf& m, uint8_t id); +private: /** * @brief Initialize the I/O handling. */ @@ -579,8 +592,7 @@ class UbloxNode : public virtual ComponentInterface { * @param for HPG/TIM products, this value is either REF or ROV, for other * products this string is empty */ - void addProductInterface(std::string product_category, - std::string ref_rov = ""); + void addProductInterface(std::string product_category, std::string ref_rov = ""); /** * @brief Poll version message from the U-Blox device to keep socket active. @@ -633,7 +645,7 @@ class UbloxNode : public virtual ComponentInterface { //! USB in protocol (see CfgPRT message for constants) uint16_t usb_in_; //! USB out protocol (see CfgPRT message for constants) - uint16_t usb_out_ ; + uint16_t usb_out_; //! The measurement rate in Hz double rate_; //! If true, set configure the User-Defined Datum @@ -666,26 +678,27 @@ class UbloxNode : public virtual ComponentInterface { * * @details The Firmware components update the fix diagnostics. */ -class UbloxFirmware : public virtual ComponentInterface { - public: +class UbloxFirmware : public virtual ComponentInterface +{ +public: /** * @brief Add the fix diagnostics to the updater. */ void initializeRosDiagnostics(); - protected: +protected: /** * @brief Handle to send fix status to ROS diagnostics. */ - virtual void fixDiagnostic( - diagnostic_updater::DiagnosticStatusWrapper& stat) = 0; + virtual void fixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) = 0; }; /** * @brief Implements functions for firmware version 6. */ -class UbloxFirmware6 : public UbloxFirmware { - public: +class UbloxFirmware6 : public UbloxFirmware +{ +public: UbloxFirmware6(); /** @@ -704,14 +717,14 @@ class UbloxFirmware6 : public UbloxFirmware { */ void subscribe(); - protected: +protected: /** * @brief Updates fix diagnostic from NavPOSLLH, NavVELNED, and NavSOL * messages. */ void fixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat); - private: +private: /** * @brief Publish the fix and call the fix diagnostic updater. * @@ -763,9 +776,10 @@ class UbloxFirmware6 : public UbloxFirmware { * * @typedef NavPVT the NavPVT message type for the given firmware version */ -template -class UbloxFirmware7Plus : public UbloxFirmware { - public: +template +class UbloxFirmware7Plus : public UbloxFirmware +{ +public: /** * @brief Publish a NavSatFix and TwistWithCovarianceStamped messages. * @@ -774,76 +788,80 @@ class UbloxFirmware7Plus : public UbloxFirmware { * is published. This function also calls the ROS diagnostics updater. * @param m the message to publish */ - void callbackNavPvt(const NavPVT& m) { - if(enabled["nav_pvt"]) { + void callbackNavPvt(const NavPVT& m) + { + if (enabled["nav_pvt"]) + { // NavPVT publisher - static ros::Publisher publisher = nh->advertise("navpvt", - kROSQueueSize); + static ros::Publisher publisher = nh->advertise("navpvt", kROSQueueSize); publisher.publish(m); } // // NavSatFix message // - static ros::Publisher fixPublisher = - nh->advertise("fix", kROSQueueSize); + static ros::Publisher fixPublisher = nh->advertise("fix", kROSQueueSize); sensor_msgs::NavSatFix fix; fix.header.frame_id = frame_id; // set the timestamp uint8_t valid_time = m.VALID_DATE | m.VALID_TIME | m.VALID_FULLY_RESOLVED; - if (((m.valid & valid_time) == valid_time) && - (m.flags2 & m.FLAGS2_CONFIRMED_AVAILABLE)) { + if (((m.valid & valid_time) == valid_time) && (m.flags2 & m.FLAGS2_CONFIRMED_AVAILABLE)) + { // Use NavPVT timestamp since it is valid // The time in nanoseconds from the NavPVT message can be between -1e9 and 1e9 // The ros time uses only unsigned values, so a negative nano seconds must be // converted to a positive value - if (m.nano < 0) { + if (m.nano < 0) + { fix.header.stamp.sec = toUtcSeconds(m) - 1; fix.header.stamp.nsec = (uint32_t)(m.nano + 1e9); } - else { + else + { fix.header.stamp.sec = toUtcSeconds(m); fix.header.stamp.nsec = (uint32_t)(m.nano); } - } else { + } + else + { // Use ROS time since NavPVT timestamp is not valid fix.header.stamp = ros::Time::now(); } // Set the LLA - fix.latitude = m.lat * 1e-7; // to deg - fix.longitude = m.lon * 1e-7; // to deg - fix.altitude = m.height * 1e-3; // to [m] + fix.latitude = m.lat * 1e-7; // to deg + fix.longitude = m.lon * 1e-7; // to deg + fix.altitude = m.height * 1e-3; // to [m] // Set the Fix status bool fixOk = m.flags & m.FLAGS_GNSS_FIX_OK; - if (fixOk && m.fixType >= m.FIX_TYPE_2D) { + if (fixOk && m.fixType >= m.FIX_TYPE_2D) + { fix.status.status = fix.status.STATUS_FIX; - if(m.flags & m.CARRIER_PHASE_FIXED) + if (m.flags & m.CARRIER_PHASE_FIXED) fix.status.status = fix.status.STATUS_GBAS_FIX; } - else { + else + { fix.status.status = fix.status.STATUS_NO_FIX; } // Set the service based on GNSS configuration fix.status.service = fix_status_service; // Set the position covariance - const double varH = pow(m.hAcc / 1000.0, 2); // to [m^2] - const double varV = pow(m.vAcc / 1000.0, 2); // to [m^2] + const double varH = pow(m.hAcc / 1000.0, 2); // to [m^2] + const double varV = pow(m.vAcc / 1000.0, 2); // to [m^2] fix.position_covariance[0] = varH; fix.position_covariance[4] = varH; fix.position_covariance[8] = varV; - fix.position_covariance_type = - sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; + fix.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; fixPublisher.publish(fix); // // Twist message // - static ros::Publisher velocityPublisher = - nh->advertise("fix_velocity", - kROSQueueSize); + static ros::Publisher velocityPublisher = nh->advertise("fix_velocity", + kROSQueueSize); geometry_msgs::TwistWithCovarianceStamped velocity; velocity.header.stamp = fix.header.stamp; velocity.header.frame_id = frame_id; @@ -870,51 +888,63 @@ class UbloxFirmware7Plus : public UbloxFirmware { updater->update(); } - protected: - +protected: /** * @brief Update the fix diagnostics from Nav PVT message. */ - void fixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) { + void fixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) + { // check the last message, convert to diagnostic - if (last_nav_pvt_.fixType == - ublox_msgs::NavPVT::FIX_TYPE_DEAD_RECKONING_ONLY) { + if (last_nav_pvt_.fixType == ublox_msgs::NavPVT::FIX_TYPE_DEAD_RECKONING_ONLY) + { stat.level = diagnostic_msgs::DiagnosticStatus::WARN; stat.message = "Dead reckoning only"; - } else if (last_nav_pvt_.fixType == ublox_msgs::NavPVT::FIX_TYPE_2D) { + } + else if (last_nav_pvt_.fixType == ublox_msgs::NavPVT::FIX_TYPE_2D) + { stat.level = diagnostic_msgs::DiagnosticStatus::WARN; stat.message = "2D fix"; - } else if (last_nav_pvt_.fixType == ublox_msgs::NavPVT::FIX_TYPE_3D) { + } + else if (last_nav_pvt_.fixType == ublox_msgs::NavPVT::FIX_TYPE_3D) + { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "3D fix"; - } else if (last_nav_pvt_.fixType == - ublox_msgs::NavPVT::FIX_TYPE_GNSS_DEAD_RECKONING_COMBINED) { + } + else if (last_nav_pvt_.fixType == ublox_msgs::NavPVT::FIX_TYPE_GNSS_DEAD_RECKONING_COMBINED) + { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "GPS and dead reckoning combined"; - } else if (last_nav_pvt_.fixType == - ublox_msgs::NavPVT::FIX_TYPE_TIME_ONLY) { + } + else if (last_nav_pvt_.fixType == ublox_msgs::NavPVT::FIX_TYPE_TIME_ONLY) + { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "Time only fix"; } - + // Check whether differential GNSS available - if (last_nav_pvt_.flags & ublox_msgs::NavPVT::FLAGS_DIFF_SOLN) { + if (last_nav_pvt_.flags & ublox_msgs::NavPVT::FLAGS_DIFF_SOLN) + { stat.message += ", DGNSS"; - } + } // If DGNSS, then update the differential solution status - if (last_nav_pvt_.flags & ublox_msgs::NavPVT::CARRIER_PHASE_FLOAT) { + if (last_nav_pvt_.flags & ublox_msgs::NavPVT::CARRIER_PHASE_FLOAT) + { stat.message += ", FLOAT FIX"; - } else if (last_nav_pvt_.flags & ublox_msgs::NavPVT::CARRIER_PHASE_FIXED) { + } + else if (last_nav_pvt_.flags & ublox_msgs::NavPVT::CARRIER_PHASE_FIXED) + { stat.message += ", RTK FIX"; } // If fix not ok (w/in DOP & Accuracy Masks), raise the diagnostic level - if (!(last_nav_pvt_.flags & ublox_msgs::NavPVT::FLAGS_GNSS_FIX_OK)) { + if (!(last_nav_pvt_.flags & ublox_msgs::NavPVT::FLAGS_GNSS_FIX_OK)) + { stat.level = diagnostic_msgs::DiagnosticStatus::WARN; stat.message += ", fix not ok"; } // Raise diagnostic level to error if no fix - if (last_nav_pvt_.fixType == ublox_msgs::NavPVT::FIX_TYPE_NO_FIX) { + if (last_nav_pvt_.fixType == ublox_msgs::NavPVT::FIX_TYPE_NO_FIX) + { stat.level = diagnostic_msgs::DiagnosticStatus::ERROR; stat.message = "No fix"; } @@ -954,8 +984,9 @@ class UbloxFirmware7Plus : public UbloxFirmware { /** * @brief Implements functions for firmware version 7. */ -class UbloxFirmware7 : public UbloxFirmware7Plus { - public: +class UbloxFirmware7 : public UbloxFirmware7Plus +{ +public: UbloxFirmware7(); /** @@ -977,21 +1008,22 @@ class UbloxFirmware7 : public UbloxFirmware7Plus { */ void subscribe(); - private: - //! Used to configure NMEA (if set_nmea_) - /*! - * Filled from ROS parameters - */ - ublox_msgs::CfgNMEA7 cfg_nmea_; - //! Whether or not to Configure the NMEA settings - bool set_nmea_; +private: + //! Used to configure NMEA (if set_nmea_) + /*! + * Filled from ROS parameters + */ + ublox_msgs::CfgNMEA7 cfg_nmea_; + //! Whether or not to Configure the NMEA settings + bool set_nmea_; }; /** * @brief Implements functions for firmware version 8. */ -class UbloxFirmware8 : public UbloxFirmware7Plus { - public: +class UbloxFirmware8 : public UbloxFirmware7Plus +{ +public: UbloxFirmware8(); /** @@ -1019,7 +1051,7 @@ class UbloxFirmware8 : public UbloxFirmware7Plus { */ void subscribe(); - private: +private: // Set from ROS parameters //! Whether or not to enable the Galileo GNSS bool enable_galileo_; @@ -1040,27 +1072,34 @@ class UbloxFirmware8 : public UbloxFirmware7Plus { * For now it simply re-uses the firmware version 8 class * but allows for future expansion of functionality */ -class UbloxFirmware9 : public UbloxFirmware8 { +class UbloxFirmware9 : public UbloxFirmware8 +{ }; /** * @brief Implements functions for Raw Data products. */ -class RawDataProduct: public virtual ComponentInterface { - public: +class RawDataProduct : public virtual ComponentInterface +{ +public: static constexpr double kRtcmFreqTol = 0.15; static constexpr int kRtcmFreqWindow = 25; /** * @brief Does nothing since there are no Raw Data product specific settings. */ - void getRosParams() {} + void getRosParams() + { + } /** * @brief Does nothing since there are no Raw Data product specific settings. * @return always returns true */ - bool configureUblox() { return true; } + bool configureUblox() + { + return true; + } /** * @brief Subscribe to Raw Data Product messages and set up ROS publishers. @@ -1074,7 +1113,7 @@ class RawDataProduct: public virtual ComponentInterface { */ void initializeRosDiagnostics(); - private: +private: //! Topic diagnostic updaters std::vector > freq_diagnostics_; }; @@ -1083,10 +1122,11 @@ class RawDataProduct: public virtual ComponentInterface { * @brief Implements functions for Automotive Dead Reckoning (ADR) and * Untethered Dead Reckoning (UDR) Devices. */ -class AdrUdrProduct: public virtual ComponentInterface { - public: +class AdrUdrProduct : public virtual ComponentInterface +{ +public: AdrUdrProduct(float protocol_version); - + /** * @brief Get the ADR/UDR parameters. * @@ -1113,34 +1153,36 @@ class AdrUdrProduct: public virtual ComponentInterface { * @brief Initialize the ROS diagnostics for the ADR/UDR device. * @todo unimplemented */ - void initializeRosDiagnostics() { + void initializeRosDiagnostics() + { ROS_WARN("ROS Diagnostics specific to u-blox ADR/UDR devices is %s", "unimplemented. See AdrUdrProduct class in node.h & node.cpp."); } - protected: +protected: //! Whether or not to enable dead reckoning - bool use_adr_; + bool use_adr_; float protocol_version_; - sensor_msgs::Imu imu_; sensor_msgs::TimeReference t_ref_; ublox_msgs::TimTM2 timtm2; - void callbackEsfMEAS(const ublox_msgs::EsfMEAS &m); + void callbackEsfMEAS(const ublox_msgs::EsfMEAS& m); }; /** * @brief Implements functions for FTS products. Currently unimplemented. * @todo Unimplemented. */ -class FtsProduct: public virtual ComponentInterface { +class FtsProduct : public virtual ComponentInterface +{ /** * @brief Get the FTS parameters. * @todo Currently unimplemented. */ - void getRosParams() { + void getRosParams() + { ROS_WARN("Functionality specific to u-blox FTS devices is %s", "unimplemented. See FtsProduct class in node.h & node.cpp."); } @@ -1149,27 +1191,35 @@ class FtsProduct: public virtual ComponentInterface { * @brief Configure FTS settings. * @todo Currently unimplemented. */ - bool configureUblox() { return false; } + bool configureUblox() + { + return false; + } /** * @brief Subscribe to FTS messages. * @todo Currently unimplemented. */ - void subscribe() {} + void subscribe() + { + } /** * @brief Adds diagnostic updaters for FTS status. * @todo Currently unimplemented. */ - void initializeRosDiagnostics() {} + void initializeRosDiagnostics() + { + } }; /** * @brief Implements functions for High Precision GNSS Reference station * devices. */ -class HpgRefProduct: public virtual ComponentInterface { - public: +class HpgRefProduct : public virtual ComponentInterface +{ +public: /** * @brief Get the ROS parameters specific to the Reference Station * configuration. @@ -1213,7 +1263,7 @@ class HpgRefProduct: public virtual ComponentInterface { */ void callbackNavSvIn(ublox_msgs::NavSVIN m); - protected: +protected: /** * @brief Update the TMODE3 diagnostics. * @@ -1265,20 +1315,22 @@ class HpgRefProduct: public virtual ComponentInterface { float sv_in_acc_lim_; //! Status of device time mode - enum { - INIT, //!< Initialization mode (before configuration) - FIXED, //!< Fixed mode (should switch to time mode almost immediately) - DISABLED, //!< Time mode disabled - SURVEY_IN, //!< Survey-In mode - TIME //!< Time mode, after survey-in or after configuring fixed mode + enum + { + INIT, //!< Initialization mode (before configuration) + FIXED, //!< Fixed mode (should switch to time mode almost immediately) + DISABLED, //!< Time mode disabled + SURVEY_IN, //!< Survey-In mode + TIME //!< Time mode, after survey-in or after configuring fixed mode } mode_; }; /** * @brief Implements functions for High Precision GNSS Rover devices. */ -class HpgRovProduct: public virtual ComponentInterface { - public: +class HpgRovProduct : public virtual ComponentInterface +{ +public: // Constants for diagnostic updater //! Diagnostic updater: RTCM topic frequency min [Hz] constexpr static double kRtcmFreqMin = 1; @@ -1314,21 +1366,19 @@ class HpgRovProduct: public virtual ComponentInterface { */ void initializeRosDiagnostics(); - protected: +protected: /** * @brief Update the rover diagnostics, including the carrier phase solution * status (float or fixed). */ - void carrierPhaseDiagnostics( - diagnostic_updater::DiagnosticStatusWrapper& stat); + void carrierPhaseDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat); /** * @brief Set the last received message and call rover diagnostic updater * * @details Publish received NavRELPOSNED messages if enabled */ - void callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED &m); - + void callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED& m); //! Last relative position (used for diagnostic updater) ublox_msgs::NavRELPOSNED last_rel_pos_; @@ -1341,15 +1391,15 @@ class HpgRovProduct: public virtual ComponentInterface { UbloxTopicDiagnostic freq_rtcm_; }; -class HpPosRecProduct: public virtual HpgRefProduct { - public: +class HpPosRecProduct : public virtual HpgRefProduct +{ +public: /** * @brief Subscribe to Rover messages, such as NavRELPOSNED. */ void subscribe(); - protected: - +protected: /** * @brief Publish a sensor_msgs/NavSatFix message upon receiving a HPPOSLLH UBX message */ @@ -1360,7 +1410,7 @@ class HpPosRecProduct: public virtual HpgRefProduct { * * @details Publish received NavRELPOSNED messages if enabled */ - void callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m); + void callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9& m); sensor_msgs::Imu imu_; @@ -1372,18 +1422,19 @@ class HpPosRecProduct: public virtual HpgRefProduct { * @brief Implements functions for Time Sync products. * @todo partially implemented */ -class TimProduct: public virtual ComponentInterface { +class TimProduct : public virtual ComponentInterface +{ /** * @brief Get the Time Sync parameters. * @todo Currently unimplemented. */ - void getRosParams(); - + void getRosParams(); + /** * @brief Configure Time Sync settings. * @todo Currently unimplemented. */ - bool configureUblox(); + bool configureUblox(); /** * @brief Subscribe to Time Sync messages. @@ -1398,16 +1449,16 @@ class TimProduct: public virtual ComponentInterface { */ void initializeRosDiagnostics(); - protected: +protected: /** - * @brief + * @brief * @details Publish recieved TimTM2 messages if enabled */ - void callbackTimTM2(const ublox_msgs::TimTM2 &m); - + void callbackTimTM2(const ublox_msgs::TimTM2& m); + sensor_msgs::TimeReference t_ref_; }; -} +} // namespace ublox_node #endif diff --git a/ublox_gps/include/ublox_gps/raw_data_pa.h b/ublox_gps/include/ublox_gps/raw_data_pa.h index 95d71ac9..8d1c15c6 100644 --- a/ublox_gps/include/ublox_gps/raw_data_pa.h +++ b/ublox_gps/include/ublox_gps/raw_data_pa.h @@ -32,7 +32,6 @@ // the data as ros messages. This is used by our group to also evaluate the // measured data with the rtklib. - #ifndef UBLOX_RAW_DATA_PA_H #define UBLOX_RAW_DATA_PA_H @@ -52,93 +51,91 @@ * This namespace is for the ROS u-blox node and handles anything regarding * ROS parameters, message passing, diagnostics, etc. */ -namespace ublox_node { - +namespace ublox_node +{ /** * @brief Implements functions for raw data stream. */ -class RawDataStreamPa { - public: - - - /** - * @brief Constructor. - * Initialises variables and the nodehandle. - */ - RawDataStreamPa(bool is_ros_subscriber = false); - - /** - * @brief Get the raw data stream parameters. - */ - void getRosParams(void); - - /** - * @brief Returns the if raw data streaming is enabled. - */ - bool isEnabled(void); - - /** - * @brief Initializes raw data streams - * If storing to file is enabled, the filename is created and the - * corresponding filedescriptor will be opened. - * If publishing ros messages is enabled, an empty msg will be published. - * (This will implicitly create the publisher) - */ - void initialize(void); - - /** - * @brief Callback function which handles raw data. - * @param data the buffer of u-blox messages to process - * @param size the size of the buffer - */ - void ubloxCallback(const unsigned char* data, - const std::size_t size); - - /** - * @brief Callback function which handles raw data. - * @param msg ros message - */ - void msgCallback(const std_msgs::UInt8MultiArray::ConstPtr& msg); - - private: - /** - * @brief Converts a string into an uint8 multibyte array - */ - std_msgs::UInt8MultiArray str2uint8(const std::string str); - - /** - * @brief Publishes data stream as ros message - * @param str raw data stream as string - */ - void publishMsg(const std::string str); - - /** - * @brief Stores data to given file - * @param str raw data stream as string - */ - void saveToFile(const std::string str); - - //! Directoy name for storing raw data - std::string file_dir_; - //! Filename for storing raw data - std::string file_name_; - //! Handle for file access - std::ofstream file_handle_; - - //! Flag for publishing raw data - bool flag_publish_; - - //! Internal flag - //! true : subscribing to ros messages and storing those to file - //! false: publishing ros messages and/or storing to file - bool is_ros_subscriber_; - - //! ROS private node handle (for params and publisher) - ros::NodeHandle pnh_; - //! ROS node handle (only for subscriber) - ros::NodeHandle nh_; +class RawDataStreamPa +{ +public: + /** + * @brief Constructor. + * Initialises variables and the nodehandle. + */ + RawDataStreamPa(bool is_ros_subscriber = false); + + /** + * @brief Get the raw data stream parameters. + */ + void getRosParams(void); + + /** + * @brief Returns the if raw data streaming is enabled. + */ + bool isEnabled(void); + + /** + * @brief Initializes raw data streams + * If storing to file is enabled, the filename is created and the + * corresponding filedescriptor will be opened. + * If publishing ros messages is enabled, an empty msg will be published. + * (This will implicitly create the publisher) + */ + void initialize(void); + + /** + * @brief Callback function which handles raw data. + * @param data the buffer of u-blox messages to process + * @param size the size of the buffer + */ + void ubloxCallback(const unsigned char* data, const std::size_t size); + + /** + * @brief Callback function which handles raw data. + * @param msg ros message + */ + void msgCallback(const std_msgs::UInt8MultiArray::ConstPtr& msg); + +private: + /** + * @brief Converts a string into an uint8 multibyte array + */ + std_msgs::UInt8MultiArray str2uint8(const std::string str); + + /** + * @brief Publishes data stream as ros message + * @param str raw data stream as string + */ + void publishMsg(const std::string str); + + /** + * @brief Stores data to given file + * @param str raw data stream as string + */ + void saveToFile(const std::string str); + + //! Directoy name for storing raw data + std::string file_dir_; + //! Filename for storing raw data + std::string file_name_; + //! Handle for file access + std::ofstream file_handle_; + + //! Flag for publishing raw data + bool flag_publish_; + + //! Internal flag + //! true : subscribing to ros messages and storing those to file + //! false: publishing ros messages and/or storing to file + bool is_ros_subscriber_; + + //! ROS private node handle (for params and publisher) + ros::NodeHandle pnh_; + //! ROS node handle (only for subscriber) + ros::NodeHandle nh_; }; -} +} // namespace ublox_node #endif diff --git a/ublox_gps/include/ublox_gps/utils.h b/ublox_gps/include/ublox_gps/utils.h index ee649eae..aa8468b1 100644 --- a/ublox_gps/include/ublox_gps/utils.h +++ b/ublox_gps/include/ublox_gps/utils.h @@ -6,24 +6,25 @@ #include "ublox_msgs/NavPVT.h" extern "C" { - #include "ublox_gps/mkgmtime.h" +#include "ublox_gps/mkgmtime.h" } /** * @brief Convert date/time to UTC time in seconds. */ -template -long toUtcSeconds(const NavPVT& msg) { +template +long toUtcSeconds(const NavPVT& msg) +{ // Create TM struct for mkgmtime - struct tm time = {0}; - time.tm_year = msg.year - 1900; - time.tm_mon = msg.month - 1; + struct tm time = { 0 }; + time.tm_year = msg.year - 1900; + time.tm_mon = msg.month - 1; time.tm_mday = msg.day; - time.tm_hour = msg.hour; - time.tm_min = msg.min; + time.tm_hour = msg.hour; + time.tm_min = msg.min; time.tm_sec = msg.sec; // C++ STL doesn't have a mkgmtime (though other libraries do) - // STL mktime converts date/time to seconds in local time + // STL mktime converts date/time to seconds in local time // A modified version of code external library is used for mkgmtime return mkgmtime(&time); } diff --git a/ublox_gps/include/ublox_gps/worker.h b/ublox_gps/include/ublox_gps/worker.h index 6a9bef52..b00aea2f 100644 --- a/ublox_gps/include/ublox_gps/worker.h +++ b/ublox_gps/include/ublox_gps/worker.h @@ -14,9 +14,9 @@ // 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 +// 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 BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; @@ -32,15 +32,18 @@ #include #include -namespace ublox_gps { - +namespace ublox_gps +{ /** * @brief Handles I/O reading and writing. */ -class Worker { - public: +class Worker +{ +public: typedef boost::function Callback; - virtual ~Worker() {} + virtual ~Worker() + { + } /** * @brief Set the callback function for received messages. @@ -60,7 +63,7 @@ class Worker { * @param size the size of the buffer */ virtual bool send(const unsigned char* data, const unsigned int size) = 0; - + /** * @brief Wait for an incoming message. * @param timeout the maximum time to wait. diff --git a/ublox_gps/src/gps.cpp b/ublox_gps/src/gps.cpp index 60cd41fb..8c3ea0de 100644 --- a/ublox_gps/src/gps.cpp +++ b/ublox_gps/src/gps.cpp @@ -30,44 +30,47 @@ #include #include -namespace ublox_gps { - +namespace ublox_gps +{ using namespace ublox_msgs; //! Sleep time [ms] after setting the baudrate constexpr static int kSetBaudrateSleepMs = 500; -const boost::posix_time::time_duration Gps::default_timeout_ = - boost::posix_time::milliseconds( - static_cast(Gps::kDefaultAckTimeout * 1000)); +const boost::posix_time::time_duration Gps::default_timeout_ = boost::posix_time::milliseconds( + static_cast(Gps::kDefaultAckTimeout * 1000)); -Gps::Gps() : configured_(false), config_on_startup_flag_(true) { - subscribeAcks(); +Gps::Gps() : configured_(false), config_on_startup_flag_(true) +{ + subscribeAcks(); } -Gps::~Gps() { close(); } +Gps::~Gps() +{ + close(); +} -void Gps::setWorker(const boost::shared_ptr& worker) { - if (worker_) return; +void Gps::setWorker(const boost::shared_ptr& worker) +{ + if (worker_) + return; worker_ = worker; - worker_->setCallback(boost::bind(&CallbackHandlers::readCallback, - &callbacks_, _1, _2)); + worker_->setCallback(boost::bind(&CallbackHandlers::readCallback, &callbacks_, _1, _2)); configured_ = static_cast(worker); } -void Gps::subscribeAcks() { +void Gps::subscribeAcks() +{ // Set NACK handler - subscribeId(boost::bind(&Gps::processNack, this, _1), - ublox_msgs::Message::ACK::NACK); + subscribeId(boost::bind(&Gps::processNack, this, _1), ublox_msgs::Message::ACK::NACK); // Set ACK handler - subscribeId(boost::bind(&Gps::processAck, this, _1), - ublox_msgs::Message::ACK::ACK); + subscribeId(boost::bind(&Gps::processAck, this, _1), ublox_msgs::Message::ACK::ACK); // Set UPD-SOS-ACK handler - subscribe( - boost::bind(&Gps::processUpdSosAck, this, _1)); + subscribe(boost::bind(&Gps::processUpdSosAck, this, _1)); } -void Gps::processAck(const ublox_msgs::Ack &m) { +void Gps::processAck(const ublox_msgs::Ack& m) +{ // Process ACK/NACK messages Ack ack; ack.type = ACK; @@ -75,11 +78,11 @@ void Gps::processAck(const ublox_msgs::Ack &m) { ack.msg_id = m.msgID; // store the ack atomically ack_.store(ack, boost::memory_order_seq_cst); - ROS_DEBUG_COND(debug >= 2, "U-blox: received ACK: 0x%02x / 0x%02x", - m.clsID, m.msgID); + ROS_DEBUG_COND(debug >= 2, "U-blox: received ACK: 0x%02x / 0x%02x", m.clsID, m.msgID); } -void Gps::processNack(const ublox_msgs::Ack &m) { +void Gps::processNack(const ublox_msgs::Ack& m) +{ // Process ACK/NACK messages Ack ack; ack.type = NACK; @@ -90,40 +93,41 @@ void Gps::processNack(const ublox_msgs::Ack &m) { ROS_ERROR("U-blox: received NACK: 0x%02x / 0x%02x", m.clsID, m.msgID); } -void Gps::processUpdSosAck(const ublox_msgs::UpdSOS_Ack &m) { - if (m.cmd == UpdSOS_Ack::CMD_BACKUP_CREATE_ACK) { +void Gps::processUpdSosAck(const ublox_msgs::UpdSOS_Ack& m) +{ + if (m.cmd == UpdSOS_Ack::CMD_BACKUP_CREATE_ACK) + { Ack ack; ack.type = (m.response == m.BACKUP_CREATE_ACK) ? ACK : NACK; ack.class_id = m.CLASS_ID; ack.msg_id = m.MESSAGE_ID; // store the ack atomically ack_.store(ack, boost::memory_order_seq_cst); - ROS_DEBUG_COND(ack.type == ACK && debug >= 2, - "U-blox: received UPD SOS Backup ACK"); - if(ack.type == NACK) + ROS_DEBUG_COND(ack.type == ACK && debug >= 2, "U-blox: received UPD SOS Backup ACK"); + if (ack.type == NACK) ROS_ERROR("U-blox: received UPD SOS Backup NACK"); } } -void Gps::initializeSerial(std::string port, unsigned int baudrate, - uint16_t uart_in, uint16_t uart_out) { +void Gps::initializeSerial(std::string port, unsigned int baudrate, uint16_t uart_in, uint16_t uart_out) +{ port_ = port; - boost::shared_ptr io_service( - new boost::asio::io_service); - boost::shared_ptr serial( - new boost::asio::serial_port(*io_service)); + boost::shared_ptr io_service(new boost::asio::io_service); + boost::shared_ptr serial(new boost::asio::serial_port(*io_service)); // open serial port - try { + try + { serial->open(port); - } catch (std::runtime_error& e) { - throw std::runtime_error("U-Blox: Could not open serial port :" - + port + " " + e.what()); + } + catch (std::runtime_error& e) + { + throw std::runtime_error("U-Blox: Could not open serial port :" + port + " " + e.what()); } ROS_INFO("U-Blox: Opened serial port %s", port.c_str()); - - if(BOOST_VERSION < 106600) + + if (BOOST_VERSION < 106600) { // NOTE(Kartik): Set serial port to "raw" mode. This is done in Boost but // until v1.66.0 there was a bug which didn't enable the relevant code, @@ -136,9 +140,9 @@ void Gps::initializeSerial(std::string port, unsigned int baudrate, } // Set the I/O worker - if (worker_) return; - setWorker(boost::shared_ptr( - new AsyncWorker(serial, io_service))); + if (worker_) + return; + setWorker(boost::shared_ptr(new AsyncWorker(serial, io_service))); configured_ = false; @@ -146,62 +150,67 @@ void Gps::initializeSerial(std::string port, unsigned int baudrate, boost::asio::serial_port_base::baud_rate current_baudrate; serial->get_option(current_baudrate); // Incrementally increase the baudrate to the desired value - for (int i = 0; i < sizeof(kBaudrates)/sizeof(kBaudrates[0]); i++) { + for (int i = 0; i < sizeof(kBaudrates) / sizeof(kBaudrates[0]); i++) + { if (current_baudrate.value() == baudrate) break; // Don't step down, unless the desired baudrate is lower - if(current_baudrate.value() > kBaudrates[i] && baudrate > kBaudrates[i]) + if (current_baudrate.value() > kBaudrates[i] && baudrate > kBaudrates[i]) continue; - serial->set_option( - boost::asio::serial_port_base::baud_rate(kBaudrates[i])); - boost::this_thread::sleep( - boost::posix_time::milliseconds(kSetBaudrateSleepMs)); + serial->set_option(boost::asio::serial_port_base::baud_rate(kBaudrates[i])); + boost::this_thread::sleep(boost::posix_time::milliseconds(kSetBaudrateSleepMs)); serial->get_option(current_baudrate); ROS_DEBUG("U-Blox: Set ASIO baudrate to %u", current_baudrate.value()); } - if (config_on_startup_flag_) { + if (config_on_startup_flag_) + { configured_ = configUart1(baudrate, uart_in, uart_out); - if(!configured_ || current_baudrate.value() != baudrate) { + if (!configured_ || current_baudrate.value() != baudrate) + { throw std::runtime_error("Could not configure serial baud rate"); } - } else { + } + else + { configured_ = true; } } -void Gps::resetSerial(std::string port) { - boost::shared_ptr io_service( - new boost::asio::io_service); - boost::shared_ptr serial( - new boost::asio::serial_port(*io_service)); +void Gps::resetSerial(std::string port) +{ + boost::shared_ptr io_service(new boost::asio::io_service); + boost::shared_ptr serial(new boost::asio::serial_port(*io_service)); // open serial port - try { + try + { serial->open(port); - } catch (std::runtime_error& e) { - throw std::runtime_error("U-Blox: Could not open serial port :" - + port + " " + e.what()); + } + catch (std::runtime_error& e) + { + throw std::runtime_error("U-Blox: Could not open serial port :" + port + " " + e.what()); } ROS_INFO("U-Blox: Reset serial port %s", port.c_str()); // Set the I/O worker - if (worker_) return; - setWorker(boost::shared_ptr( - new AsyncWorker(serial, io_service))); + if (worker_) + return; + setWorker(boost::shared_ptr(new AsyncWorker(serial, io_service))); configured_ = false; // Poll UART PRT Config std::vector payload; payload.push_back(CfgPRT::PORT_ID_UART1); - if (!poll(CfgPRT::CLASS_ID, CfgPRT::MESSAGE_ID, payload)) { + if (!poll(CfgPRT::CLASS_ID, CfgPRT::MESSAGE_ID, payload)) + { ROS_ERROR("Resetting Serial Port: Could not poll UART1 CfgPRT"); return; } CfgPRT prt; - if(!read(prt, default_timeout_)) { - ROS_ERROR("Resetting Serial Port: Could not read polled UART1 CfgPRT %s", - "message"); + if (!read(prt, default_timeout_)) + { + ROS_ERROR("Resetting Serial Port: Could not read polled UART1 CfgPRT %s", "message"); return; } @@ -210,81 +219,83 @@ void Gps::resetSerial(std::string port) { configured_ = true; } -void Gps::initializeTcp(std::string host, std::string port) { +void Gps::initializeTcp(std::string host, std::string port) +{ host_ = host; port_ = port; - boost::shared_ptr io_service( - new boost::asio::io_service); + boost::shared_ptr io_service(new boost::asio::io_service); boost::asio::ip::tcp::resolver::iterator endpoint; - try { + try + { boost::asio::ip::tcp::resolver resolver(*io_service); - endpoint = - resolver.resolve(boost::asio::ip::tcp::resolver::query(host, port)); - } catch (std::runtime_error& e) { - throw std::runtime_error("U-Blox: Could not resolve" + host + " " + - port + " " + e.what()); + endpoint = resolver.resolve(boost::asio::ip::tcp::resolver::query(host, port)); + } + catch (std::runtime_error& e) + { + throw std::runtime_error("U-Blox: Could not resolve" + host + " " + port + " " + e.what()); } - boost::shared_ptr socket( - new boost::asio::ip::tcp::socket(*io_service)); + boost::shared_ptr socket(new boost::asio::ip::tcp::socket(*io_service)); - try { + try + { socket->connect(*endpoint); - } catch (std::runtime_error& e) { - throw std::runtime_error("U-Blox: Could not connect to " + - endpoint->host_name() + ":" + - endpoint->service_name() + ": " + e.what()); + } + catch (std::runtime_error& e) + { + throw std::runtime_error("U-Blox: Could not connect to " + endpoint->host_name() + ":" + endpoint->service_name() + + ": " + e.what()); } - ROS_INFO("U-Blox: Connected to %s:%s.", endpoint->host_name().c_str(), - endpoint->service_name().c_str()); + ROS_INFO("U-Blox: Connected to %s:%s.", endpoint->host_name().c_str(), endpoint->service_name().c_str()); - if (worker_) return; - setWorker(boost::shared_ptr( - new AsyncWorker(socket, - io_service))); + if (worker_) + return; + setWorker(boost::shared_ptr(new AsyncWorker(socket, io_service))); } -void Gps::initializeUdp(std::string host, std::string port) { +void Gps::initializeUdp(std::string host, std::string port) +{ host_ = host; port_ = port; - boost::shared_ptr io_service( - new boost::asio::io_service); + boost::shared_ptr io_service(new boost::asio::io_service); boost::asio::ip::udp::resolver::iterator endpoint; - try { + try + { boost::asio::ip::udp::resolver resolver(*io_service); - endpoint = - resolver.resolve(boost::asio::ip::udp::resolver::query(host, port)); - } catch (std::runtime_error& e) { - throw std::runtime_error("U-Blox: Could not resolve" + host + " " + - port + " " + e.what()); + endpoint = resolver.resolve(boost::asio::ip::udp::resolver::query(host, port)); + } + catch (std::runtime_error& e) + { + throw std::runtime_error("U-Blox: Could not resolve" + host + " " + port + " " + e.what()); } - boost::shared_ptr socket( - new boost::asio::ip::udp::socket(*io_service)); + boost::shared_ptr socket(new boost::asio::ip::udp::socket(*io_service)); - try { + try + { socket->connect(*endpoint); - } catch (std::runtime_error& e) { - throw std::runtime_error("U-Blox: Could not connect to " + - endpoint->host_name() + ":" + - endpoint->service_name() + ": " + e.what()); + } + catch (std::runtime_error& e) + { + throw std::runtime_error("U-Blox: Could not connect to " + endpoint->host_name() + ":" + endpoint->service_name() + + ": " + e.what()); } - ROS_INFO("U-Blox: Connected to %s:%s.", endpoint->host_name().c_str(), - endpoint->service_name().c_str()); + ROS_INFO("U-Blox: Connected to %s:%s.", endpoint->host_name().c_str(), endpoint->service_name().c_str()); - if (worker_) return; - setWorker(boost::shared_ptr( - new AsyncWorker(socket, - io_service))); + if (worker_) + return; + setWorker(boost::shared_ptr(new AsyncWorker(socket, io_service))); } -void Gps::close() { - if(save_on_shutdown_) { - if(saveOnShutdown()) +void Gps::close() +{ + if (save_on_shutdown_) + { + if (saveOnShutdown()) ROS_INFO("U-Blox Flash BBR saved"); else ROS_INFO("U-Blox Flash BBR failed to save"); @@ -293,7 +304,8 @@ void Gps::close() { configured_ = false; } -void Gps::reset(const boost::posix_time::time_duration& wait) { +void Gps::reset(const boost::posix_time::time_duration& wait) +{ worker_.reset(); configured_ = false; // sleep because of undefined behavior after I/O reset @@ -304,9 +316,9 @@ void Gps::reset(const boost::posix_time::time_duration& wait) { initializeTcp(host_, port_); } -bool Gps::configReset(uint16_t nav_bbr_mask, uint16_t reset_mode) { - ROS_WARN("Resetting u-blox. If device address changes, %s", - "node must be relaunched."); +bool Gps::configReset(uint16_t nav_bbr_mask, uint16_t reset_mode) +{ + ROS_WARN("Resetting u-blox. If device address changes, %s", "node must be relaunched."); CfgRST rst; rst.navBbrMask = nav_bbr_mask; @@ -318,8 +330,8 @@ bool Gps::configReset(uint16_t nav_bbr_mask, uint16_t reset_mode) { return true; } -bool Gps::configGnss(CfgGNSS gnss, - const boost::posix_time::time_duration& wait) { +bool Gps::configGnss(CfgGNSS gnss, const boost::posix_time::time_duration& wait) +{ // Configure the GNSS settingshttps://mail.google.com/mail/u/0/#inbox ROS_DEBUG("Re-configuring GNSS."); if (!configure(gnss)) @@ -334,12 +346,13 @@ bool Gps::configGnss(CfgGNSS gnss, return isConfigured(); } -bool Gps::saveOnShutdown() { +bool Gps::saveOnShutdown() +{ // Command the receiver to stop CfgRST rst; rst.navBbrMask = rst.NAV_BBR_HOT_START; rst.resetMode = rst.RESET_MODE_GNSS_STOP; - if(!configure(rst)) + if (!configure(rst)) return false; // Command saving the contents of BBR to flash memory // And wait for UBX-UPD-SOS-ACK @@ -347,7 +360,8 @@ bool Gps::saveOnShutdown() { return configure(backup); } -bool Gps::clearBbr() { +bool Gps::clearBbr() +{ // Command saving the contents of BBR to flash memory // And wait for UBX-UPD-SOS-ACK UpdSOS sos; @@ -355,34 +369,36 @@ bool Gps::clearBbr() { return configure(sos); } -bool Gps::configUart1(unsigned int baudrate, uint16_t in_proto_mask, - uint16_t out_proto_mask) { - if (!worker_) return true; +bool Gps::configUart1(unsigned int baudrate, uint16_t in_proto_mask, uint16_t out_proto_mask) +{ + if (!worker_) + return true; - ROS_DEBUG("Configuring UART1 baud rate: %u, In/Out Protocol: %u / %u", - baudrate, in_proto_mask, out_proto_mask); + ROS_DEBUG("Configuring UART1 baud rate: %u, In/Out Protocol: %u / %u", baudrate, in_proto_mask, out_proto_mask); CfgPRT port; port.portID = CfgPRT::PORT_ID_UART1; port.baudRate = baudrate; - port.mode = CfgPRT::MODE_RESERVED1 | CfgPRT::MODE_CHAR_LEN_8BIT | - CfgPRT::MODE_PARITY_NO | CfgPRT::MODE_STOP_BITS_1; + port.mode = CfgPRT::MODE_RESERVED1 | CfgPRT::MODE_CHAR_LEN_8BIT | CfgPRT::MODE_PARITY_NO | CfgPRT::MODE_STOP_BITS_1; port.inProtoMask = in_proto_mask; port.outProtoMask = out_proto_mask; return configure(port); } -bool Gps::disableUart1(CfgPRT& prev_config) { +bool Gps::disableUart1(CfgPRT& prev_config) +{ ROS_DEBUG("Disabling UART1"); // Poll UART PRT Config std::vector payload; payload.push_back(CfgPRT::PORT_ID_UART1); - if (!poll(CfgPRT::CLASS_ID, CfgPRT::MESSAGE_ID, payload)) { + if (!poll(CfgPRT::CLASS_ID, CfgPRT::MESSAGE_ID, payload)) + { ROS_ERROR("disableUart: Could not poll UART1 CfgPRT"); return false; } - if(!read(prev_config, default_timeout_)) { + if (!read(prev_config, default_timeout_)) + { ROS_ERROR("disableUart: Could not read polled UART1 CfgPRT message"); return false; } @@ -398,13 +414,12 @@ bool Gps::disableUart1(CfgPRT& prev_config) { return configure(port); } -bool Gps::configUsb(uint16_t tx_ready, - uint16_t in_proto_mask, - uint16_t out_proto_mask) { - if (!worker_) return true; +bool Gps::configUsb(uint16_t tx_ready, uint16_t in_proto_mask, uint16_t out_proto_mask) +{ + if (!worker_) + return true; - ROS_DEBUG("Configuring USB tx_ready: %u, In/Out Protocol: %u / %u", - tx_ready, in_proto_mask, out_proto_mask); + ROS_DEBUG("Configuring USB tx_ready: %u, In/Out Protocol: %u / %u", tx_ready, in_proto_mask, out_proto_mask); CfgPRT port; port.portID = CfgPRT::PORT_ID_USB; @@ -414,9 +429,9 @@ bool Gps::configUsb(uint16_t tx_ready, return configure(port); } -bool Gps::configRate(uint16_t meas_rate, uint16_t nav_rate) { - ROS_DEBUG("Configuring measurement rate to %u ms and nav rate to %u cycles", - meas_rate, nav_rate); +bool Gps::configRate(uint16_t meas_rate, uint16_t nav_rate) +{ + ROS_DEBUG("Configuring measurement rate to %u ms and nav rate to %u cycles", meas_rate, nav_rate); CfgRATE rate; rate.measRate = meas_rate; @@ -425,10 +440,13 @@ bool Gps::configRate(uint16_t meas_rate, uint16_t nav_rate) { return configure(rate); } -bool Gps::configRtcm(std::vector ids, std::vector rates) { - for(size_t i = 0; i < ids.size(); ++i) { +bool Gps::configRtcm(std::vector ids, std::vector rates) +{ + for (size_t i = 0; i < ids.size(); ++i) + { ROS_DEBUG("Setting RTCM %d Rate %u", ids[i], rates[i]); - if(!setRate(ublox_msgs::Class::RTCM, (uint8_t)ids[i], rates[i])) { + if (!setRate(ublox_msgs::Class::RTCM, (uint8_t)ids[i], rates[i])) + { ROS_ERROR("Could not set RTCM %d to rate %u", ids[i], rates[i]); return false; } @@ -436,7 +454,8 @@ bool Gps::configRtcm(std::vector ids, std::vector rates) { return true; } -bool Gps::configSbas(bool enable, uint8_t usage, uint8_t max_sbas) { +bool Gps::configSbas(bool enable, uint8_t usage, uint8_t max_sbas) +{ ROS_DEBUG("Configuring SBAS: usage %u, max_sbas %u", usage, max_sbas); ublox_msgs::CfgSBAS msg; @@ -449,10 +468,11 @@ bool Gps::configSbas(bool enable, uint8_t usage, uint8_t max_sbas) { bool Gps::configTmode3Fixed(bool lla_flag, std::vector arp_position, std::vector arp_position_hp, - float fixed_pos_acc) { - if(arp_position.size() != 3 || arp_position_hp.size() != 3) { - ROS_ERROR("Configuring TMODE3 to Fixed: size of position %s", - "& arp_position_hp args must be 3"); + float fixed_pos_acc) +{ + if (arp_position.size() != 3 || arp_position_hp.size() != 3) + { + ROS_ERROR("Configuring TMODE3 to Fixed: size of position %s", "& arp_position_hp args must be 3"); return false; } @@ -463,12 +483,15 @@ bool Gps::configTmode3Fixed(bool lla_flag, tmode3.flags |= lla_flag ? tmode3.FLAGS_LLA : 0; // Set position - if(lla_flag) { + if (lla_flag) + { // Convert from [deg] to [deg * 1e-7] tmode3.ecefXOrLat = (int)round(arp_position[0] * 1e7); tmode3.ecefYOrLon = (int)round(arp_position[1] * 1e7); tmode3.ecefZOrAlt = (int)round(arp_position[2] * 1e7); - } else { + } + else + { // Convert from m to cm tmode3.ecefXOrLat = (int)round(arp_position[0] * 1e2); tmode3.ecefYOrLon = (int)round(arp_position[1] * 1e2); @@ -482,8 +505,8 @@ bool Gps::configTmode3Fixed(bool lla_flag, return configure(tmode3); } -bool Gps::configTmode3SurveyIn(unsigned int svin_min_dur, - float svin_acc_limit) { +bool Gps::configTmode3SurveyIn(unsigned int svin_min_dur, float svin_acc_limit) +{ CfgTMODE3 tmode3; ROS_DEBUG("Setting TMODE3 to Survey In"); tmode3.flags = tmode3.FLAGS_MODE_SURVEY_IN & tmode3.FLAGS_MODE_MASK; @@ -493,7 +516,8 @@ bool Gps::configTmode3SurveyIn(unsigned int svin_min_dur, return configure(tmode3); } -bool Gps::disableTmode3() { +bool Gps::disableTmode3() +{ ROS_DEBUG("Disabling TMODE3"); CfgTMODE3 tmode3; @@ -501,9 +525,9 @@ bool Gps::disableTmode3() { return configure(tmode3); } -bool Gps::setRate(uint8_t class_id, uint8_t message_id, uint8_t rate) { - ROS_DEBUG_COND(debug >= 2, "Setting rate 0x%02x, 0x%02x, %u", class_id, - message_id, rate); +bool Gps::setRate(uint8_t class_id, uint8_t message_id, uint8_t rate) +{ + ROS_DEBUG_COND(debug >= 2, "Setting rate 0x%02x, 0x%02x, %u", class_id, message_id, rate); ublox_msgs::CfgMSG msg; msg.msgClass = class_id; msg.msgID = message_id; @@ -511,7 +535,8 @@ bool Gps::setRate(uint8_t class_id, uint8_t message_id, uint8_t rate) { return configure(msg); } -bool Gps::setDynamicModel(uint8_t model) { +bool Gps::setDynamicModel(uint8_t model) +{ ROS_DEBUG("Setting dynamic model to %u", model); ublox_msgs::CfgNAV5 msg; @@ -520,7 +545,8 @@ bool Gps::setDynamicModel(uint8_t model) { return configure(msg); } -bool Gps::setFixMode(uint8_t mode) { +bool Gps::setFixMode(uint8_t mode) +{ ROS_DEBUG("Setting fix mode to %u", mode); ublox_msgs::CfgNAV5 msg; @@ -529,7 +555,8 @@ bool Gps::setFixMode(uint8_t mode) { return configure(msg); } -bool Gps::setDeadReckonLimit(uint8_t limit) { +bool Gps::setDeadReckonLimit(uint8_t limit) +{ ROS_DEBUG("Setting DR Limit to %u", limit); ublox_msgs::CfgNAV5 msg; @@ -538,47 +565,53 @@ bool Gps::setDeadReckonLimit(uint8_t limit) { return configure(msg); } -bool Gps::setPpp(bool enable, float protocol_version) { +bool Gps::setPpp(bool enable, float protocol_version) +{ ROS_DEBUG("%s PPP", (enable ? "Enabling" : "Disabling")); ublox_msgs::CfgNAVX5 msg; msg.usePPP = enable; - if(protocol_version >= 18) + if (protocol_version >= 18) msg.version = 2; msg.mask1 = ublox_msgs::CfgNAVX5::MASK1_PPP; return configure(msg); } -bool Gps::setDgnss(uint8_t mode) { +bool Gps::setDgnss(uint8_t mode) +{ CfgDGNSS cfg; ROS_DEBUG("Setting DGNSS mode to %u", mode); cfg.dgnssMode = mode; return configure(cfg); } -bool Gps::setUseAdr(bool enable, float protocol_version) { +bool Gps::setUseAdr(bool enable, float protocol_version) +{ ROS_DEBUG("%s ADR/UDR", (enable ? "Enabling" : "Disabling")); ublox_msgs::CfgNAVX5 msg; msg.useAdr = enable; - - if(protocol_version >= 18) + + if (protocol_version >= 18) msg.version = 2; msg.mask2 = ublox_msgs::CfgNAVX5::MASK2_ADR; return configure(msg); } -bool Gps::sendRtcm(const std::vector& rtcm){ +bool Gps::sendRtcm(const std::vector& rtcm) +{ return worker_->send(rtcm.data(), rtcm.size()); } -bool Gps::sendSpartn(const std::vector& message){ +bool Gps::sendSpartn(const std::vector& message) +{ return worker_->send(message.data(), message.size()); } -bool Gps::poll(uint8_t class_id, uint8_t message_id, - const std::vector& payload) { - if (!worker_) return false; +bool Gps::poll(uint8_t class_id, uint8_t message_id, const std::vector& payload) +{ + if (!worker_) + return false; std::vector out(kWriterSize); ublox::Writer writer(out.data(), out.size()); @@ -589,33 +622,31 @@ bool Gps::poll(uint8_t class_id, uint8_t message_id, return true; } -bool Gps::waitForAcknowledge(const boost::posix_time::time_duration& timeout, - uint8_t class_id, uint8_t msg_id) { - ROS_DEBUG_COND(debug >= 2, "Waiting for ACK 0x%02x / 0x%02x", - class_id, msg_id); - boost::posix_time::ptime wait_until( - boost::posix_time::second_clock::local_time() + timeout); +bool Gps::waitForAcknowledge(const boost::posix_time::time_duration& timeout, uint8_t class_id, uint8_t msg_id) +{ + ROS_DEBUG_COND(debug >= 2, "Waiting for ACK 0x%02x / 0x%02x", class_id, msg_id); + boost::posix_time::ptime wait_until(boost::posix_time::second_clock::local_time() + timeout); Ack ack = ack_.load(boost::memory_order_seq_cst); - while (boost::posix_time::second_clock::local_time() < wait_until - && (ack.class_id != class_id - || ack.msg_id != msg_id - || ack.type == WAIT)) { + while (boost::posix_time::second_clock::local_time() < wait_until && + (ack.class_id != class_id || ack.msg_id != msg_id || ack.type == WAIT)) + { worker_->wait(timeout); ack = ack_.load(boost::memory_order_seq_cst); } - bool result = ack.type == ACK - && ack.class_id == class_id - && ack.msg_id == msg_id; + bool result = ack.type == ACK && ack.class_id == class_id && ack.msg_id == msg_id; return result; } -void Gps::setRawDataCallback(const Worker::Callback& callback) { - if (! worker_) return; +void Gps::setRawDataCallback(const Worker::Callback& callback) +{ + if (!worker_) + return; worker_->setRawDataCallback(callback); } -bool Gps::setUTCtime() { +bool Gps::setUTCtime() +{ ROS_DEBUG("Setting time to UTC time"); ublox_msgs::CfgNAV5 msg; @@ -623,12 +654,13 @@ bool Gps::setUTCtime() { return configure(msg); } -bool Gps::setTimtm2(uint8_t rate) { - ROS_DEBUG("TIM-TM2 send rate on current port set to %u", rate ); +bool Gps::setTimtm2(uint8_t rate) +{ + ROS_DEBUG("TIM-TM2 send rate on current port set to %u", rate); ublox_msgs::CfgMSG msg; msg.msgClass = ublox_msgs::TimTM2::CLASS_ID; msg.msgID = ublox_msgs::TimTM2::MESSAGE_ID; - msg.rate = rate; + msg.rate = rate; return configure(msg); } } // namespace ublox_gps diff --git a/ublox_gps/src/logger_node_pa.cpp b/ublox_gps/src/logger_node_pa.cpp index 5cb4fb91..e34fec99 100644 --- a/ublox_gps/src/logger_node_pa.cpp +++ b/ublox_gps/src/logger_node_pa.cpp @@ -43,15 +43,15 @@ using namespace ublox_node; // Raw Data Stream (feature from TUC-ProAut) // -int main(int argc, char** argv) { +int main(int argc, char** argv) +{ + ros::init(argc, argv, "ublox_logger"); - ros::init(argc, argv, "ublox_logger"); - - RawDataStreamPa node(true); - node.getRosParams(); - node.initialize(); + RawDataStreamPa node(true); + node.getRosParams(); + node.initialize(); - ros::spin(); + ros::spin(); - return 0; + return 0; } diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 09c99cb6..dba144ac 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -40,69 +40,102 @@ constexpr static int kResetWait = 10; // // ublox_node namespace // -uint8_t ublox_node::modelFromString(const std::string& model) { +uint8_t ublox_node::modelFromString(const std::string& model) +{ std::string lower = model; std::transform(lower.begin(), lower.end(), lower.begin(), ::tolower); - if(lower == "portable") { + if (lower == "portable") + { return ublox_msgs::CfgNAV5::DYN_MODEL_PORTABLE; - } else if(lower == "stationary") { + } + else if (lower == "stationary") + { return ublox_msgs::CfgNAV5::DYN_MODEL_STATIONARY; - } else if(lower == "pedestrian") { + } + else if (lower == "pedestrian") + { return ublox_msgs::CfgNAV5::DYN_MODEL_PEDESTRIAN; - } else if(lower == "automotive") { + } + else if (lower == "automotive") + { return ublox_msgs::CfgNAV5::DYN_MODEL_AUTOMOTIVE; - } else if(lower == "sea") { + } + else if (lower == "sea") + { return ublox_msgs::CfgNAV5::DYN_MODEL_SEA; - } else if(lower == "airborne1") { + } + else if (lower == "airborne1") + { return ublox_msgs::CfgNAV5::DYN_MODEL_AIRBORNE_1G; - } else if(lower == "airborne2") { + } + else if (lower == "airborne2") + { return ublox_msgs::CfgNAV5::DYN_MODEL_AIRBORNE_2G; - } else if(lower == "airborne4") { + } + else if (lower == "airborne4") + { return ublox_msgs::CfgNAV5::DYN_MODEL_AIRBORNE_4G; - } else if(lower == "wristwatch") { + } + else if (lower == "wristwatch") + { return ublox_msgs::CfgNAV5::DYN_MODEL_WRIST_WATCH; - } else if(lower == "bike") { + } + else if (lower == "bike") + { return ublox_msgs::CfgNAV5::DYN_MODEL_BIKE; } - throw std::runtime_error("Invalid settings: " + lower + - " is not a valid dynamic model."); + throw std::runtime_error("Invalid settings: " + lower + " is not a valid dynamic model."); } -uint8_t ublox_node::fixModeFromString(const std::string& mode) { +uint8_t ublox_node::fixModeFromString(const std::string& mode) +{ std::string lower = mode; std::transform(lower.begin(), lower.end(), lower.begin(), ::tolower); - if (lower == "2d") { + if (lower == "2d") + { return ublox_msgs::CfgNAV5::FIX_MODE_2D_ONLY; - } else if (lower == "3d") { + } + else if (lower == "3d") + { return ublox_msgs::CfgNAV5::FIX_MODE_3D_ONLY; - } else if (lower == "auto") { + } + else if (lower == "auto") + { return ublox_msgs::CfgNAV5::FIX_MODE_AUTO; } - throw std::runtime_error("Invalid settings: " + mode + - " is not a valid fix mode."); + throw std::runtime_error("Invalid settings: " + mode + " is not a valid fix mode."); } // // u-blox ROS Node // -UbloxNode::UbloxNode() { +UbloxNode::UbloxNode() +{ initialize(); } -void UbloxNode::addFirmwareInterface() { +void UbloxNode::addFirmwareInterface() +{ int ublox_version; - if (protocol_version_ < 14) { + if (protocol_version_ < 14) + { components_.push_back(ComponentPtr(new UbloxFirmware6)); ublox_version = 6; - } else if (protocol_version_ >= 14 && protocol_version_ <= 15) { + } + else if (protocol_version_ >= 14 && protocol_version_ <= 15) + { components_.push_back(ComponentPtr(new UbloxFirmware7)); ublox_version = 7; - } else if (protocol_version_ > 15 && protocol_version_ <= 23) { + } + else if (protocol_version_ > 15 && protocol_version_ <= 23) + { components_.push_back(ComponentPtr(new UbloxFirmware8)); ublox_version = 8; - } else { + } + else + { components_.push_back(ComponentPtr(new UbloxFirmware9)); ublox_version = 9; } @@ -110,9 +143,8 @@ void UbloxNode::addFirmwareInterface() { ROS_INFO("U-Blox Firmware Version: %d", ublox_version); } - -void UbloxNode::addProductInterface(std::string product_category, - std::string ref_rov) { +void UbloxNode::addProductInterface(std::string product_category, std::string ref_rov) +{ if (product_category.compare("HPG") == 0 && ref_rov.compare("REF") == 0) components_.push_back(ComponentPtr(new HpgRefProduct)); else if (product_category.compare("HPG") == 0 && ref_rov.compare("ROV") == 0) @@ -123,18 +155,19 @@ void UbloxNode::addProductInterface(std::string product_category, components_.push_back(ComponentPtr(new HpPosRecProduct)); else if (product_category.compare("TIM") == 0) components_.push_back(ComponentPtr(new TimProduct)); - else if (product_category.compare("ADR") == 0 || - product_category.compare("UDR") == 0) + else if (product_category.compare("ADR") == 0 || product_category.compare("UDR") == 0) components_.push_back(ComponentPtr(new AdrUdrProduct(protocol_version_))); else if (product_category.compare("FTS") == 0) components_.push_back(ComponentPtr(new FtsProduct)); - else if(product_category.compare("SPG") != 0) + else if (product_category.compare("SPG") != 0) ROS_WARN("Product category %s %s from MonVER message not recognized %s", - product_category.c_str(), ref_rov.c_str(), + product_category.c_str(), + ref_rov.c_str(), "options are HPG REF, HPG ROV, HPG #.#, HDG #.#, TIM, ADR, UDR, FTS, SPG"); } -void UbloxNode::getRosParams() { +void UbloxNode::getRosParams() +{ nh->param("device", device_, std::string("/dev/ttyACM0")); nh->param("frame_id", frame_id, std::string("gps")); @@ -146,65 +179,62 @@ void UbloxNode::getRosParams() { // UART 1 params getRosUint("uart1/baudrate", baudrate_, 9600); - getRosUint("uart1/in", uart_in_, ublox_msgs::CfgPRT::PROTO_UBX - | ublox_msgs::CfgPRT::PROTO_NMEA - | ublox_msgs::CfgPRT::PROTO_RTCM); + getRosUint("uart1/in", + uart_in_, + ublox_msgs::CfgPRT::PROTO_UBX | ublox_msgs::CfgPRT::PROTO_NMEA | ublox_msgs::CfgPRT::PROTO_RTCM); getRosUint("uart1/out", uart_out_, ublox_msgs::CfgPRT::PROTO_UBX); // USB params set_usb_ = false; - if (nh->hasParam("usb/in") || nh->hasParam("usb/out")) { + if (nh->hasParam("usb/in") || nh->hasParam("usb/out")) + { set_usb_ = true; - if(!getRosUint("usb/in", usb_in_)) { - throw std::runtime_error(std::string("usb/out is set, therefore ") + - "usb/in must be set"); + if (!getRosUint("usb/in", usb_in_)) + { + throw std::runtime_error(std::string("usb/out is set, therefore ") + "usb/in must be set"); } - if(!getRosUint("usb/out", usb_out_)) { - throw std::runtime_error(std::string("usb/in is set, therefore ") + - "usb/out must be set"); + if (!getRosUint("usb/out", usb_out_)) + { + throw std::runtime_error(std::string("usb/in is set, therefore ") + "usb/out must be set"); } getRosUint("usb/tx_ready", usb_tx_, 0); } // Measurement rate params - nh->param("rate", rate_, 4.0); // in Hz + nh->param("rate", rate_, 4.0); // in Hz getRosUint("nav_rate", nav_rate, 1); // # of measurement rate cycles // RTCM params - getRosUint("rtcm/ids", rtcm_ids); // RTCM output message IDs + getRosUint("rtcm/ids", rtcm_ids); // RTCM output message IDs getRosUint("rtcm/rates", rtcm_rates); // RTCM output message rates // PPP: Advanced Setting nh->param("enable_ppp", enable_ppp_, false); // SBAS params, only for some devices nh->param("sbas", enable_sbas_, false); - getRosUint("sbas/max", max_sbas_, 0); // Maximum number of SBAS channels + getRosUint("sbas/max", max_sbas_, 0); // Maximum number of SBAS channels getRosUint("sbas/usage", sbas_usage_, 0); nh->param("dynamic_model", dynamic_model_, std::string("portable")); nh->param("fix_mode", fix_mode_, std::string("auto")); - getRosUint("dr_limit", dr_limit_, 0); // Dead reckoning limit + getRosUint("dr_limit", dr_limit_, 0); // Dead reckoning limit if (enable_ppp_) ROS_WARN("Warning: PPP is enabled - this is an expert setting."); checkMin(rate_, 0, "rate"); - if(rtcm_ids.size() != rtcm_rates.size()) - throw std::runtime_error(std::string("Invalid settings: size of rtcm_ids") + - " must match size of rtcm_rates"); + if (rtcm_ids.size() != rtcm_rates.size()) + throw std::runtime_error(std::string("Invalid settings: size of rtcm_ids") + " must match size of rtcm_rates"); dmodel_ = modelFromString(dynamic_model_); fmode_ = fixModeFromString(fix_mode_); nh->param("dat/set", set_dat_, false); - if(set_dat_) { + if (set_dat_) + { std::vector shift, rot; - if (!nh->getParam("dat/majA", cfg_dat_.majA) - || nh->getParam("dat/flat", cfg_dat_.flat) - || nh->getParam("dat/shift", shift) - || nh->getParam("dat/rot", rot) - || nh->getParam("dat/scale", cfg_dat_.scale)) + if (!nh->getParam("dat/majA", cfg_dat_.majA) || nh->getParam("dat/flat", cfg_dat_.flat) || + nh->getParam("dat/shift", shift) || nh->getParam("dat/rot", rot) || nh->getParam("dat/scale", cfg_dat_.scale)) throw std::runtime_error(std::string("dat/set is true, therefore ") + - "dat/majA, dat/flat, dat/shift, dat/rot, & dat/scale must be set"); - if(shift.size() != 3 || rot.size() != 3) - throw std::runtime_error(std::string("size of dat/shift & dat/rot ") + - "must be 3"); + "dat/majA, dat/flat, dat/shift, dat/rot, & dat/scale must be set"); + if (shift.size() != 3 || rot.size() != 3) + throw std::runtime_error(std::string("size of dat/shift & dat/rot ") + "must be 3"); checkRange(cfg_dat_.majA, 6300000.0, 6500000.0, "dat/majA"); checkRange(cfg_dat_.flat, 0.0, 500.0, "dat/flat"); @@ -227,16 +257,18 @@ void UbloxNode::getRosParams() { // activate/deactivate any config nh->param("config_on_startup", config_on_startup_flag_, true); - // raw data stream logging + // raw data stream logging rawDataStreamPa_.getRosParams(); } -void UbloxNode::keepAlive(const ros::TimerEvent& event) { +void UbloxNode::keepAlive(const ros::TimerEvent& event) +{ // Poll version message to keep UDP socket active gps.poll(ublox_msgs::MonVER::CLASS_ID, ublox_msgs::MonVER::MESSAGE_ID); } -void UbloxNode::pollMessages(const ros::TimerEvent& event) { +void UbloxNode::pollMessages(const ros::TimerEvent& event) +{ static std::vector payload(1, 1); if (enabled["aid_alm"]) gps.poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::ALM, payload); @@ -246,12 +278,14 @@ void UbloxNode::pollMessages(const ros::TimerEvent& event) { gps.poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::HUI); payload[0]++; - if (payload[0] > 32) { + if (payload[0] > 32) + { payload[0] = 1; } } -void UbloxNode::printInf(const ublox_msgs::Inf &m, uint8_t id) { +void UbloxNode::printInf(const ublox_msgs::Inf& m, uint8_t id) +{ if (id == ublox_msgs::Message::INF::ERROR) ROS_ERROR_STREAM("INF: " << std::string(m.str.begin(), m.str.end())); else if (id == ublox_msgs::Message::INF::WARNING) @@ -262,7 +296,8 @@ void UbloxNode::printInf(const ublox_msgs::Inf &m, uint8_t id) { ROS_INFO_STREAM("INF: " << std::string(m.str.begin(), m.str.end())); } -void UbloxNode::subscribe() { +void UbloxNode::subscribe() +{ ROS_DEBUG("Subscribing to U-Blox messages"); // subscribe messages nh->param("publish/all", enabled["all"], false); @@ -275,76 +310,62 @@ void UbloxNode::subscribe() { // Nav Messages nh->param("publish/nav/status", enabled["nav_status"], enabled["nav"]); if (enabled["nav_status"]) - gps.subscribe(boost::bind( - publish, _1, "navstatus"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "navstatus"), kSubscribeRate); nh->param("publish/nav/posecef", enabled["nav_posecef"], enabled["nav"]); if (enabled["nav_posecef"]) - gps.subscribe(boost::bind( - publish, _1, "navposecef"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "navposecef"), + kSubscribeRate); nh->param("publish/nav/clock", enabled["nav_clock"], enabled["nav"]); if (enabled["nav_clock"]) - gps.subscribe(boost::bind( - publish, _1, "navclock"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "navclock"), kSubscribeRate); // INF messages nh->param("inf/debug", enabled["inf_debug"], false); if (enabled["inf_debug"]) - gps.subscribeId( - boost::bind(&UbloxNode::printInf, this, _1, - ublox_msgs::Message::INF::DEBUG), - ublox_msgs::Message::INF::DEBUG); + gps.subscribeId(boost::bind(&UbloxNode::printInf, this, _1, ublox_msgs::Message::INF::DEBUG), + ublox_msgs::Message::INF::DEBUG); nh->param("inf/error", enabled["inf_error"], enabled["inf"]); if (enabled["inf_error"]) - gps.subscribeId( - boost::bind(&UbloxNode::printInf, this, _1, - ublox_msgs::Message::INF::ERROR), - ublox_msgs::Message::INF::ERROR); + gps.subscribeId(boost::bind(&UbloxNode::printInf, this, _1, ublox_msgs::Message::INF::ERROR), + ublox_msgs::Message::INF::ERROR); nh->param("inf/notice", enabled["inf_notice"], enabled["inf"]); if (enabled["inf_notice"]) - gps.subscribeId( - boost::bind(&UbloxNode::printInf, this, _1, - ublox_msgs::Message::INF::NOTICE), - ublox_msgs::Message::INF::NOTICE); + gps.subscribeId(boost::bind(&UbloxNode::printInf, this, _1, ublox_msgs::Message::INF::NOTICE), + ublox_msgs::Message::INF::NOTICE); nh->param("inf/test", enabled["inf_test"], enabled["inf"]); if (enabled["inf_test"]) - gps.subscribeId( - boost::bind(&UbloxNode::printInf, this, _1, - ublox_msgs::Message::INF::TEST), - ublox_msgs::Message::INF::TEST); + gps.subscribeId(boost::bind(&UbloxNode::printInf, this, _1, ublox_msgs::Message::INF::TEST), + ublox_msgs::Message::INF::TEST); nh->param("inf/warning", enabled["inf_warning"], enabled["inf"]); if (enabled["inf_warning"]) - gps.subscribeId( - boost::bind(&UbloxNode::printInf, this, _1, - ublox_msgs::Message::INF::WARNING), - ublox_msgs::Message::INF::WARNING); + gps.subscribeId(boost::bind(&UbloxNode::printInf, this, _1, ublox_msgs::Message::INF::WARNING), + ublox_msgs::Message::INF::WARNING); // AID messages nh->param("publish/aid/alm", enabled["aid_alm"], enabled["aid"]); if (enabled["aid_alm"]) - gps.subscribe(boost::bind( - publish, _1, "aidalm"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "aidalm"), kSubscribeRate); nh->param("publish/aid/eph", enabled["aid_eph"], enabled["aid"]); if (enabled["aid_eph"]) - gps.subscribe(boost::bind( - publish, _1, "aideph"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "aideph"), kSubscribeRate); nh->param("publish/aid/hui", enabled["aid_hui"], enabled["aid"]); if (enabled["aid_hui"]) - gps.subscribe(boost::bind( - publish, _1, "aidhui"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "aidhui"), kSubscribeRate); - for(int i = 0; i < components_.size(); i++) + for (int i = 0; i < components_.size(); i++) components_[i]->subscribe(); } -void UbloxNode::initializeRosDiagnostics() { +void UbloxNode::initializeRosDiagnostics() +{ if (!nh->hasParam("diagnostic_period")) nh->setParam("diagnostic_period", kDiagnosticPeriod); @@ -352,61 +373,66 @@ void UbloxNode::initializeRosDiagnostics() { updater->setHardwareID("ublox"); // configure diagnostic updater for frequency - freq_diag.reset(new FixDiagnostic(std::string("fix"), kFixFreqTol, - kFixFreqWindow, kTimeStampStatusMin)); - for(int i = 0; i < components_.size(); i++) + freq_diag.reset(new FixDiagnostic(std::string("fix"), kFixFreqTol, kFixFreqWindow, kTimeStampStatusMin)); + for (int i = 0; i < components_.size(); i++) components_[i]->initializeRosDiagnostics(); } - -void UbloxNode::processMonVer() { +void UbloxNode::processMonVer() +{ ublox_msgs::MonVER monVer; if (!gps.poll(monVer)) throw std::runtime_error("Failed to poll MonVER & set relevant settings"); - ROS_DEBUG("%s, HW VER: %s", monVer.swVersion.c_array(), - monVer.hwVersion.c_array()); + ROS_DEBUG("%s, HW VER: %s", monVer.swVersion.c_array(), monVer.hwVersion.c_array()); // Convert extension to vector of strings std::vector extension; extension.reserve(monVer.extension.size()); - for(std::size_t i = 0; i < monVer.extension.size(); ++i) { + for (std::size_t i = 0; i < monVer.extension.size(); ++i) + { ROS_DEBUG("%s", monVer.extension[i].field.c_array()); // Find the end of the string (null character) - unsigned char* end = std::find(monVer.extension[i].field.begin(), - monVer.extension[i].field.end(), '\0'); + unsigned char* end = std::find(monVer.extension[i].field.begin(), monVer.extension[i].field.end(), '\0'); extension.push_back(std::string(monVer.extension[i].field.begin(), end)); } // Get the protocol version - for(std::size_t i = 0; i < extension.size(); ++i) { + for (std::size_t i = 0; i < extension.size(); ++i) + { std::size_t found = extension[i].find("PROTVER"); - if (found != std::string::npos) { - protocol_version_ = ::atof( - extension[i].substr(8, extension[i].size()-8).c_str()); + if (found != std::string::npos) + { + protocol_version_ = ::atof(extension[i].substr(8, extension[i].size() - 8).c_str()); break; } } if (protocol_version_ == 0) - ROS_WARN("Failed to parse MonVER and determine protocol version. %s", - "Defaulting to firmware version 6."); + ROS_WARN("Failed to parse MonVER and determine protocol version. %s", "Defaulting to firmware version 6."); addFirmwareInterface(); - if(protocol_version_ < 18) { + if (protocol_version_ < 18) + { // Final line contains supported GNSS delimited by ; std::vector strs; - if(extension.size() > 0) - boost::split(strs, extension[extension.size()-1], boost::is_any_of(";")); - for(size_t i = 0; i < strs.size(); i++) + if (extension.size() > 0) + boost::split(strs, extension[extension.size() - 1], boost::is_any_of(";")); + for (size_t i = 0; i < strs.size(); i++) supported.insert(strs[i]); - } else { - for(std::size_t i = 0; i < extension.size(); ++i) { + } + else + { + for (std::size_t i = 0; i < extension.size(); ++i) + { std::vector strs; // Up to 2nd to last line - if(i <= extension.size() - 2) { + if (i <= extension.size() - 2) + { boost::split(strs, extension[i], boost::is_any_of("=")); - if(strs.size() > 1) { - if (strs[0].compare(std::string("FWVER")) == 0) { - if(strs[1].length() > 8) + if (strs.size() > 1) + { + if (strs[0].compare(std::string("FWVER")) == 0) + { + if (strs[1].length() > 8) addProductInterface(strs[1].substr(0, 3), strs[1].substr(8, 10)); else addProductInterface(strs[1].substr(0, 3)); @@ -415,62 +441,66 @@ void UbloxNode::processMonVer() { } } // Last 1-2 lines contain supported GNSS - if(i >= extension.size() - 2) { + if (i >= extension.size() - 2) + { boost::split(strs, extension[i], boost::is_any_of(";")); - for(size_t i = 0; i < strs.size(); i++) + for (size_t i = 0; i < strs.size(); i++) supported.insert(strs[i]); } } } } -bool UbloxNode::configureUblox() { - try { +bool UbloxNode::configureUblox() +{ + try + { if (!gps.isInitialized()) throw std::runtime_error("Failed to initialize."); - if (load_.loadMask != 0) { + if (load_.loadMask != 0) + { ROS_DEBUG("Loading u-blox configuration from memory. %u", load_.loadMask); if (!gps.configure(load_)) - throw std::runtime_error(std::string("Failed to load configuration ") + - "from memory"); - if (load_.loadMask & load_.MASK_IO_PORT) { - ROS_DEBUG("Loaded I/O configuration from memory, resetting serial %s", - "communications."); + throw std::runtime_error(std::string("Failed to load configuration ") + "from memory"); + if (load_.loadMask & load_.MASK_IO_PORT) + { + ROS_DEBUG("Loaded I/O configuration from memory, resetting serial %s", "communications."); boost::posix_time::seconds wait(kResetWait); gps.reset(wait); if (!gps.isConfigured()) throw std::runtime_error(std::string("Failed to reset serial I/O") + - "after loading I/O configurations from device memory."); + "after loading I/O configurations from device memory."); } } - if (config_on_startup_flag_) { - if (set_usb_) { + if (config_on_startup_flag_) + { + if (set_usb_) + { gps.configUsb(usb_tx_, usb_in_, usb_out_); } - if (!gps.configRate(meas_rate, nav_rate)) { + if (!gps.configRate(meas_rate, nav_rate)) + { std::stringstream ss; - ss << "Failed to set measurement rate to " << meas_rate - << "ms and navigation rate to " << nav_rate; + ss << "Failed to set measurement rate to " << meas_rate << "ms and navigation rate to " << nav_rate; throw std::runtime_error(ss.str()); } // If device doesn't have SBAS, will receive NACK (causes exception) - if(supportsGnss("SBAS")) { - if (!gps.configSbas(enable_sbas_, sbas_usage_, max_sbas_)) { - throw std::runtime_error(std::string("Failed to ") + - ((enable_sbas_) ? "enable" : "disable") + - " SBAS."); + if (supportsGnss("SBAS")) + { + if (!gps.configSbas(enable_sbas_, sbas_usage_, max_sbas_)) + { + throw std::runtime_error(std::string("Failed to ") + ((enable_sbas_) ? "enable" : "disable") + " SBAS."); } } if (!gps.setPpp(enable_ppp_, protocol_version_)) - throw std::runtime_error(std::string("Failed to ") + - ((enable_ppp_) ? "enable" : "disable") - + " PPP."); + throw std::runtime_error(std::string("Failed to ") + ((enable_ppp_) ? "enable" : "disable") + " PPP."); if (!gps.setDynamicModel(dmodel_)) throw std::runtime_error("Failed to set model: " + dynamic_model_ + "."); if (!gps.setFixMode(fmode_)) throw std::runtime_error("Failed to set fix mode: " + fix_mode_ + "."); - if (!gps.setDeadReckonLimit(dr_limit_)) { + if (!gps.setDeadReckonLimit(dr_limit_)) + { std::stringstream ss; ss << "Failed to set dead reckoning limit: " << dr_limit_ << "."; throw std::runtime_error(ss.str()); @@ -478,25 +508,29 @@ bool UbloxNode::configureUblox() { if (set_dat_ && !gps.configure(cfg_dat_)) throw std::runtime_error("Failed to set user-defined datum."); // Configure each component - for (int i = 0; i < components_.size(); i++) { - if(!components_[i]->configureUblox()) + for (int i = 0; i < components_.size(); i++) + { + if (!components_[i]->configureUblox()) return false; } } - if (save_.saveMask != 0) { - ROS_DEBUG("Saving the u-blox configuration, mask %u, device %u", - save_.saveMask, save_.deviceMask); - if(!gps.configure(save_)) + if (save_.saveMask != 0) + { + ROS_DEBUG("Saving the u-blox configuration, mask %u, device %u", save_.saveMask, save_.deviceMask); + if (!gps.configure(save_)) ROS_ERROR("u-blox unable to save configuration to non-volatile memory"); } - } catch (std::exception& e) { + } + catch (std::exception& e) + { ROS_FATAL("Error configuring u-blox: %s", e.what()); return false; } return true; } -void UbloxNode::configureInf() { +void UbloxNode::configureInf() +{ ublox_msgs::CfgINF msg; // Subscribe to UBX INF messages ublox_msgs::CfgINF_Block block; @@ -504,8 +538,7 @@ void UbloxNode::configureInf() { // Enable desired INF messages on each UBX port uint8_t mask = (enabled["inf_error"] ? block.INF_MSG_ERROR : 0) | (enabled["inf_warning"] ? block.INF_MSG_WARNING : 0) | - (enabled["inf_notice"] ? block.INF_MSG_NOTICE : 0) | - (enabled["inf_test"] ? block.INF_MSG_TEST : 0) | + (enabled["inf_notice"] ? block.INF_MSG_NOTICE : 0) | (enabled["inf_test"] ? block.INF_MSG_TEST : 0) | (enabled["inf_debug"] ? block.INF_MSG_DEBUG : 0); for (int i = 0; i < block.infMsgMask.size(); i++) block.infMsgMask[i] = mask; @@ -513,7 +546,8 @@ void UbloxNode::configureInf() { msg.blocks.push_back(block); // IF NMEA is enabled - if (uart_in_ & ublox_msgs::CfgPRT::PROTO_NMEA) { + if (uart_in_ & ublox_msgs::CfgPRT::PROTO_NMEA) + { ublox_msgs::CfgINF_Block block; block.protocolID = block.PROTOCOL_ID_NMEA; // Enable desired INF messages on each NMEA port @@ -527,48 +561,56 @@ void UbloxNode::configureInf() { ROS_WARN("Failed to configure INF messages"); } -void UbloxNode::initializeIo() { +void UbloxNode::initializeIo() +{ gps.setConfigOnStartup(config_on_startup_flag_); boost::smatch match; - if (boost::regex_match(device_, match, - boost::regex("(tcp|udp)://(.+):(\\d+)"))) { + if (boost::regex_match(device_, match, boost::regex("(tcp|udp)://(.+):(\\d+)"))) + { std::string proto(match[1]); - if (proto == "tcp") { + if (proto == "tcp") + { std::string host(match[2]); std::string port(match[3]); - ROS_INFO("Connecting to %s://%s:%s ...", proto.c_str(), host.c_str(), - port.c_str()); + ROS_INFO("Connecting to %s://%s:%s ...", proto.c_str(), host.c_str(), port.c_str()); gps.initializeTcp(host, port); - } else if (proto == "udp") { + } + else if (proto == "udp") + { std::string host(match[2]); std::string port(match[3]); - ROS_INFO("Connecting to %s://%s:%s ...", proto.c_str(), host.c_str(), - port.c_str()); + ROS_INFO("Connecting to %s://%s:%s ...", proto.c_str(), host.c_str(), port.c_str()); gps.initializeUdp(host, port); - } else { + } + else + { throw std::runtime_error("Protocol '" + proto + "' is unsupported"); } - } else { + } + else + { gps.initializeSerial(device_, baudrate_, uart_in_, uart_out_); } // raw data stream logging - if (rawDataStreamPa_.isEnabled()) { - gps.setRawDataCallback( - boost::bind(&RawDataStreamPa::ubloxCallback,&rawDataStreamPa_, _1, _2)); + if (rawDataStreamPa_.isEnabled()) + { + gps.setRawDataCallback(boost::bind(&RawDataStreamPa::ubloxCallback, &rawDataStreamPa_, _1, _2)); rawDataStreamPa_.initialize(); } } -void UbloxNode::initialize() { +void UbloxNode::initialize() +{ // Params must be set before initializing IO getRosParams(); initializeIo(); // Must process Mon VER before setting firmware/hardware params processMonVer(); - if(protocol_version_ <= 14) { - if(nh->param("raw_data", false)) + if (protocol_version_ <= 14) + { + if (nh->param("raw_data", false)) components_.push_back(ComponentPtr(new RawDataProduct)); } // Must set firmware & hardware params before initializing diagnostics @@ -577,7 +619,8 @@ void UbloxNode::initialize() { // Do this last initializeRosDiagnostics(); - if (configureUblox()) { + if (configureUblox()) + { ROS_INFO("U-Blox configured successfully."); // Subscribe to all U-Blox messages subscribe(); @@ -585,25 +628,24 @@ void UbloxNode::initialize() { configureInf(); ros::Timer keep_alive; - if (device_.substr(0, 6) == "udp://") { + if (device_.substr(0, 6) == "udp://") + { // Setup timer to poll version message to keep UDP socket active - keep_alive = nh->createTimer(ros::Duration(kKeepAlivePeriod), - &UbloxNode::keepAlive, - this); + keep_alive = nh->createTimer(ros::Duration(kKeepAlivePeriod), &UbloxNode::keepAlive, this); } ros::Timer poller; - poller = nh->createTimer(ros::Duration(kPollDuration), - &UbloxNode::pollMessages, - this); + poller = nh->createTimer(ros::Duration(kPollDuration), &UbloxNode::pollMessages, this); poller.start(); ros::spin(); } shutdown(); } -void UbloxNode::shutdown() { - if (gps.isInitialized()) { +void UbloxNode::shutdown() +{ + if (gps.isInitialized()) + { gps.close(); ROS_INFO("Closed connection to %s.", device_.c_str()); } @@ -612,7 +654,8 @@ void UbloxNode::shutdown() { // // U-Blox Firmware (all versions) // -void UbloxFirmware::initializeRosDiagnostics() { +void UbloxFirmware::initializeRosDiagnostics() +{ updater->add("fix", this, &UbloxFirmware::fixDiagnostic); updater->force_update(); } @@ -620,28 +663,32 @@ void UbloxFirmware::initializeRosDiagnostics() { // // U-Blox Firmware Version 6 // -UbloxFirmware6::UbloxFirmware6() {} +UbloxFirmware6::UbloxFirmware6() +{ +} -void UbloxFirmware6::getRosParams() { +void UbloxFirmware6::getRosParams() +{ // Fix Service type, used when publishing fix status messages fix_status_service = sensor_msgs::NavSatStatus::SERVICE_GPS; nh->param("nmea/set", set_nmea_, false); - if (set_nmea_) { + if (set_nmea_) + { bool compat, consider; if (!getRosUint("nmea/version", cfg_nmea_.version)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/version must be set"); + "true, therefore nmea/version must be set"); if (!getRosUint("nmea/num_sv", cfg_nmea_.numSV)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/num_sv must be set"); + "true, therefore nmea/num_sv must be set"); if (!nh->getParam("nmea/compat", compat)) - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/compat must be set"); + throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + + "true, therefore nmea/compat must be set"); if (!nh->getParam("nmea/consider", consider)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/consider must be set"); + "true, therefore nmea/consider must be set"); // set flags cfg_nmea_.flags = compat ? cfg_nmea_.FLAGS_COMPAT : 0; @@ -664,7 +711,8 @@ void UbloxFirmware6::getRosParams() { } } -bool UbloxFirmware6::configureUblox() { +bool UbloxFirmware6::configureUblox() +{ ROS_WARN("ublox_version < 7, ignoring GNSS settings"); if (set_nmea_ && !gps.configure(cfg_nmea_)) @@ -673,7 +721,8 @@ bool UbloxFirmware6::configureUblox() { return true; } -void UbloxFirmware6::subscribe() { +void UbloxFirmware6::subscribe() +{ // Whether or not to publish Nav POS LLH (always subscribes) nh->param("publish/nav/posllh", enabled["nav_posllh"], enabled["nav"]); nh->param("publish/nav/sol", enabled["nav_sol"], enabled["nav"]); @@ -681,56 +730,61 @@ void UbloxFirmware6::subscribe() { // Always subscribes to these messages, but may not publish to ROS topic // Subscribe to Nav POSLLH - gps.subscribe(boost::bind( - &UbloxFirmware6::callbackNavPosLlh, this, _1), kSubscribeRate); + gps.subscribe(boost::bind(&UbloxFirmware6::callbackNavPosLlh, this, _1), kSubscribeRate); // Subscribe to Nav SOL - gps.subscribe(boost::bind( - &UbloxFirmware6::callbackNavSol, this, _1), kSubscribeRate); + gps.subscribe(boost::bind(&UbloxFirmware6::callbackNavSol, this, _1), kSubscribeRate); // Subscribe to Nav VELNED - gps.subscribe(boost::bind( - &UbloxFirmware6::callbackNavVelNed, this, _1), kSubscribeRate); + gps.subscribe(boost::bind(&UbloxFirmware6::callbackNavVelNed, this, _1), kSubscribeRate); // Subscribe to Nav SVINFO nh->param("publish/nav/svinfo", enabled["nav_svinfo"], enabled["nav"]); if (enabled["nav_svinfo"]) - gps.subscribe(boost::bind( - publish, _1, "navsvinfo"), - kNavSvInfoSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "navsvinfo"), + kNavSvInfoSubscribeRate); // Subscribe to Mon HW nh->param("publish/mon_hw", enabled["mon_hw"], enabled["mon"]); if (enabled["mon_hw"]) - gps.subscribe(boost::bind( - publish, _1, "monhw"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "monhw"), kSubscribeRate); } -void UbloxFirmware6::fixDiagnostic( - diagnostic_updater::DiagnosticStatusWrapper& stat) { +void UbloxFirmware6::fixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) +{ // Set the diagnostic level based on the fix status - if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_DEAD_RECKONING_ONLY) { + if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_DEAD_RECKONING_ONLY) + { stat.level = diagnostic_msgs::DiagnosticStatus::WARN; stat.message = "Dead reckoning only"; - } else if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_2D_FIX) { + } + else if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_2D_FIX) + { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "2D fix"; - } else if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_3D_FIX) { + } + else if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_3D_FIX) + { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "3D fix"; - } else if (last_nav_sol_.gpsFix == - ublox_msgs::NavSOL::GPS_GPS_DEAD_RECKONING_COMBINED) { + } + else if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_GPS_DEAD_RECKONING_COMBINED) + { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "GPS and dead reckoning combined"; - } else if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_TIME_ONLY_FIX) { + } + else if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_TIME_ONLY_FIX) + { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "Time fix only"; } // If fix is not ok (within DOP & Accuracy Masks), raise the diagnostic level - if (!(last_nav_sol_.flags & ublox_msgs::NavSOL::FLAGS_GPS_FIX_OK)) { + if (!(last_nav_sol_.flags & ublox_msgs::NavSOL::FLAGS_GPS_FIX_OK)) + { stat.level = diagnostic_msgs::DiagnosticStatus::WARN; stat.message += ", fix not ok"; } // Raise diagnostic level to error if no fix - if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_NO_FIX) { + if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_NO_FIX) + { stat.level = diagnostic_msgs::DiagnosticStatus::ERROR; stat.message = "No fix"; } @@ -746,20 +800,20 @@ void UbloxFirmware6::fixDiagnostic( stat.add("# SVs used", (int)last_nav_sol_.numSV); } -void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m) { - if(enabled["nav_posllh"]) { - static ros::Publisher publisher = - nh->advertise("navposllh", kROSQueueSize); +void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m) +{ + if (enabled["nav_posllh"]) + { + static ros::Publisher publisher = nh->advertise("navposllh", kROSQueueSize); publisher.publish(m); } // Position message - static ros::Publisher fixPublisher = - nh->advertise("fix", kROSQueueSize); + static ros::Publisher fixPublisher = nh->advertise("fix", kROSQueueSize); if (m.iTOW == last_nav_vel_.iTOW) - fix_.header.stamp = velocity_.header.stamp; // use last timestamp + fix_.header.stamp = velocity_.header.stamp; // use last timestamp else - fix_.header.stamp = ros::Time::now(); // new timestamp + fix_.header.stamp = ros::Time::now(); // new timestamp fix_.header.frame_id = frame_id; fix_.latitude = m.lat * 1e-7; @@ -778,8 +832,7 @@ void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m) { fix_.position_covariance[0] = varH; fix_.position_covariance[4] = varH; fix_.position_covariance[8] = varV; - fix_.position_covariance_type = - sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; + fix_.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; fix_.status.service = fix_.status.SERVICE_GPS; fixPublisher.publish(fix_); @@ -789,21 +842,21 @@ void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m) { updater->update(); } -void UbloxFirmware6::callbackNavVelNed(const ublox_msgs::NavVELNED& m) { - if(enabled["nav_velned"]) { - static ros::Publisher publisher = - nh->advertise("navvelned", kROSQueueSize); +void UbloxFirmware6::callbackNavVelNed(const ublox_msgs::NavVELNED& m) +{ + if (enabled["nav_velned"]) + { + static ros::Publisher publisher = nh->advertise("navvelned", kROSQueueSize); publisher.publish(m); } // Example geometry message - static ros::Publisher velocityPublisher = - nh->advertise("fix_velocity", - kROSQueueSize); + static ros::Publisher velocityPublisher = nh->advertise("fix_velocity", + kROSQueueSize); if (m.iTOW == last_nav_pos_.iTOW) - velocity_.header.stamp = fix_.header.stamp; // same time as last navposllh + velocity_.header.stamp = fix_.header.stamp; // same time as last navposllh else - velocity_.header.stamp = ros::Time::now(); // create a new timestamp + velocity_.header.stamp = ros::Time::now(); // create a new timestamp velocity_.header.frame_id = frame_id; // convert to XYZ linear velocity @@ -823,10 +876,11 @@ void UbloxFirmware6::callbackNavVelNed(const ublox_msgs::NavVELNED& m) { last_nav_vel_ = m; } -void UbloxFirmware6::callbackNavSol(const ublox_msgs::NavSOL& m) { - if(enabled["nav_sol"]) { - static ros::Publisher publisher = - nh->advertise("navsol", kROSQueueSize); +void UbloxFirmware6::callbackNavSol(const ublox_msgs::NavSOL& m) +{ + if (enabled["nav_sol"]) + { + static ros::Publisher publisher = nh->advertise("navsol", kROSQueueSize); publisher.publish(m); } last_nav_sol_ = m; @@ -835,9 +889,12 @@ void UbloxFirmware6::callbackNavSol(const ublox_msgs::NavSOL& m) { // // Ublox Firmware Version 7 // -UbloxFirmware7::UbloxFirmware7() {} +UbloxFirmware7::UbloxFirmware7() +{ +} -void UbloxFirmware7::getRosParams() { +void UbloxFirmware7::getRosParams() +{ // // GNSS configuration // @@ -845,53 +902,52 @@ void UbloxFirmware7::getRosParams() { nh->param("gnss/gps", enable_gps_, true); nh->param("gnss/glonass", enable_glonass_, false); nh->param("gnss/qzss", enable_qzss_, false); - getRosUint("gnss/qzss_sig_cfg", qzss_sig_cfg_, - ublox_msgs::CfgGNSS_Block::SIG_CFG_QZSS_L1CA); + getRosUint("gnss/qzss_sig_cfg", qzss_sig_cfg_, ublox_msgs::CfgGNSS_Block::SIG_CFG_QZSS_L1CA); nh->param("gnss/sbas", enable_sbas_, false); - if(enable_gps_ && !supportsGnss("GPS")) + if (enable_gps_ && !supportsGnss("GPS")) ROS_WARN("gnss/gps is true, but GPS GNSS is not supported by this device"); - if(enable_glonass_ && !supportsGnss("GLO")) - ROS_WARN("gnss/glonass is true, but GLONASS is not %s", - "supported by this device"); - if(enable_qzss_ && !supportsGnss("QZSS")) + if (enable_glonass_ && !supportsGnss("GLO")) + ROS_WARN("gnss/glonass is true, but GLONASS is not %s", "supported by this device"); + if (enable_qzss_ && !supportsGnss("QZSS")) ROS_WARN("gnss/qzss is true, but QZSS is not supported by this device"); - if(enable_sbas_ && !supportsGnss("SBAS")) + if (enable_sbas_ && !supportsGnss("SBAS")) ROS_WARN("gnss/sbas is true, but SBAS is not supported by this device"); - if(nh->hasParam("gnss/galileo")) + if (nh->hasParam("gnss/galileo")) ROS_WARN("ublox_version < 8, ignoring Galileo GNSS Settings"); - if(nh->hasParam("gnss/beidou")) + if (nh->hasParam("gnss/beidou")) ROS_WARN("ublox_version < 8, ignoring BeiDou Settings"); - if(nh->hasParam("gnss/imes")) + if (nh->hasParam("gnss/imes")) ROS_WARN("ublox_version < 8, ignoring IMES GNSS Settings"); // Fix Service type, used when publishing fix status messages - fix_status_service = sensor_msgs::NavSatStatus::SERVICE_GPS - + (enable_glonass_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GLONASS; + fix_status_service = sensor_msgs::NavSatStatus::SERVICE_GPS + + (enable_glonass_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GLONASS; // // NMEA Configuration // nh->param("nmea/set", set_nmea_, false); - if (set_nmea_) { + if (set_nmea_) + { bool compat, consider; if (!getRosUint("nmea/version", cfg_nmea_.nmeaVersion)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/version must be set"); + "true, therefore nmea/version must be set"); if (!getRosUint("nmea/num_sv", cfg_nmea_.numSV)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/num_sv must be set"); + "true, therefore nmea/num_sv must be set"); if (!getRosUint("nmea/sv_numbering", cfg_nmea_.svNumbering)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/sv_numbering must be set"); + "true, therefore nmea/sv_numbering must be set"); if (!nh->getParam("nmea/compat", compat)) - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/compat must be set"); + throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + + "true, therefore nmea/compat must be set"); if (!nh->getParam("nmea/consider", consider)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/consider must be set"); + "true, therefore nmea/consider must be set"); // set flags cfg_nmea_.flags = compat ? cfg_nmea_.FLAGS_COMPAT : 0; @@ -925,14 +981,18 @@ void UbloxFirmware7::getRosParams() { } } -bool UbloxFirmware7::configureUblox() { +bool UbloxFirmware7::configureUblox() +{ /** Configure the GNSS **/ ublox_msgs::CfgGNSS cfgGNSSRead; - if (gps.poll(cfgGNSSRead)) { + if (gps.poll(cfgGNSSRead)) + { ROS_DEBUG("Read GNSS config."); ROS_DEBUG("Num. tracking channels in hardware: %i", cfgGNSSRead.numTrkChHw); ROS_DEBUG("Num. tracking channels to use: %i", cfgGNSSRead.numTrkChUse); - } else { + } + else + { throw std::runtime_error("Failed to read the GNSS config."); } @@ -943,21 +1003,22 @@ bool UbloxFirmware7::configureUblox() { cfgGNSSWrite.msgVer = 0; // configure GLONASS - if(supportsGnss("GLO")) { + if (supportsGnss("GLO")) + { ublox_msgs::CfgGNSS_Block block; block.gnssId = block.GNSS_ID_GLONASS; block.resTrkCh = block.RES_TRK_CH_GLONASS; block.maxTrkCh = block.MAX_TRK_CH_GLONASS; block.flags = enable_glonass_ ? block.SIG_CFG_GLONASS_L1OF : 0; cfgGNSSWrite.blocks.push_back(block); - if (!gps.configure(cfgGNSSWrite)) { - throw std::runtime_error(std::string("Failed to ") + - ((enable_glonass_) ? "enable" : "disable") + - " GLONASS."); + if (!gps.configure(cfgGNSSWrite)) + { + throw std::runtime_error(std::string("Failed to ") + ((enable_glonass_) ? "enable" : "disable") + " GLONASS."); } } - if(supportsGnss("QZSS")) { + if (supportsGnss("QZSS")) + { // configure QZSS ublox_msgs::CfgGNSS_Block block; block.gnssId = block.GNSS_ID_QZSS; @@ -965,14 +1026,14 @@ bool UbloxFirmware7::configureUblox() { block.maxTrkCh = block.MAX_TRK_CH_QZSS; block.flags = enable_qzss_ ? qzss_sig_cfg_ : 0; cfgGNSSWrite.blocks[0] = block; - if (!gps.configure(cfgGNSSWrite)) { - throw std::runtime_error(std::string("Failed to ") + - ((enable_glonass_) ? "enable" : "disable") + - " QZSS."); + if (!gps.configure(cfgGNSSWrite)) + { + throw std::runtime_error(std::string("Failed to ") + ((enable_glonass_) ? "enable" : "disable") + " QZSS."); } } - if(supportsGnss("SBAS")) { + if (supportsGnss("SBAS")) + { // configure SBAS ublox_msgs::CfgGNSS_Block block; block.gnssId = block.GNSS_ID_SBAS; @@ -980,48 +1041,47 @@ bool UbloxFirmware7::configureUblox() { block.maxTrkCh = block.MAX_TRK_CH_SBAS; block.flags = enable_sbas_ ? block.SIG_CFG_SBAS_L1CA : 0; cfgGNSSWrite.blocks[0] = block; - if (!gps.configure(cfgGNSSWrite)) { - throw std::runtime_error(std::string("Failed to ") + - ((enable_sbas_) ? "enable" : "disable") + - " SBAS."); + if (!gps.configure(cfgGNSSWrite)) + { + throw std::runtime_error(std::string("Failed to ") + ((enable_sbas_) ? "enable" : "disable") + " SBAS."); } } - if(set_nmea_ && !gps.configure(cfg_nmea_)) + if (set_nmea_ && !gps.configure(cfg_nmea_)) throw std::runtime_error("Failed to configure NMEA"); return true; } -void UbloxFirmware7::subscribe() { +void UbloxFirmware7::subscribe() +{ // Whether to publish Nav PVT messages to a ROS topic nh->param("publish/nav/pvt", enabled["nav_pvt"], enabled["nav"]); // Subscribe to Nav PVT (always does so since fix information is published // from this) - gps.subscribe(boost::bind( - &UbloxFirmware7Plus::callbackNavPvt, this, _1), - kSubscribeRate); + gps.subscribe(boost::bind(&UbloxFirmware7Plus::callbackNavPvt, this, _1), kSubscribeRate); // Subscribe to Nav SVINFO nh->param("publish/nav/svinfo", enabled["nav_svinfo"], enabled["nav"]); if (enabled["nav_svinfo"]) - gps.subscribe(boost::bind( - publish, _1, "navsvinfo"), - kNavSvInfoSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "navsvinfo"), + kNavSvInfoSubscribeRate); // Subscribe to Mon HW nh->param("publish/mon_hw", enabled["mon_hw"], enabled["mon"]); if (enabled["mon_hw"]) - gps.subscribe(boost::bind( - publish, _1, "monhw"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "monhw"), kSubscribeRate); } // // Ublox Version 8 // -UbloxFirmware8::UbloxFirmware8() {} +UbloxFirmware8::UbloxFirmware8() +{ +} -void UbloxFirmware8::getRosParams() { +void UbloxFirmware8::getRosParams() +{ // UPD SOS configuration nh->param("clear_bbr", clear_bbr_, false); gps.setSaveOnShutdown(nh->param("save_on_shutdown", false)); @@ -1035,59 +1095,54 @@ void UbloxFirmware8::getRosParams() { nh->param("gnss/qzss", enable_qzss_, false); nh->param("gnss/sbas", enable_sbas_, false); // QZSS Signal Configuration - getRosUint("gnss/qzss_sig_cfg", qzss_sig_cfg_, - ublox_msgs::CfgGNSS_Block::SIG_CFG_QZSS_L1CA); + getRosUint("gnss/qzss_sig_cfg", qzss_sig_cfg_, ublox_msgs::CfgGNSS_Block::SIG_CFG_QZSS_L1CA); if (enable_gps_ && !supportsGnss("GPS")) - ROS_WARN("gnss/gps is true, but GPS GNSS is not supported by %s", - "this device"); + ROS_WARN("gnss/gps is true, but GPS GNSS is not supported by %s", "this device"); if (enable_glonass_ && !supportsGnss("GLO")) - ROS_WARN("gnss/glonass is true, but GLONASS is not supported by %s", - "this device"); + ROS_WARN("gnss/glonass is true, but GLONASS is not supported by %s", "this device"); if (enable_galileo_ && !supportsGnss("GAL")) - ROS_WARN("gnss/galileo is true, but Galileo GNSS is not supported %s", - "by this device"); + ROS_WARN("gnss/galileo is true, but Galileo GNSS is not supported %s", "by this device"); if (enable_beidou_ && !supportsGnss("BDS")) - ROS_WARN("gnss/beidou is true, but Beidou GNSS is not supported %s", - "by this device"); + ROS_WARN("gnss/beidou is true, but Beidou GNSS is not supported %s", "by this device"); if (enable_imes_ && !supportsGnss("IMES")) - ROS_WARN("gnss/imes is true, but IMES GNSS is not supported by %s", - "this device"); + ROS_WARN("gnss/imes is true, but IMES GNSS is not supported by %s", "this device"); if (enable_qzss_ && !supportsGnss("QZSS")) ROS_WARN("gnss/qzss is true, but QZSS is not supported by this device"); if (enable_sbas_ && !supportsGnss("SBAS")) ROS_WARN("gnss/sbas is true, but SBAS is not supported by this device"); // Fix Service type, used when publishing fix status messages - fix_status_service = sensor_msgs::NavSatStatus::SERVICE_GPS - + (enable_glonass_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GLONASS - + (enable_beidou_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_COMPASS - + (enable_galileo_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GALILEO; + fix_status_service = sensor_msgs::NavSatStatus::SERVICE_GPS + + (enable_glonass_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GLONASS + + (enable_beidou_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_COMPASS + + (enable_galileo_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GALILEO; // // NMEA Configuration // nh->param("nmea/set", set_nmea_, false); - if (set_nmea_) { + if (set_nmea_) + { bool compat, consider; - cfg_nmea_.version = cfg_nmea_.VERSION; // message version + cfg_nmea_.version = cfg_nmea_.VERSION; // message version // Verify that parameters are set if (!getRosUint("nmea/version", cfg_nmea_.nmeaVersion)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/version must be set"); + "true, therefore nmea/version must be set"); if (!getRosUint("nmea/num_sv", cfg_nmea_.numSV)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/num_sv must be set"); + "true, therefore nmea/num_sv must be set"); if (!getRosUint("nmea/sv_numbering", cfg_nmea_.svNumbering)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/sv_numbering must be set"); + "true, therefore nmea/sv_numbering must be set"); if (!nh->getParam("nmea/compat", compat)) - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/compat must be set"); + throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + + "true, therefore nmea/compat must be set"); if (!nh->getParam("nmea/consider", consider)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/consider must be set"); + "true, therefore nmea/consider must be set"); // set flags cfg_nmea_.flags = compat ? cfg_nmea_.FLAGS_COMPAT : 0; @@ -1132,11 +1187,12 @@ void UbloxFirmware8::getRosParams() { } } - -bool UbloxFirmware8::configureUblox() { - if(clear_bbr_) { +bool UbloxFirmware8::configureUblox() +{ + if (clear_bbr_) + { // clear flash memory - if(!gps.clearBbr()) + if (!gps.clearBbr()) ROS_ERROR("u-blox failed to clear flash memory"); } // @@ -1144,67 +1200,68 @@ bool UbloxFirmware8::configureUblox() { // // First, get the current GNSS configuration ublox_msgs::CfgGNSS cfg_gnss; - if (gps.poll(cfg_gnss)) { + if (gps.poll(cfg_gnss)) + { ROS_DEBUG("Read GNSS config."); ROS_DEBUG("Num. tracking channels in hardware: %i", cfg_gnss.numTrkChHw); ROS_DEBUG("Num. tracking channels to use: %i", cfg_gnss.numTrkChUse); - } else { + } + else + { throw std::runtime_error("Failed to read the GNSS config."); } // Then, check the configuration for each GNSS. If it is different, change it. bool correct = true; - for (int i = 0; i < cfg_gnss.blocks.size(); i++) { + for (int i = 0; i < cfg_gnss.blocks.size(); i++) + { ublox_msgs::CfgGNSS_Block block = cfg_gnss.blocks[i]; - if (block.gnssId == block.GNSS_ID_GPS - && enable_gps_ != (block.flags & block.FLAGS_ENABLE)) { + if (block.gnssId == block.GNSS_ID_GPS && enable_gps_ != (block.flags & block.FLAGS_ENABLE)) + { correct = false; - cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_gps_; + cfg_gnss.blocks[i].flags = (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_gps_; ROS_DEBUG("GPS Configuration is different"); - } else if (block.gnssId == block.GNSS_ID_SBAS - && enable_sbas_ != (block.flags & block.FLAGS_ENABLE)) { + } + else if (block.gnssId == block.GNSS_ID_SBAS && enable_sbas_ != (block.flags & block.FLAGS_ENABLE)) + { correct = false; - cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_sbas_; + cfg_gnss.blocks[i].flags = (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_sbas_; ROS_DEBUG("SBAS Configuration is different"); - } else if (block.gnssId == block.GNSS_ID_GALILEO - && enable_galileo_ != (block.flags & block.FLAGS_ENABLE)) { + } + else if (block.gnssId == block.GNSS_ID_GALILEO && enable_galileo_ != (block.flags & block.FLAGS_ENABLE)) + { correct = false; - cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_galileo_; + cfg_gnss.blocks[i].flags = (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_galileo_; ROS_DEBUG("Galileo GNSS Configuration is different"); - } else if (block.gnssId == block.GNSS_ID_BEIDOU - && enable_beidou_ != (block.flags & block.FLAGS_ENABLE)) { + } + else if (block.gnssId == block.GNSS_ID_BEIDOU && enable_beidou_ != (block.flags & block.FLAGS_ENABLE)) + { correct = false; - cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_beidou_; + cfg_gnss.blocks[i].flags = (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_beidou_; ROS_DEBUG("BeiDou Configuration is different"); - } else if (block.gnssId == block.GNSS_ID_IMES - && enable_imes_ != (block.flags & block.FLAGS_ENABLE)) { + } + else if (block.gnssId == block.GNSS_ID_IMES && enable_imes_ != (block.flags & block.FLAGS_ENABLE)) + { correct = false; - cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_imes_; - } else if (block.gnssId == block.GNSS_ID_QZSS - && (enable_qzss_ != (block.flags & block.FLAGS_ENABLE) - || (enable_qzss_ - && qzss_sig_cfg_ != (block.flags & block.FLAGS_SIG_CFG_MASK)))) { - ROS_DEBUG("QZSS Configuration is different %u, %u", - block.flags & block.FLAGS_ENABLE, - enable_qzss_); + cfg_gnss.blocks[i].flags = (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_imes_; + } + else if (block.gnssId == block.GNSS_ID_QZSS && + (enable_qzss_ != (block.flags & block.FLAGS_ENABLE) || + (enable_qzss_ && qzss_sig_cfg_ != (block.flags & block.FLAGS_SIG_CFG_MASK)))) + { + ROS_DEBUG("QZSS Configuration is different %u, %u", block.flags & block.FLAGS_ENABLE, enable_qzss_); correct = false; ROS_DEBUG("QZSS Configuration: %u", block.flags); - cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_qzss_; + cfg_gnss.blocks[i].flags = (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_qzss_; ROS_DEBUG("QZSS Configuration: %u", cfg_gnss.blocks[i].flags); if (enable_qzss_) // Only change sig cfg if enabling cfg_gnss.blocks[i].flags |= qzss_sig_cfg_; - } else if (block.gnssId == block.GNSS_ID_GLONASS - && enable_glonass_ != (block.flags & block.FLAGS_ENABLE)) { + } + else if (block.gnssId == block.GNSS_ID_GLONASS && enable_glonass_ != (block.flags & block.FLAGS_ENABLE)) + { correct = false; - cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_glonass_; + cfg_gnss.blocks[i].flags = (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_glonass_; ROS_DEBUG("GLONASS Configuration is different"); } } @@ -1214,8 +1271,7 @@ bool UbloxFirmware8::configureUblox() { if (correct) ROS_DEBUG("U-Blox GNSS configuration is correct. GNSS not re-configured."); else if (!gps.configGnss(cfg_gnss, boost::posix_time::seconds(15))) - throw std::runtime_error(std::string("Failed to cold reset device ") + - "after configuring GNSS"); + throw std::runtime_error(std::string("Failed to cold reset device ") + "after configuring GNSS"); // // NMEA config @@ -1226,377 +1282,408 @@ bool UbloxFirmware8::configureUblox() { return true; } -void UbloxFirmware8::subscribe() { +void UbloxFirmware8::subscribe() +{ // Whether to publish Nav PVT messages nh->param("publish/nav/pvt", enabled["nav_pvt"], enabled["nav"]); // Subscribe to Nav PVT - gps.subscribe( - boost::bind(&UbloxFirmware7Plus::callbackNavPvt, this, _1), kSubscribeRate); + gps.subscribe(boost::bind(&UbloxFirmware7Plus::callbackNavPvt, this, _1), kSubscribeRate); // Subscribe to Nav SAT messages nh->param("publish/nav/sat", enabled["nav_sat"], enabled["nav"]); if (enabled["nav_sat"]) - gps.subscribe(boost::bind( - publish, _1, "navsat"), kNavSvInfoSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "navsat"), kNavSvInfoSubscribeRate); // Subscribe to Mon HW nh->param("publish/mon/hw", enabled["mon_hw"], enabled["mon"]); if (enabled["mon_hw"]) - gps.subscribe(boost::bind( - publish, _1, "monhw"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "monhw"), kSubscribeRate); // Subscribe to RTCM messages nh->param("publish/rxm/rtcm", enabled["rxm_rtcm"], enabled["rxm"]); if (enabled["rxm_rtcm"]) - gps.subscribe(boost::bind( - publish, _1, "rxmrtcm"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "rxmrtcm"), kSubscribeRate); } // // Raw Data Products // -void RawDataProduct::subscribe() { +void RawDataProduct::subscribe() +{ // Defaults to true instead of to all nh->param("publish/rxm/all", enabled["rxm"], true); // Subscribe to RXM Raw nh->param("publish/rxm/raw", enabled["rxm_raw"], enabled["rxm"]); if (enabled["rxm_raw"]) - gps.subscribe(boost::bind( - publish, _1, "rxmraw"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "rxmraw"), kSubscribeRate); // Subscribe to RXM SFRB nh->param("publish/rxm/sfrb", enabled["rxm_sfrb"], enabled["rxm"]); if (enabled["rxm_sfrb"]) - gps.subscribe(boost::bind( - publish, _1, "rxmsfrb"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "rxmsfrb"), kSubscribeRate); // Subscribe to RXM EPH nh->param("publish/rxm/eph", enabled["rxm_eph"], enabled["rxm"]); if (enabled["rxm_eph"]) - gps.subscribe(boost::bind( - publish, _1, "rxmeph"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "rxmeph"), kSubscribeRate); // Subscribe to RXM ALM nh->param("publish/rxm/almRaw", enabled["rxm_alm"], enabled["rxm"]); if (enabled["rxm_alm"]) - gps.subscribe(boost::bind( - publish, _1, "rxmalm"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "rxmalm"), kSubscribeRate); } -void RawDataProduct::initializeRosDiagnostics() { +void RawDataProduct::initializeRosDiagnostics() +{ if (enabled["rxm_raw"]) - freq_diagnostics_.push_back(boost::shared_ptr( - new UbloxTopicDiagnostic("rxmraw", kRtcmFreqTol, kRtcmFreqWindow))); + freq_diagnostics_.push_back( + boost::shared_ptr(new UbloxTopicDiagnostic("rxmraw", kRtcmFreqTol, kRtcmFreqWindow))); if (enabled["rxm_sfrb"]) - freq_diagnostics_.push_back(boost::shared_ptr( - new UbloxTopicDiagnostic("rxmsfrb", kRtcmFreqTol, kRtcmFreqWindow))); + freq_diagnostics_.push_back( + boost::shared_ptr(new UbloxTopicDiagnostic("rxmsfrb", kRtcmFreqTol, kRtcmFreqWindow))); if (enabled["rxm_eph"]) - freq_diagnostics_.push_back(boost::shared_ptr( - new UbloxTopicDiagnostic("rxmeph", kRtcmFreqTol, kRtcmFreqWindow))); + freq_diagnostics_.push_back( + boost::shared_ptr(new UbloxTopicDiagnostic("rxmeph", kRtcmFreqTol, kRtcmFreqWindow))); if (enabled["rxm_alm"]) - freq_diagnostics_.push_back(boost::shared_ptr( - new UbloxTopicDiagnostic("rxmalm", kRtcmFreqTol, kRtcmFreqWindow))); + freq_diagnostics_.push_back( + boost::shared_ptr(new UbloxTopicDiagnostic("rxmalm", kRtcmFreqTol, kRtcmFreqWindow))); } -AdrUdrProduct::AdrUdrProduct(float protocol_version) - : protocol_version_(protocol_version) -{} +AdrUdrProduct::AdrUdrProduct(float protocol_version) : protocol_version_(protocol_version) +{ +} // // u-blox ADR devices, partially implemented // -void AdrUdrProduct::getRosParams() { +void AdrUdrProduct::getRosParams() +{ nh->param("use_adr", use_adr_, true); // Check the nav rate float nav_rate_hz = 1000 / (meas_rate * nav_rate); - if(nav_rate_hz != 1) + if (nav_rate_hz != 1) ROS_WARN("Nav Rate recommended to be 1 Hz"); } -bool AdrUdrProduct::configureUblox() { - if(!gps.setUseAdr(use_adr_, protocol_version_)) - throw std::runtime_error(std::string("Failed to ") - + (use_adr_ ? "enable" : "disable") + "use_adr"); +bool AdrUdrProduct::configureUblox() +{ + if (!gps.setUseAdr(use_adr_, protocol_version_)) + throw std::runtime_error(std::string("Failed to ") + (use_adr_ ? "enable" : "disable") + "use_adr"); return true; } -void AdrUdrProduct::subscribe() { +void AdrUdrProduct::subscribe() +{ nh->param("publish/esf/all", enabled["esf"], true); // Subscribe to NAV ATT messages nh->param("publish/nav/att", enabled["nav_att"], enabled["nav"]); if (enabled["nav_att"]) - gps.subscribe(boost::bind( - publish, _1, "navatt"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "navatt"), kSubscribeRate); // Subscribe to ESF INS messages nh->param("publish/esf/ins", enabled["esf_ins"], enabled["esf"]); if (enabled["esf_ins"]) - gps.subscribe(boost::bind( - publish, _1, "esfins"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "esfins"), kSubscribeRate); // Subscribe to ESF Meas messages nh->param("publish/esf/meas", enabled["esf_meas"], enabled["esf"]); if (enabled["esf_meas"]) - gps.subscribe(boost::bind( - publish, _1, "esfmeas"), kSubscribeRate); - // also publish sensor_msgs::Imu - gps.subscribe(boost::bind( - &AdrUdrProduct::callbackEsfMEAS, this, _1), kSubscribeRate); - + gps.subscribe(boost::bind(publish, _1, "esfmeas"), kSubscribeRate); + // also publish sensor_msgs::Imu + gps.subscribe(boost::bind(&AdrUdrProduct::callbackEsfMEAS, this, _1), kSubscribeRate); + // Subscribe to ESF Raw messages nh->param("publish/esf/raw", enabled["esf_raw"], enabled["esf"]); if (enabled["esf_raw"]) - gps.subscribe(boost::bind( - publish, _1, "esfraw"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "esfraw"), kSubscribeRate); // Subscribe to ESF Status messages nh->param("publish/esf/status", enabled["esf_status"], enabled["esf"]); if (enabled["esf_status"]) - gps.subscribe(boost::bind( - publish, _1, "esfstatus"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "esfstatus"), kSubscribeRate); // Subscribe to HNR PVT messages nh->param("publish/hnr/pvt", enabled["hnr_pvt"], true); if (enabled["hnr_pvt"]) - gps.subscribe(boost::bind( - publish, _1, "hnrpvt"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "hnrpvt"), kSubscribeRate); } -void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS &m) { - if (enabled["esf_meas"]) { - static ros::Publisher imu_pub = - nh->advertise("imu_meas", kROSQueueSize); - static ros::Publisher time_ref_pub = - nh->advertise("interrupt_time", kROSQueueSize); - +void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS& m) +{ + if (enabled["esf_meas"]) + { + static ros::Publisher imu_pub = nh->advertise("imu_meas", kROSQueueSize); + static ros::Publisher time_ref_pub = nh->advertise("interrupt_time", kROSQueueSize); + imu_.header.stamp = ros::Time::now(); imu_.header.frame_id = frame_id; - + static const float deg_per_sec = pow(2, -12); static const float m_per_sec_sq = pow(2, -10); static const float deg_c = 1e-2; - + std::vector imu_data = m.data; - for (int i=0; i < imu_data.size(); i++){ - unsigned int data_type = imu_data[i] >> 24; //grab the last six bits of data - int32_t data_value = static_cast(imu_data[i] << 8); //shift to extend sign from 24 to 32 bit integer + for (int i = 0; i < imu_data.size(); i++) + { + unsigned int data_type = imu_data[i] >> 24; // grab the last six bits of data + int32_t data_value = static_cast(imu_data[i] << 8); // shift to extend sign from 24 to 32 bit integer data_value >>= 8; imu_.orientation_covariance[0] = -1; imu_.linear_acceleration_covariance[0] = -1; imu_.angular_velocity_covariance[0] = -1; - if (data_type == 14) { - imu_.angular_velocity.x = data_value * deg_per_sec; - } else if (data_type == 16) { - imu_.linear_acceleration.x = data_value * m_per_sec_sq; - } else if (data_type == 13) { - imu_.angular_velocity.y = data_value * deg_per_sec; - } else if (data_type == 17) { - imu_.linear_acceleration.y = data_value * m_per_sec_sq; - } else if (data_type == 5) { - imu_.angular_velocity.z = data_value * deg_per_sec; - } else if (data_type == 18) { - imu_.linear_acceleration.z = data_value * m_per_sec_sq; - } else if (data_type == 12) { - //ROS_INFO("Temperature in celsius: %f", data_value * deg_c); - } else { + if (data_type == 14) + { + imu_.angular_velocity.x = data_value * deg_per_sec; + } + else if (data_type == 16) + { + imu_.linear_acceleration.x = data_value * m_per_sec_sq; + } + else if (data_type == 13) + { + imu_.angular_velocity.y = data_value * deg_per_sec; + } + else if (data_type == 17) + { + imu_.linear_acceleration.y = data_value * m_per_sec_sq; + } + else if (data_type == 5) + { + imu_.angular_velocity.z = data_value * deg_per_sec; + } + else if (data_type == 18) + { + imu_.linear_acceleration.z = data_value * m_per_sec_sq; + } + else if (data_type == 12) + { + // ROS_INFO("Temperature in celsius: %f", data_value * deg_c); + } + else + { ROS_INFO("data_type: %u", data_type); ROS_INFO("data_value: %u", data_value); } // create time ref message and put in the data - //t_ref_.header.seq = m.risingEdgeCount; - //t_ref_.header.stamp = ros::Time::now(); - //t_ref_.header.frame_id = frame_id; + // t_ref_.header.seq = m.risingEdgeCount; + // t_ref_.header.stamp = ros::Time::now(); + // t_ref_.header.frame_id = frame_id; + + // t_ref_.time_ref = ros::Time((m.wnR * 604800 + m.towMsR / 1000), (m.towMsR % 1000) * 1000000 + m.towSubMsR); - //t_ref_.time_ref = ros::Time((m.wnR * 604800 + m.towMsR / 1000), (m.towMsR % 1000) * 1000000 + m.towSubMsR); - - //std::ostringstream src; - //src << "TIM" << int(m.ch); - //t_ref_.source = src.str(); + // std::ostringstream src; + // src << "TIM" << int(m.ch); + // t_ref_.source = src.str(); - t_ref_.header.stamp = ros::Time::now(); // create a new timestamp + t_ref_.header.stamp = ros::Time::now(); // create a new timestamp t_ref_.header.frame_id = frame_id; - + time_ref_pub.publish(t_ref_); imu_pub.publish(imu_); } } - + updater->force_update(); } // // u-blox High Precision GNSS Reference Station // -void HpgRefProduct::getRosParams() { - if (config_on_startup_flag_) { - if(nav_rate * meas_rate != 1000) +void HpgRefProduct::getRosParams() +{ + if (config_on_startup_flag_) + { + if (nav_rate * meas_rate != 1000) ROS_WARN("For HPG Ref devices, nav_rate should be exactly 1 Hz."); - if(!getRosUint("tmode3", tmode3_)) + if (!getRosUint("tmode3", tmode3_)) throw std::runtime_error("Invalid settings: TMODE3 must be set"); - if(tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_FIXED) { - if(!nh->getParam("arp/position", arp_position_)) - throw std::runtime_error(std::string("Invalid settings: arp/position ") - + "must be set if TMODE3 is fixed"); - if(!getRosInt("arp/position_hp", arp_position_hp_)) - throw std::runtime_error(std::string("Invalid settings: arp/position_hp ") - + "must be set if TMODE3 is fixed"); - if(!nh->getParam("arp/acc", fixed_pos_acc_)) - throw std::runtime_error(std::string("Invalid settings: arp/acc ") - + "must be set if TMODE3 is fixed"); - if(!nh->getParam("arp/lla_flag", lla_flag_)) { - ROS_WARN("arp/lla_flag param not set, assuming ARP coordinates are %s", - "in ECEF"); + if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_FIXED) + { + if (!nh->getParam("arp/position", arp_position_)) + throw std::runtime_error(std::string("Invalid settings: arp/position ") + "must be set if TMODE3 is fixed"); + if (!getRosInt("arp/position_hp", arp_position_hp_)) + throw std::runtime_error(std::string("Invalid settings: arp/position_hp ") + "must be set if TMODE3 is fixed"); + if (!nh->getParam("arp/acc", fixed_pos_acc_)) + throw std::runtime_error(std::string("Invalid settings: arp/acc ") + "must be set if TMODE3 is fixed"); + if (!nh->getParam("arp/lla_flag", lla_flag_)) + { + ROS_WARN("arp/lla_flag param not set, assuming ARP coordinates are %s", "in ECEF"); lla_flag_ = false; } - } else if(tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_SURVEY_IN) { + } + else if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_SURVEY_IN) + { nh->param("sv_in/reset", svin_reset_, true); - if(!getRosUint("sv_in/min_dur", sv_in_min_dur_)) - throw std::runtime_error(std::string("Invalid settings: sv_in/min_dur ") - + "must be set if TMODE3 is survey-in"); - if(!nh->getParam("sv_in/acc_lim", sv_in_acc_lim_)) - throw std::runtime_error(std::string("Invalid settings: sv_in/acc_lim ") - + "must be set if TMODE3 is survey-in"); - } else if(tmode3_ != ublox_msgs::CfgTMODE3::FLAGS_MODE_DISABLED) { - throw std::runtime_error(std::string("tmode3 param invalid. See CfgTMODE3") - + " flag constants for possible values."); + if (!getRosUint("sv_in/min_dur", sv_in_min_dur_)) + throw std::runtime_error(std::string("Invalid settings: sv_in/min_dur ") + + "must be set if TMODE3 is survey-in"); + if (!nh->getParam("sv_in/acc_lim", sv_in_acc_lim_)) + throw std::runtime_error(std::string("Invalid settings: sv_in/acc_lim ") + + "must be set if TMODE3 is survey-in"); + } + else if (tmode3_ != ublox_msgs::CfgTMODE3::FLAGS_MODE_DISABLED) + { + throw std::runtime_error(std::string("tmode3 param invalid. See CfgTMODE3") + + " flag constants for possible values."); } } } -bool HpgRefProduct::configureUblox() { +bool HpgRefProduct::configureUblox() +{ // Configure TMODE3 - if(tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_DISABLED) { - if(!gps.disableTmode3()) + if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_DISABLED) + { + if (!gps.disableTmode3()) throw std::runtime_error("Failed to disable TMODE3."); mode_ = DISABLED; - } else if(tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_FIXED) { - if(!gps.configTmode3Fixed(lla_flag_, arp_position_, arp_position_hp_, - fixed_pos_acc_)) + } + else if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_FIXED) + { + if (!gps.configTmode3Fixed(lla_flag_, arp_position_, arp_position_hp_, fixed_pos_acc_)) throw std::runtime_error("Failed to set TMODE3 to fixed."); - if(!gps.configRtcm(rtcm_ids, rtcm_rates)) + if (!gps.configRtcm(rtcm_ids, rtcm_rates)) throw std::runtime_error("Failed to set RTCM rates"); mode_ = FIXED; - } else if(tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_SURVEY_IN) { - if(!svin_reset_) { + } + else if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_SURVEY_IN) + { + if (!svin_reset_) + { ublox_msgs::NavSVIN nav_svin; - if(!gps.poll(nav_svin)) - throw std::runtime_error(std::string("Failed to poll NavSVIN while") + - " configuring survey-in"); + if (!gps.poll(nav_svin)) + throw std::runtime_error(std::string("Failed to poll NavSVIN while") + " configuring survey-in"); // Don't reset survey-in if it's already active - if(nav_svin.active) { + if (nav_svin.active) + { mode_ = SURVEY_IN; return true; } // Don't reset survey-in if it already has a valid value - if(nav_svin.valid) { + if (nav_svin.valid) + { setTimeMode(); return true; } ublox_msgs::NavPVT nav_pvt; - if(!gps.poll(nav_pvt)) - throw std::runtime_error(std::string("Failed to poll NavPVT while") + - " configuring survey-in"); + if (!gps.poll(nav_pvt)) + throw std::runtime_error(std::string("Failed to poll NavPVT while") + " configuring survey-in"); // Don't reset survey in if in time mode with a good fix - if (nav_pvt.fixType == nav_pvt.FIX_TYPE_TIME_ONLY - && nav_pvt.flags & nav_pvt.FLAGS_GNSS_FIX_OK) { + if (nav_pvt.fixType == nav_pvt.FIX_TYPE_TIME_ONLY && nav_pvt.flags & nav_pvt.FLAGS_GNSS_FIX_OK) + { setTimeMode(); return true; } } // Reset the Survey In // For Survey in, meas rate must be at least 1 Hz - uint16_t meas_rate_temp = meas_rate < 1000 ? meas_rate : 1000; // [ms] + uint16_t meas_rate_temp = meas_rate < 1000 ? meas_rate : 1000; // [ms] // If measurement period isn't a factor of 1000, set to default - if(1000 % meas_rate_temp != 0) + if (1000 % meas_rate_temp != 0) meas_rate_temp = kDefaultMeasPeriod; // Set nav rate to 1 Hz during survey in - if(!gps.configRate(meas_rate_temp, (int) 1000 / meas_rate_temp)) - throw std::runtime_error(std::string("Failed to set nav rate to 1 Hz") + - "before setting TMODE3 to survey-in."); + if (!gps.configRate(meas_rate_temp, (int)1000 / meas_rate_temp)) + throw std::runtime_error(std::string("Failed to set nav rate to 1 Hz") + "before setting TMODE3 to survey-in."); // As recommended in the documentation, first disable, then set to survey in - if(!gps.disableTmode3()) + if (!gps.disableTmode3()) ROS_ERROR("Failed to disable TMODE3 before setting to survey-in."); else mode_ = DISABLED; // Set to Survey in mode - if(!gps.configTmode3SurveyIn(sv_in_min_dur_, sv_in_acc_lim_)) + if (!gps.configTmode3SurveyIn(sv_in_min_dur_, sv_in_acc_lim_)) throw std::runtime_error("Failed to set TMODE3 to survey-in."); mode_ = SURVEY_IN; } return true; } -void HpgRefProduct::subscribe() { +void HpgRefProduct::subscribe() +{ // Whether to publish Nav Survey-In messages nh->param("publish/nav/svin", enabled["nav_svin"], enabled["nav"]); // Subscribe to Nav Survey-In - gps.subscribe(boost::bind( - &HpgRefProduct::callbackNavSvIn, this, _1), kSubscribeRate); + gps.subscribe(boost::bind(&HpgRefProduct::callbackNavSvIn, this, _1), kSubscribeRate); } -void HpgRefProduct::callbackNavSvIn(ublox_msgs::NavSVIN m) { - if(enabled["nav_svin"]) { - static ros::Publisher publisher = - nh->advertise("navsvin", kROSQueueSize); +void HpgRefProduct::callbackNavSvIn(ublox_msgs::NavSVIN m) +{ + if (enabled["nav_svin"]) + { + static ros::Publisher publisher = nh->advertise("navsvin", kROSQueueSize); publisher.publish(m); } last_nav_svin_ = m; - if(!m.active && m.valid && mode_ == SURVEY_IN) { + if (!m.active && m.valid && mode_ == SURVEY_IN) + { setTimeMode(); } updater->update(); } -bool HpgRefProduct::setTimeMode() { +bool HpgRefProduct::setTimeMode() +{ ROS_INFO("Setting mode (internal state) to Time Mode"); mode_ = TIME; // Set the Measurement & nav rate to user config // (survey-in sets nav_rate to 1 Hz regardless of user setting) - if(!gps.configRate(meas_rate, nav_rate)) - ROS_ERROR("Failed to set measurement rate to %d ms %s %d", meas_rate, - "navigation rate to ", nav_rate); + if (!gps.configRate(meas_rate, nav_rate)) + ROS_ERROR("Failed to set measurement rate to %d ms %s %d", meas_rate, "navigation rate to ", nav_rate); // Enable the RTCM out messages - if(!gps.configRtcm(rtcm_ids, rtcm_rates)) { + if (!gps.configRtcm(rtcm_ids, rtcm_rates)) + { ROS_ERROR("Failed to configure RTCM IDs"); return false; } return true; } -void HpgRefProduct::initializeRosDiagnostics() { +void HpgRefProduct::initializeRosDiagnostics() +{ updater->add("TMODE3", this, &HpgRefProduct::tmode3Diagnostics); updater->force_update(); } -void HpgRefProduct::tmode3Diagnostics( - diagnostic_updater::DiagnosticStatusWrapper& stat) { - if (mode_ == INIT) { +void HpgRefProduct::tmode3Diagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat) +{ + if (mode_ == INIT) + { stat.level = diagnostic_msgs::DiagnosticStatus::WARN; stat.message = "Not configured"; - } else if (mode_ == DISABLED){ + } + else if (mode_ == DISABLED) + { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "Disabled"; - } else if (mode_ == SURVEY_IN) { - if (!last_nav_svin_.active && !last_nav_svin_.valid) { + } + else if (mode_ == SURVEY_IN) + { + if (!last_nav_svin_.active && !last_nav_svin_.valid) + { stat.level = diagnostic_msgs::DiagnosticStatus::ERROR; stat.message = "Survey-In inactive and invalid"; - } else if (last_nav_svin_.active && !last_nav_svin_.valid) { + } + else if (last_nav_svin_.active && !last_nav_svin_.valid) + { stat.level = diagnostic_msgs::DiagnosticStatus::WARN; stat.message = "Survey-In active but invalid"; - } else if (!last_nav_svin_.active && last_nav_svin_.valid) { + } + else if (!last_nav_svin_.active && last_nav_svin_.valid) + { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "Survey-In complete"; - } else if (last_nav_svin_.active && last_nav_svin_.valid) { + } + else if (last_nav_svin_.active && last_nav_svin_.valid) + { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "Survey-In active and valid"; } @@ -1611,10 +1698,14 @@ void HpgRefProduct::tmode3Diagnostics( stat.add("Mean Y HP [m]", last_nav_svin_.meanYHP * 1e-4); stat.add("Mean Z HP [m]", last_nav_svin_.meanZHP * 1e-4); stat.add("Mean Accuracy [m]", last_nav_svin_.meanAcc * 1e-4); - } else if(mode_ == FIXED) { + } + else if (mode_ == FIXED) + { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "Fixed Position"; - } else if(mode_ == TIME) { + } + else if (mode_ == TIME) + { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "Time"; } @@ -1623,61 +1714,62 @@ void HpgRefProduct::tmode3Diagnostics( // // U-Blox High Precision GNSS Rover // -void HpgRovProduct::getRosParams() { +void HpgRovProduct::getRosParams() +{ // default to float, see CfgDGNSS message for details - getRosUint("dgnss_mode", dgnss_mode_, - ublox_msgs::CfgDGNSS::DGNSS_MODE_RTK_FIXED); + getRosUint("dgnss_mode", dgnss_mode_, ublox_msgs::CfgDGNSS::DGNSS_MODE_RTK_FIXED); } -bool HpgRovProduct::configureUblox() { +bool HpgRovProduct::configureUblox() +{ // Configure the DGNSS - if(!gps.setDgnss(dgnss_mode_)) + if (!gps.setDgnss(dgnss_mode_)) throw std::runtime_error(std::string("Failed to Configure DGNSS")); return true; } -void HpgRovProduct::subscribe() { +void HpgRovProduct::subscribe() +{ // Whether to publish Nav Relative Position NED nh->param("publish/nav/relposned", enabled["nav_relposned"], enabled["nav"]); // Subscribe to Nav Relative Position NED messages (also updates diagnostics) - gps.subscribe(boost::bind( - &HpgRovProduct::callbackNavRelPosNed, this, _1), kSubscribeRate); + gps.subscribe(boost::bind(&HpgRovProduct::callbackNavRelPosNed, this, _1), kSubscribeRate); } -void HpgRovProduct::initializeRosDiagnostics() { - freq_rtcm_ = UbloxTopicDiagnostic(std::string("rxmrtcm"), - kRtcmFreqMin, kRtcmFreqMax, - kRtcmFreqTol, kRtcmFreqWindow); - updater->add("Carrier Phase Solution", this, - &HpgRovProduct::carrierPhaseDiagnostics); +void HpgRovProduct::initializeRosDiagnostics() +{ + freq_rtcm_ = UbloxTopicDiagnostic(std::string("rxmrtcm"), kRtcmFreqMin, kRtcmFreqMax, kRtcmFreqTol, kRtcmFreqWindow); + updater->add("Carrier Phase Solution", this, &HpgRovProduct::carrierPhaseDiagnostics); updater->force_update(); } -void HpgRovProduct::carrierPhaseDiagnostics( - diagnostic_updater::DiagnosticStatusWrapper& stat) { +void HpgRovProduct::carrierPhaseDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat) +{ uint32_t carr_soln = last_rel_pos_.flags & last_rel_pos_.FLAGS_CARR_SOLN_MASK; stat.add("iTOW", last_rel_pos_.iTOW); if (carr_soln & last_rel_pos_.FLAGS_CARR_SOLN_NONE || - !(last_rel_pos_.flags & last_rel_pos_.FLAGS_DIFF_SOLN && - last_rel_pos_.flags & last_rel_pos_.FLAGS_REL_POS_VALID)) { + !(last_rel_pos_.flags & last_rel_pos_.FLAGS_DIFF_SOLN && last_rel_pos_.flags & last_rel_pos_.FLAGS_REL_POS_VALID)) + { stat.level = diagnostic_msgs::DiagnosticStatus::ERROR; stat.message = "None"; - } else { - if (carr_soln & last_rel_pos_.FLAGS_CARR_SOLN_FLOAT) { + } + else + { + if (carr_soln & last_rel_pos_.FLAGS_CARR_SOLN_FLOAT) + { stat.level = diagnostic_msgs::DiagnosticStatus::WARN; stat.message = "Float"; - } else if (carr_soln & last_rel_pos_.FLAGS_CARR_SOLN_FIXED) { + } + else if (carr_soln & last_rel_pos_.FLAGS_CARR_SOLN_FIXED) + { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "Fixed"; } stat.add("Ref Station ID", last_rel_pos_.refStationId); - double rel_pos_n = (last_rel_pos_.relPosN - + (last_rel_pos_.relPosHPN * 1e-2)) * 1e-2; - double rel_pos_e = (last_rel_pos_.relPosE - + (last_rel_pos_.relPosHPE * 1e-2)) * 1e-2; - double rel_pos_d = (last_rel_pos_.relPosD - + (last_rel_pos_.relPosHPD * 1e-2)) * 1e-2; + double rel_pos_n = (last_rel_pos_.relPosN + (last_rel_pos_.relPosHPN * 1e-2)) * 1e-2; + double rel_pos_e = (last_rel_pos_.relPosE + (last_rel_pos_.relPosHPE * 1e-2)) * 1e-2; + double rel_pos_d = (last_rel_pos_.relPosD + (last_rel_pos_.relPosHPD * 1e-2)) * 1e-2; stat.add("Relative Position N [m]", rel_pos_n); stat.add("Relative Accuracy N [m]", last_rel_pos_.accN * 1e-4); stat.add("Relative Position E [m]", rel_pos_e); @@ -1687,10 +1779,11 @@ void HpgRovProduct::carrierPhaseDiagnostics( } } -void HpgRovProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED &m) { - if (enabled["nav_relposned"]) { - static ros::Publisher publisher = - nh->advertise("navrelposned", kROSQueueSize); +void HpgRovProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED& m) +{ + if (enabled["nav_relposned"]) + { + static ros::Publisher publisher = nh->advertise("navrelposned", kROSQueueSize); publisher.publish(m); } @@ -1701,12 +1794,13 @@ void HpgRovProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED &m) { // // U-Blox High Precision Positioning Receiver // -void HpPosRecProduct::subscribe() { +void HpPosRecProduct::subscribe() +{ // Subscribe to Nav High Precision Position ECEF nh->param("publish/nav/hpposecef", enabled["nav_hpposecef"], enabled["nav"]); if (enabled["nav_hpposecef"]) - gps.subscribe(boost::bind( - publish, _1, "navhpposecef"), kSubscribeRate); + gps.subscribe(boost::bind(publish, _1, "navhpposecef"), + kSubscribeRate); // Whether to publish the NavSatFix info from Nav High Precision Position LLH nh->param("publish/nav/hp_fix", enabled["nav_hpfix"], enabled["nav"]); @@ -1716,30 +1810,31 @@ void HpPosRecProduct::subscribe() { // Subscribe to Nav High Precision Position LLH if (enabled["nav_hpposllh"] || enabled["nav_hpfix"]) - gps.subscribe(boost::bind( - &HpPosRecProduct::callbackNavHpPosLlh, this, _1), kSubscribeRate); + gps.subscribe(boost::bind(&HpPosRecProduct::callbackNavHpPosLlh, this, _1), + kSubscribeRate); // Whether to publish Nav Relative Position NED nh->param("publish/nav/relposned", enabled["nav_relposned"], enabled["nav"]); // Subscribe to Nav Relative Position NED messages (also updates diagnostics) - gps.subscribe(boost::bind( - &HpPosRecProduct::callbackNavRelPosNed, this, _1), kSubscribeRate); + gps.subscribe(boost::bind(&HpPosRecProduct::callbackNavRelPosNed, this, _1), + kSubscribeRate); // Whether to publish the Heading info from Nav Relative Position NED nh->param("publish/nav/heading", enabled["nav_heading"], enabled["nav"]); } -void HpPosRecProduct::callbackNavHpPosLlh(const ublox_msgs::NavHPPOSLLH& m) { - if (enabled["nav_hpposllh"]) { - static ros::Publisher publisher = - nh->advertise("navhpposllh", kROSQueueSize); +void HpPosRecProduct::callbackNavHpPosLlh(const ublox_msgs::NavHPPOSLLH& m) +{ + if (enabled["nav_hpposllh"]) + { + static ros::Publisher publisher = nh->advertise("navhpposllh", kROSQueueSize); publisher.publish(m); } - if (enabled["nav_hpfix"]) { + if (enabled["nav_hpfix"]) + { sensor_msgs::NavSatFix fix_msg; - static ros::Publisher fixPublisher = - nh->advertise("hp_fix", kROSQueueSize); + static ros::Publisher fixPublisher = nh->advertise("hp_fix", kROSQueueSize); fix_msg.header.stamp = ros::Time::now(); fix_msg.header.frame_id = frame_id; @@ -1747,9 +1842,12 @@ void HpPosRecProduct::callbackNavHpPosLlh(const ublox_msgs::NavHPPOSLLH& m) { fix_msg.longitude = m.lon * 1e-7 + m.lonHp * 1e-9; fix_msg.altitude = m.height * 1e-3 + m.heightHp * 1e-4; - if (m.invalid_llh) { + if (m.invalid_llh) + { fix_msg.status.status = fix_msg.status.STATUS_NO_FIX; - } else { + } + else + { fix_msg.status.status = fix_msg.status.STATUS_FIX; } @@ -1760,24 +1858,24 @@ void HpPosRecProduct::callbackNavHpPosLlh(const ublox_msgs::NavHPPOSLLH& m) { fix_msg.position_covariance[0] = varH; fix_msg.position_covariance[4] = varH; fix_msg.position_covariance[8] = varV; - fix_msg.position_covariance_type = - sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; + fix_msg.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; fix_msg.status.service = fix_msg.status.SERVICE_GPS; fixPublisher.publish(fix_msg); } } -void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m) { - if (enabled["nav_relposned"]) { - static ros::Publisher publisher = - nh->advertise("navrelposned", kROSQueueSize); +void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9& m) +{ + if (enabled["nav_relposned"]) + { + static ros::Publisher publisher = nh->advertise("navrelposned", kROSQueueSize); publisher.publish(m); } - if (enabled["nav_heading"]) { - static ros::Publisher imu_pub = - nh->advertise("navheading", kROSQueueSize); + if (enabled["nav_heading"]) + { + static ros::Publisher imu_pub = nh->advertise("navheading", kROSQueueSize); imu_.header.stamp = ros::Time::now(); imu_.header.frame_id = frame_id; @@ -1787,7 +1885,7 @@ void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m) { // Transform angle since ublox is representing heading as NED but ROS uses ENU as convention (REP-103). // Also convert the base-to-rover angle to a robot-to-base angle (consistent with frame_id) - double heading = - (static_cast(m.relPosHeading) * 1e-5 / 180.0 * M_PI) - M_PI_2; + double heading = -(static_cast(m.relPosHeading) * 1e-5 / 180.0 * M_PI) - M_PI_2; tf::Quaternion orientation; orientation.setRPY(0, 0, heading); imu_.orientation.x = orientation[0]; @@ -1813,97 +1911,99 @@ void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m) { // // U-Blox Time Sync Products, partially implemented. // -void TimProduct::getRosParams() { +void TimProduct::getRosParams() +{ } -bool TimProduct::configureUblox() { +bool TimProduct::configureUblox() +{ uint8_t r = 1; // Configure the reciever - if(!gps.setUTCtime()) + if (!gps.setUTCtime()) throw std::runtime_error(std::string("Failed to Configure TIM Product to UTC Time")); - - if(!gps.setTimtm2(r)) + + if (!gps.setTimtm2(r)) throw std::runtime_error(std::string("Failed to Configure TIM Product")); return true; } -void TimProduct::subscribe() { +void TimProduct::subscribe() +{ ROS_INFO("TIM is Enabled: %u", enabled["tim"]); ROS_INFO("TIM-TM2 is Enabled: %u", enabled["tim_tm2"]); // Subscribe to TIM-TM2 messages (Time mark messages) nh->param("publish/tim/tm2", enabled["tim_tm2"], enabled["tim"]); - gps.subscribe(boost::bind( - &TimProduct::callbackTimTM2, this, _1), kSubscribeRate); - + gps.subscribe(boost::bind(&TimProduct::callbackTimTM2, this, _1), kSubscribeRate); + ROS_INFO("Subscribed to TIM-TM2 messages on topic tim/tm2"); - + // Subscribe to SFRBX messages nh->param("publish/rxm/sfrb", enabled["rxm_sfrb"], enabled["rxm"]); if (enabled["rxm_sfrb"]) - gps.subscribe(boost::bind( - publish, _1, "rxmsfrb"), kSubscribeRate); - - // Subscribe to RawX messages - nh->param("publish/rxm/raw", enabled["rxm_raw"], enabled["rxm"]); - if (enabled["rxm_raw"]) - gps.subscribe(boost::bind( - publish, _1, "rxmraw"), kSubscribeRate); -} - -void TimProduct::callbackTimTM2(const ublox_msgs::TimTM2 &m) { - - if (enabled["tim_tm2"]) { - static ros::Publisher publisher = - nh->advertise("timtm2", kROSQueueSize); - static ros::Publisher time_ref_pub = - nh->advertise("interrupt_time", kROSQueueSize); - + gps.subscribe(boost::bind(publish, _1, "rxmsfrb"), kSubscribeRate); + + // Subscribe to RawX messages + nh->param("publish/rxm/raw", enabled["rxm_raw"], enabled["rxm"]); + if (enabled["rxm_raw"]) + gps.subscribe(boost::bind(publish, _1, "rxmraw"), kSubscribeRate); +} + +void TimProduct::callbackTimTM2(const ublox_msgs::TimTM2& m) +{ + if (enabled["tim_tm2"]) + { + static ros::Publisher publisher = nh->advertise("timtm2", kROSQueueSize); + static ros::Publisher time_ref_pub = nh->advertise("interrupt_time", kROSQueueSize); + // create time ref message and put in the data t_ref_.header.seq = m.risingEdgeCount; t_ref_.header.stamp = ros::Time::now(); t_ref_.header.frame_id = frame_id; - t_ref_.time_ref = ros::Time((m.wnR * 604800 + m.towMsR / 1000), (m.towMsR % 1000) * 1000000 + m.towSubMsR); - + t_ref_.time_ref = ros::Time((m.wnR * 604800 + m.towMsR / 1000), (m.towMsR % 1000) * 1000000 + m.towSubMsR); + std::ostringstream src; - src << "TIM" << int(m.ch); + src << "TIM" << int(m.ch); t_ref_.source = src.str(); - t_ref_.header.stamp = ros::Time::now(); // create a new timestamp + t_ref_.header.stamp = ros::Time::now(); // create a new timestamp t_ref_.header.frame_id = frame_id; - + publisher.publish(m); time_ref_pub.publish(t_ref_); } - + updater->force_update(); } -void TimProduct::initializeRosDiagnostics() { +void TimProduct::initializeRosDiagnostics() +{ updater->force_update(); } -void rtcmCallback(const rtcm_msgs::Message::ConstPtr &msg) { +void rtcmCallback(const rtcm_msgs::Message::ConstPtr& msg) +{ gps.sendRtcm(msg->message); } -void spartnCallback(const std_msgs::UInt8MultiArray::ConstPtr &msg) { +void spartnCallback(const std_msgs::UInt8MultiArray::ConstPtr& msg) +{ gps.sendSpartn(msg->data); } -int main(int argc, char** argv) { +int main(int argc, char** argv) +{ ros::init(argc, argv, "ublox_gps"); nh.reset(new ros::NodeHandle("~")); ros::Subscriber subRtcm = nh->subscribe("/rtcm", 10, rtcmCallback); ros::Subscriber subSpartn = nh->subscribe("/spartn", 10, spartnCallback); nh->param("debug", ublox_gps::debug, 1); - if(ublox_gps::debug) { - if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, - ros::console::levels::Debug)) - ros::console::notifyLoggerLevelsChanged(); - + if (ublox_gps::debug) + { + if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) + ros::console::notifyLoggerLevelsChanged(); } UbloxNode node; return 0; diff --git a/ublox_gps/src/raw_data_pa.cpp b/ublox_gps/src/raw_data_pa.cpp index 872fca9a..29973def 100644 --- a/ublox_gps/src/raw_data_pa.cpp +++ b/ublox_gps/src/raw_data_pa.cpp @@ -47,146 +47,169 @@ using namespace ublox_node; // ublox_node namespace // -RawDataStreamPa::RawDataStreamPa(bool is_ros_subscriber) : - pnh_(ros::NodeHandle("~")), - flag_publish_(false), - is_ros_subscriber_(is_ros_subscriber) { - +RawDataStreamPa::RawDataStreamPa(bool is_ros_subscriber) + : pnh_(ros::NodeHandle("~")), flag_publish_(false), is_ros_subscriber_(is_ros_subscriber) +{ } -void RawDataStreamPa::getRosParams() { - - if (is_ros_subscriber_) { - pnh_.param("dir", file_dir_, ""); - } else { - pnh_.param("raw_data_stream/dir", file_dir_, ""); - pnh_.param("raw_data_stream/publish", flag_publish_, false); - } +void RawDataStreamPa::getRosParams() +{ + if (is_ros_subscriber_) + { + pnh_.param("dir", file_dir_, ""); + } + else + { + pnh_.param("raw_data_stream/dir", file_dir_, ""); + pnh_.param("raw_data_stream/publish", flag_publish_, false); + } } -bool RawDataStreamPa::isEnabled() { - - if (is_ros_subscriber_) { - return !file_dir_.empty(); - } else { - return flag_publish_ || (!file_dir_.empty()); - } +bool RawDataStreamPa::isEnabled() +{ + if (is_ros_subscriber_) + { + return !file_dir_.empty(); + } + else + { + return flag_publish_ || (!file_dir_.empty()); + } } - -void RawDataStreamPa::initialize() { - - if (is_ros_subscriber_) { - ROS_INFO("Subscribing to raw data stream."); - static ros::Subscriber subscriber = - nh_.subscribe ("raw_data_stream", 100, - &RawDataStreamPa::msgCallback, this); - } else if (flag_publish_) { - ROS_INFO("Publishing raw data stream."); - RawDataStreamPa::publishMsg(std::string()); +void RawDataStreamPa::initialize() +{ + if (is_ros_subscriber_) + { + ROS_INFO("Subscribing to raw data stream."); + static ros::Subscriber subscriber = nh_.subscribe("raw_data_stream", 100, &RawDataStreamPa::msgCallback, this); + } + else if (flag_publish_) + { + ROS_INFO("Publishing raw data stream."); + RawDataStreamPa::publishMsg(std::string()); + } + + if (!file_dir_.empty()) + { + struct stat stat_info; + if (stat(file_dir_.c_str(), &stat_info) != 0) + { + ROS_ERROR("Can't log raw data to file. " + "Directory \"%s\" does not exist.", + file_dir_.c_str()); } - - if (!file_dir_.empty()) { - struct stat stat_info; - if (stat(file_dir_.c_str(), &stat_info ) != 0) { - ROS_ERROR("Can't log raw data to file. " - "Directory \"%s\" does not exist.", file_dir_.c_str()); - - } else if ((stat_info.st_mode & S_IFDIR) != S_IFDIR) { - ROS_ERROR("Can't log raw data to file. " - "\"%s\" exists, but is not a directory.", file_dir_.c_str()); - - } else { - if (file_dir_.back() != '/') { - file_dir_ += '/'; - } - - time_t t = time(NULL); - struct tm time_struct = *localtime(&t); - - std::stringstream filename; - filename.width(4); filename.fill('0'); - filename << time_struct.tm_year + 1900; - filename.width(0); filename << '_'; - filename.width(2); filename.fill('0'); - filename << time_struct.tm_mon + 1; - filename.width(0); filename << '_'; - filename.width(2); filename.fill('0'); - filename << time_struct.tm_mday; - filename.width(0); filename << '_'; - filename.width(2); filename.fill('0'); - filename << time_struct.tm_hour; - filename.width(2); filename.fill('0'); - filename << time_struct.tm_min ; - filename.width(0); filename << ".log"; - file_name_ = file_dir_ + filename.str(); - - try { - file_handle_.open(file_name_); - ROS_INFO("Logging raw data to file \"%s\"", + else if ((stat_info.st_mode & S_IFDIR) != S_IFDIR) + { + ROS_ERROR("Can't log raw data to file. " + "\"%s\" exists, but is not a directory.", + file_dir_.c_str()); + } + else + { + if (file_dir_.back() != '/') + { + file_dir_ += '/'; + } + + time_t t = time(NULL); + struct tm time_struct = *localtime(&t); + + std::stringstream filename; + filename.width(4); + filename.fill('0'); + filename << time_struct.tm_year + 1900; + filename.width(0); + filename << '_'; + filename.width(2); + filename.fill('0'); + filename << time_struct.tm_mon + 1; + filename.width(0); + filename << '_'; + filename.width(2); + filename.fill('0'); + filename << time_struct.tm_mday; + filename.width(0); + filename << '_'; + filename.width(2); + filename.fill('0'); + filename << time_struct.tm_hour; + filename.width(2); + filename.fill('0'); + filename << time_struct.tm_min; + filename.width(0); + filename << ".log"; + file_name_ = file_dir_ + filename.str(); + + try + { + file_handle_.open(file_name_); + ROS_INFO("Logging raw data to file \"%s\"", file_name_.c_str()); + } + catch (const std::exception& e) + { + ROS_ERROR("Can't log raw data to file. " + "Can't create file \"%s\".", file_name_.c_str()); - } catch(const std::exception& e) { - ROS_ERROR("Can't log raw data to file. " - "Can't create file \"%s\".", file_name_.c_str()); - } - } + } } + } } -void RawDataStreamPa::ubloxCallback(const unsigned char* data, - const std::size_t size) { +void RawDataStreamPa::ubloxCallback(const unsigned char* data, const std::size_t size) +{ + std::string str((const char*)data, size); - std::string str((const char*) data, size); + if (flag_publish_) + { + publishMsg(str); + } - if (flag_publish_) { - publishMsg(str); - } - - saveToFile(str); + saveToFile(str); } -void RawDataStreamPa::msgCallback( - const std_msgs::UInt8MultiArray::ConstPtr& msg) { - - std::string str(msg->data.size(), ' '); - std::copy(msg->data.begin(), msg->data.end(), str.begin()); - saveToFile(str); +void RawDataStreamPa::msgCallback(const std_msgs::UInt8MultiArray::ConstPtr& msg) +{ + std::string str(msg->data.size(), ' '); + std::copy(msg->data.begin(), msg->data.end(), str.begin()); + saveToFile(str); } -std_msgs::UInt8MultiArray RawDataStreamPa::str2uint8( - const std::string str) { - - std_msgs::UInt8MultiArray msg; +std_msgs::UInt8MultiArray RawDataStreamPa::str2uint8(const std::string str) +{ + std_msgs::UInt8MultiArray msg; - msg.layout.data_offset = 0; - msg.layout.dim.push_back(std_msgs::MultiArrayDimension()); - msg.layout.dim[0].size = str.length(); - msg.layout.dim[0].stride = 1; - msg.layout.dim[0].label = "raw_data_stream"; + msg.layout.data_offset = 0; + msg.layout.dim.push_back(std_msgs::MultiArrayDimension()); + msg.layout.dim[0].size = str.length(); + msg.layout.dim[0].stride = 1; + msg.layout.dim[0].label = "raw_data_stream"; - msg.data.resize(str.length()); - std::copy(str.begin(), str.end(), msg.data.begin()); + msg.data.resize(str.length()); + std::copy(str.begin(), str.end(), msg.data.begin()); - return msg; + return msg; } -void RawDataStreamPa::publishMsg(const std::string str) { +void RawDataStreamPa::publishMsg(const std::string str) +{ + static ros::Publisher publisher = pnh_.advertise("raw_data_stream", 100); - static ros::Publisher publisher = - pnh_.advertise("raw_data_stream", 100); - - publisher.publish(RawDataStreamPa::str2uint8(str)); + publisher.publish(RawDataStreamPa::str2uint8(str)); } -void RawDataStreamPa::saveToFile(const std::string str) { - - if (file_handle_.is_open()) { - try { - file_handle_ << str; - // file_handle_.flush(); - } catch(const std::exception& e) { - ROS_WARN("Error writing to file \"%s\"", file_name_.c_str()); - } +void RawDataStreamPa::saveToFile(const std::string str) +{ + if (file_handle_.is_open()) + { + try + { + file_handle_ << str; + // file_handle_.flush(); } + catch (const std::exception& e) + { + ROS_WARN("Error writing to file \"%s\"", file_name_.c_str()); + } + } } - diff --git a/ublox_msg_filters/include/ublox_msg_filters/exact_time.h b/ublox_msg_filters/include/ublox_msg_filters/exact_time.h index f11859b4..61d133f6 100644 --- a/ublox_msg_filters/include/ublox_msg_filters/exact_time.h +++ b/ublox_msg_filters/include/ublox_msg_filters/exact_time.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2009, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * 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. -* * Neither the name of the Willow Garage 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 OWNER 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + * * Neither the name of the Willow Garage 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 OWNER 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. + *********************************************************************/ #ifndef UBLOX_MSG_FILTERS_EXACT_TIME_H #define UBLOX_MSG_FILTERS_EXACT_TIME_H @@ -62,33 +62,52 @@ namespace ublox_msg_filters { - using NullType = message_filters::NullType; using Connection = message_filters::Connection; -template +template using PolicyBase = message_filters::PolicyBase; -template +template using Synchronizer = message_filters::Synchronizer; namespace mpl = boost::mpl; -template +template struct iTOW { - static u_int32_t value(const M& m) { return m.iTOW; } + static u_int32_t value(const M& m) + { + return m.iTOW; + } }; -template<> +template <> struct iTOW { - static u_int32_t value(const NullType& m) { return 0; } + static u_int32_t value(const NullType& m) + { + return 0; + } }; -template +template struct ExactTime : public PolicyBase { typedef Synchronizer Sync; @@ -108,11 +127,7 @@ struct ExactTime : public PolicyBase typedef typename Super::M8Event M8Event; typedef boost::tuple Tuple; - ExactTime(uint32_t queue_size) - : parent_(0) - , queue_size_(queue_size) - , enable_reset_(false) - , last_stamp_(0) + ExactTime(uint32_t queue_size) : parent_(0), queue_size_(queue_size), enable_reset_(false), last_stamp_(0) { } @@ -137,7 +152,7 @@ struct ExactTime : public PolicyBase parent_ = parent; } - template + template void add(const typename mpl::at_c::type& evt) { ROS_ASSERT(parent_); @@ -152,44 +167,44 @@ struct ExactTime : public PolicyBase checkTuple(t); } - template + template Connection registerDropCallback(const C& callback) { - #ifndef _WIN32 +#ifndef _WIN32 return drop_signal_.template addCallback(callback); - #else +#else return drop_signal_.addCallback(callback); - #endif +#endif } - template + template Connection registerDropCallback(C& callback) { - #ifndef _WIN32 +#ifndef _WIN32 return drop_signal_.template addCallback(callback); - #else +#else return drop_signal_.addCallback(callback); - #endif +#endif } - template + template Connection registerDropCallback(const C& callback, T* t) { - #ifndef _WIN32 +#ifndef _WIN32 return drop_signal_.template addCallback(callback, t); - #else +#else return drop_signal_.addCallback(callback, t); - #endif +#endif } - template + template Connection registerDropCallback(C& callback, T* t) { - #ifndef _WIN32 +#ifndef _WIN32 return drop_signal_.template addCallback(callback, t); - #else +#else return drop_signal_.addCallback(callback, t); - #endif +#endif } void setReset(const bool reset) @@ -198,7 +213,6 @@ struct ExactTime : public PolicyBase } private: - // assumes mutex_ is already locked void checkTuple(Tuple& t) { @@ -217,9 +231,15 @@ struct ExactTime : public PolicyBase if (full) { - parent_->signal(boost::get<0>(t), boost::get<1>(t), boost::get<2>(t), - boost::get<3>(t), boost::get<4>(t), boost::get<5>(t), - boost::get<6>(t), boost::get<7>(t), boost::get<8>(t)); + parent_->signal(boost::get<0>(t), + boost::get<1>(t), + boost::get<2>(t), + boost::get<3>(t), + boost::get<4>(t), + boost::get<5>(t), + boost::get<6>(t), + boost::get<7>(t), + boost::get<8>(t)); last_signal_time_ = iTOW::value(*boost::get<0>(t).getMessage()); @@ -233,9 +253,15 @@ struct ExactTime : public PolicyBase while (tuples_.size() > queue_size_) { Tuple& t2 = tuples_.begin()->second; - drop_signal_.call(boost::get<0>(t2), boost::get<1>(t2), boost::get<2>(t2), - boost::get<3>(t2), boost::get<4>(t2), boost::get<5>(t2), - boost::get<6>(t2), boost::get<7>(t2), boost::get<8>(t2)); + drop_signal_.call(boost::get<0>(t2), + boost::get<1>(t2), + boost::get<2>(t2), + boost::get<3>(t2), + boost::get<4>(t2), + boost::get<5>(t2), + boost::get<6>(t2), + boost::get<7>(t2), + boost::get<8>(t2)); tuples_.erase(tuples_.begin()); } } @@ -254,9 +280,15 @@ struct ExactTime : public PolicyBase ++it; Tuple& t = old->second; - drop_signal_.call(boost::get<0>(t), boost::get<1>(t), boost::get<2>(t), - boost::get<3>(t), boost::get<4>(t), boost::get<5>(t), - boost::get<6>(t), boost::get<7>(t), boost::get<8>(t)); + drop_signal_.call(boost::get<0>(t), + boost::get<1>(t), + boost::get<2>(t), + boost::get<3>(t), + boost::get<4>(t), + boost::get<5>(t), + boost::get<6>(t), + boost::get<7>(t), + boost::get<8>(t)); tuples_.erase(old); } else @@ -282,6 +314,6 @@ struct ExactTime : public PolicyBase boost::mutex mutex_; }; -} // namespace ublox_msg_filters +} // namespace ublox_msg_filters -#endif // UBLOX_MSG_FILTERS_EXACT_TIME_H +#endif // UBLOX_MSG_FILTERS_EXACT_TIME_H diff --git a/ublox_msg_filters/src/example.cpp b/ublox_msg_filters/src/example.cpp index ba0bf707..67ec9a6f 100644 --- a/ublox_msg_filters/src/example.cpp +++ b/ublox_msg_filters/src/example.cpp @@ -5,13 +5,15 @@ #include #include -void callback(const ublox_msgs::NavHPPOSLLHConstPtr &msg1, - const ublox_msgs::NavRELPOSNED9ConstPtr &msg2, - const ublox_msgs::NavVELNEDConstPtr &msg3) { +void callback(const ublox_msgs::NavHPPOSLLHConstPtr& msg1, + const ublox_msgs::NavRELPOSNED9ConstPtr& msg2, + const ublox_msgs::NavVELNEDConstPtr& msg3) +{ ROS_INFO("RX %u %u %u", msg1->iTOW, msg2->iTOW, msg3->iTOW); } -int main(int argc, char **argv) { +int main(int argc, char** argv) +{ ros::init(argc, argv, "ublox_sync"); ros::NodeHandle nh; @@ -19,7 +21,8 @@ int main(int argc, char **argv) { message_filters::Subscriber sub2(nh, "msg2", 1); message_filters::Subscriber sub3(nh, "msg3", 1); - typedef ublox_msg_filters::ExactTime MySyncPolicy; + typedef ublox_msg_filters::ExactTime + MySyncPolicy; message_filters::Synchronizer sync(MySyncPolicy(10), sub1, sub2, sub3); sync.registerCallback(boost::bind(callback, _1, _2, _3)); diff --git a/ublox_msg_filters/tests/test.cpp b/ublox_msg_filters/tests/test.cpp index 553dbcdf..5258ee0b 100644 --- a/ublox_msg_filters/tests/test.cpp +++ b/ublox_msg_filters/tests/test.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * 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. -* * Neither the name of the Willow Garage 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 OWNER 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + * * Neither the name of the Willow Garage 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 OWNER 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. + *********************************************************************/ #include @@ -50,14 +50,16 @@ typedef boost::shared_ptr MsgConstPtr; class Helper { public: - Helper() {} + Helper() + { + } void cb() { ++count_; } - void cb3(const MsgConstPtr &msg1, const MsgConstPtr &msg2, const MsgConstPtr &msg3) + void cb3(const MsgConstPtr& msg1, const MsgConstPtr& msg2, const MsgConstPtr& msg3) { ++count_; stamp1_ = msg1->iTOW; @@ -154,7 +156,8 @@ TEST(ExactTime, dropCallback) ASSERT_EQ(h.drop_count_, 1); } -int main(int argc, char **argv){ +int main(int argc, char** argv) +{ testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "blah"); diff --git a/ublox_msgs/include/ublox/serialization/ublox_msgs.h b/ublox_msgs/include/ublox/serialization/ublox_msgs.h index fc7b2c41..b180117d 100644 --- a/ublox_msgs/include/ublox/serialization/ublox_msgs.h +++ b/ublox_msgs/include/ublox/serialization/ublox_msgs.h @@ -14,9 +14,9 @@ // 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 +// 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 BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; @@ -26,7 +26,6 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //============================================================================== - #ifndef UBLOX_SERIALIZATION_UBLOX_MSGS_H #define UBLOX_SERIALIZATION_UBLOX_MSGS_H @@ -35,24 +34,24 @@ #include /// -/// This file declares custom serializers for u-blox messages with dynamic +/// This file declares custom serializers for u-blox messages with dynamic /// lengths and messages where the get/set messages have different sizes, but /// share the same parameters, such as CfgDAT. /// -namespace ublox { - +namespace ublox +{ /// -/// @brief Serializes the CfgDAT message which has a different length for +/// @brief Serializes the CfgDAT message which has a different length for /// get/set. /// template -struct Serializer > { - typedef boost::call_traits > - CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); +struct Serializer > +{ + typedef boost::call_traits > CallTraits; + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.datumNum); stream.next(m.datumName); stream.next(m.majA); @@ -66,14 +65,15 @@ struct Serializer > { stream.next(m.scale); } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { // this is the size of CfgDAT set messages // serializedLength is only used for writes so this is ok return 44; } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { ros::serialization::OStream stream(data, size); // ignores datumNum & datumName stream.next(m.majA); @@ -92,38 +92,40 @@ struct Serializer > { /// @brief Serializes the CfgGNSS message which has a repeated block. /// template -struct Serializer > { +struct Serializer > +{ typedef ublox_msgs::CfgGNSS_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.msgVer); stream.next(m.numTrkChHw); stream.next(m.numTrkChUse); stream.next(m.numConfigBlocks); m.blocks.resize(m.numConfigBlocks); - for(std::size_t i = 0; i < m.blocks.size(); ++i) + for (std::size_t i = 0; i < m.blocks.size(); ++i) ros::serialization::deserialize(stream, m.blocks[i]); } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 4 + 8 * m.numConfigBlocks; } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.blocks.size() != m.numConfigBlocks) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { + if (m.blocks.size() != m.numConfigBlocks) + { ROS_ERROR("CfgGNSS numConfigBlocks must equal blocks size"); } ros::serialization::OStream stream(data, size); stream.next(m.msgVer); stream.next(m.numTrkChHw); stream.next(m.numTrkChUse); - stream.next( - static_cast(m.blocks.size())); - for(std::size_t i = 0; i < m.blocks.size(); ++i) + stream.next(static_cast(m.blocks.size())); + for (std::size_t i = 0; i < m.blocks.size(); ++i) ros::serialization::serialize(stream, m.blocks[i]); } }; @@ -132,27 +134,28 @@ struct Serializer > { /// @brief Serializes the CfgInf message which has a repeated block. /// template -struct Serializer > { - typedef boost::call_traits > - CallTraits; +struct Serializer > +{ + typedef boost::call_traits > CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); int num_blocks = count / 10; m.blocks.resize(num_blocks); - for(std::size_t i = 0; i < num_blocks; ++i) + for (std::size_t i = 0; i < num_blocks; ++i) ros::serialization::deserialize(stream, m.blocks[i]); } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 10 * m.blocks.size(); } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { ros::serialization::OStream stream(data, size); - for(std::size_t i = 0; i < m.blocks.size(); ++i) + for (std::size_t i = 0; i < m.blocks.size(); ++i) ros::serialization::serialize(stream, m.blocks[i]); } }; @@ -161,25 +164,27 @@ struct Serializer > { /// @brief Serializes the Inf message which has a dynamic length string. /// template -struct Serializer > { +struct Serializer > +{ typedef boost::call_traits > CallTraits; - - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); m.str.resize(count); for (int i = 0; i < count; ++i) ros::serialization::deserialize(stream, m.str[i]); } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return m.str.size(); } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { ros::serialization::OStream stream(data, size); - for(std::size_t i = 0; i < m.str.size(); ++i) + for (std::size_t i = 0; i < m.str.size(); ++i) ros::serialization::serialize(stream, m.str[i]); } }; @@ -188,13 +193,14 @@ struct Serializer > { /// @brief Serializes the MonVER message which has a repeated block. /// template -struct Serializer > { +struct Serializer > +{ typedef ublox_msgs::MonVER_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.swVersion); stream.next(m.hwVersion); @@ -202,23 +208,25 @@ struct Serializer > { int N = (count - 40) / 30; m.extension.reserve(N); typename Msg::_extension_type::value_type ext; - for (int i = 0; i < N; i++) { + for (int i = 0; i < N; i++) + { // Read each extension string stream.next(ext); m.extension.push_back(ext); } } - static uint32_t serializedLength(typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 40 + (30 * m.extension.size()); } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { ros::serialization::OStream stream(data, size); stream.next(m.swVersion); stream.next(m.hwVersion); - for(std::size_t i = 0; i < m.extension.size(); ++i) + for (std::size_t i = 0; i < m.extension.size(); ++i) ros::serialization::serialize(stream, m.extension[i]); } }; @@ -227,13 +235,14 @@ struct Serializer > { /// @brief Serializes the NavDGPS message which has a repeated block. /// template -struct Serializer > { +struct Serializer > +{ typedef ublox_msgs::NavDGPS_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.iTOW); stream.next(m.age); stream.next(m.baseId); @@ -242,17 +251,19 @@ struct Serializer > { stream.next(m.status); stream.next(m.reserved1); m.sv.resize(m.numCh); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::deserialize(stream, m.sv[i]); } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 16 + 12 * m.numCh; } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.sv.size() != m.numCh) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { + if (m.sv.size() != m.numCh) + { ROS_ERROR("NavDGPS numCh must equal sv size"); } ros::serialization::OStream stream(data, size); @@ -263,23 +274,23 @@ struct Serializer > { stream.next(static_cast(m.sv.size())); stream.next(m.status); stream.next(m.reserved1); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::serialize(stream, m.sv[i]); } }; - /// /// @brief Serializes the NavSBAS message which has a repeated block. /// template -struct Serializer > { +struct Serializer > +{ typedef ublox_msgs::NavSBAS_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.iTOW); stream.next(m.geo); stream.next(m.mode); @@ -288,17 +299,19 @@ struct Serializer > { stream.next(m.cnt); stream.next(m.reserved0); m.sv.resize(m.cnt); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::deserialize(stream, m.sv[i]); } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 12 + 12 * m.cnt; } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.sv.size() != m.cnt) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { + if (m.sv.size() != m.cnt) + { ROS_ERROR("NavSBAS cnt must equal sv size"); } ros::serialization::OStream stream(data, size); @@ -309,7 +322,7 @@ struct Serializer > { stream.next(m.service); stream.next(static_cast(m.sv.size())); stream.next(m.reserved0); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::serialize(stream, m.sv[i]); } }; @@ -318,29 +331,32 @@ struct Serializer > { /// @brief Serializes the NavSAT message which has a repeated block. /// template -struct Serializer > { +struct Serializer > +{ typedef ublox_msgs::NavSAT_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.iTOW); stream.next(m.version); stream.next(m.numSvs); stream.next(m.reserved0); m.sv.resize(m.numSvs); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::deserialize(stream, m.sv[i]); } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 8 + 12 * m.numSvs; } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.sv.size() != m.numSvs) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { + if (m.sv.size() != m.numSvs) + { ROS_ERROR("NavSAT numSvs must equal sv size"); } ros::serialization::OStream stream(data, size); @@ -348,7 +364,7 @@ struct Serializer > { stream.next(m.version); stream.next(static_cast(m.sv.size())); stream.next(m.reserved0); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::serialize(stream, m.sv[i]); } }; @@ -357,29 +373,32 @@ struct Serializer > { /// @brief Serializes the NavDGPS message which has a repeated block. /// template -struct Serializer > { +struct Serializer > +{ typedef ublox_msgs::NavSVINFO_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.iTOW); stream.next(m.numCh); stream.next(m.globalFlags); stream.next(m.reserved2); m.sv.resize(m.numCh); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::deserialize(stream, m.sv[i]); } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 8 + 12 * m.numCh; } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.sv.size() != m.numCh) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { + if (m.sv.size() != m.numCh) + { ROS_ERROR("NavSVINFO numCh must equal sv size"); } ros::serialization::OStream stream(data, size); @@ -387,7 +406,7 @@ struct Serializer > { stream.next(static_cast(m.sv.size())); stream.next(m.globalFlags); stream.next(m.reserved2); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::serialize(stream, m.sv[i]); } }; @@ -396,29 +415,32 @@ struct Serializer > { /// @brief Serializes the RxmRAW message which has a repeated block. /// template -struct Serializer > { +struct Serializer > +{ typedef ublox_msgs::RxmRAW_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.rcvTOW); stream.next(m.week); stream.next(m.numSV); stream.next(m.reserved1); m.sv.resize(m.numSV); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::deserialize(stream, m.sv[i]); } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 8 + 24 * m.numSV; } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.sv.size() != m.numSV) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { + if (m.sv.size() != m.numSV) + { ROS_ERROR("RxmRAW numSV must equal sv size"); } ros::serialization::OStream stream(data, size); @@ -426,7 +448,7 @@ struct Serializer > { stream.next(m.week); stream.next(static_cast(m.sv.size())); stream.next(m.reserved1); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::serialize(stream, m.sv[i]); } }; @@ -435,13 +457,14 @@ struct Serializer > { /// @brief Serializes the RxmRAWX message which has a repeated block. /// template -struct Serializer > { +struct Serializer > +{ typedef ublox_msgs::RxmRAWX_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.rcvTOW); stream.next(m.week); stream.next(m.leapS); @@ -450,17 +473,19 @@ struct Serializer > { stream.next(m.version); stream.next(m.reserved1); m.meas.resize(m.numMeas); - for(std::size_t i = 0; i < m.meas.size(); ++i) + for (std::size_t i = 0; i < m.meas.size(); ++i) ros::serialization::deserialize(stream, m.meas[i]); } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 16 + 32 * m.numMeas; } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.meas.size() != m.numMeas) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { + if (m.meas.size() != m.numMeas) + { ROS_ERROR("RxmRAWX numMeas must equal meas size"); } ros::serialization::OStream stream(data, size); @@ -471,7 +496,7 @@ struct Serializer > { stream.next(m.recStat); stream.next(m.version); stream.next(m.reserved1); - for(std::size_t i = 0; i < m.meas.size(); ++i) + for (std::size_t i = 0; i < m.meas.size(); ++i) ros::serialization::serialize(stream, m.meas[i]); } }; @@ -480,13 +505,14 @@ struct Serializer > { /// @brief Serializes the RxmSFRBX message which has a repeated block. /// template -struct Serializer > { +struct Serializer > +{ typedef ublox_msgs::RxmSFRBX_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.gnssId); stream.next(m.svId); stream.next(m.reserved0); @@ -496,17 +522,19 @@ struct Serializer > { stream.next(m.version); stream.next(m.reserved1); m.dwrd.resize(m.numWords); - for(std::size_t i = 0; i < m.dwrd.size(); ++i) + for (std::size_t i = 0; i < m.dwrd.size(); ++i) ros::serialization::deserialize(stream, m.dwrd[i]); } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 8 + 4 * m.numWords; } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.dwrd.size() != m.numWords) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { + if (m.dwrd.size() != m.numWords) + { ROS_ERROR("RxmSFRBX numWords must equal dwrd size"); } ros::serialization::OStream stream(data, size); @@ -518,7 +546,7 @@ struct Serializer > { stream.next(m.chn); stream.next(m.version); stream.next(m.reserved1); - for(std::size_t i = 0; i < m.dwrd.size(); ++i) + for (std::size_t i = 0; i < m.dwrd.size(); ++i) ros::serialization::serialize(stream, m.dwrd[i]); } }; @@ -527,29 +555,32 @@ struct Serializer > { /// @brief Serializes the RxmSVSI message which has a repeated block. /// template -struct Serializer > { +struct Serializer > +{ typedef ublox_msgs::RxmSVSI_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.iTOW); stream.next(m.week); stream.next(m.numVis); stream.next(m.numSV); m.sv.resize(m.numSV); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::deserialize(stream, m.sv[i]); } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 8 + 6 * m.numSV; } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.sv.size() != m.numSV) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { + if (m.sv.size() != m.numSV) + { ROS_ERROR("RxmSVSI numSV must equal sv size"); } ros::serialization::OStream stream(data, size); @@ -557,7 +588,7 @@ struct Serializer > { stream.next(m.week); stream.next(m.numVis); stream.next(static_cast(m.sv.size())); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::serialize(stream, m.sv[i]); } }; @@ -566,37 +597,41 @@ struct Serializer > { /// @brief Serializes the RxmALM message which has a repeated block. /// template -struct Serializer > { +struct Serializer > +{ typedef ublox_msgs::RxmALM_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.svid); stream.next(m.week); m.dwrd.clear(); - if(count == 40) { + if (count == 40) + { typename Msg::_dwrd_type::value_type temp; m.dwrd.resize(8); - for(std::size_t i = 0; i < 8; ++i) { + for (std::size_t i = 0; i < 8; ++i) + { stream.next(temp); m.dwrd.push_back(temp); } } } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 8 + (4 * m.dwrd.size()); } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { ros::serialization::OStream stream(data, size); stream.next(m.svid); stream.next(m.week); - for(std::size_t i = 0; i < m.dwrd.size(); ++i) + for (std::size_t i = 0; i < m.dwrd.size(); ++i) ros::serialization::serialize(stream, m.dwrd[i]); } }; @@ -610,52 +645,57 @@ struct Serializer > typedef ublox_msgs::RxmEPH_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.svid); stream.next(m.how); m.sf1d.clear(); m.sf2d.clear(); m.sf3d.clear(); - if (count == 104) { + if (count == 104) + { typename Msg::_sf1d_type::value_type temp1; typename Msg::_sf2d_type::value_type temp2; typename Msg::_sf3d_type::value_type temp3; m.sf1d.resize(8); - for(std::size_t i = 0; i < 8; ++i) { + for (std::size_t i = 0; i < 8; ++i) + { stream.next(temp1); m.sf1d.push_back(temp1); } m.sf2d.resize(8); - for(std::size_t i = 0; i < 8; ++i) { + for (std::size_t i = 0; i < 8; ++i) + { stream.next(temp2); m.sf2d.push_back(temp2); } m.sf3d.resize(8); - for(std::size_t i = 0; i < 8; ++i) { + for (std::size_t i = 0; i < 8; ++i) + { stream.next(temp3); m.sf3d.push_back(temp3); } } } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 8 + (4 * m.sf1d.size()) + (4 * m.sf2d.size()) + (4 * m.sf3d.size()); } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { ros::serialization::OStream stream(data, size); stream.next(m.svid); stream.next(m.how); - for(std::size_t i = 0; i < m.sf1d.size(); ++i) + for (std::size_t i = 0; i < m.sf1d.size(); ++i) ros::serialization::serialize(stream, m.sf1d[i]); - for(std::size_t i = 0; i < m.sf2d.size(); ++i) + for (std::size_t i = 0; i < m.sf2d.size(); ++i) ros::serialization::serialize(stream, m.sf2d[i]); - for(std::size_t i = 0; i < m.sf3d.size(); ++i) + for (std::size_t i = 0; i < m.sf3d.size(); ++i) ros::serialization::serialize(stream, m.sf3d[i]); } }; @@ -664,37 +704,41 @@ struct Serializer > /// @brief Serializes the AidALM message which has a repeated block. /// template -struct Serializer > { +struct Serializer > +{ typedef ublox_msgs::AidALM_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.svid); stream.next(m.week); m.dwrd.clear(); - if (count == 40) { + if (count == 40) + { typename Msg::_dwrd_type::value_type temp; m.dwrd.resize(8); - for(std::size_t i = 0; i < 8; ++i) { + for (std::size_t i = 0; i < 8; ++i) + { stream.next(temp); m.dwrd.push_back(temp); } - } + } } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 8 + (4 * m.dwrd.size()); } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { ros::serialization::OStream stream(data, size); stream.next(m.svid); stream.next(m.week); - for(std::size_t i = 0; i < m.dwrd.size(); ++i) + for (std::size_t i = 0; i < m.dwrd.size(); ++i) ros::serialization::serialize(stream, m.dwrd[i]); } }; @@ -708,51 +752,56 @@ struct Serializer > typedef ublox_msgs::AidEPH_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.svid); stream.next(m.how); m.sf1d.clear(); m.sf2d.clear(); m.sf3d.clear(); - if (count == 104) { + if (count == 104) + { typename Msg::_sf1d_type::value_type temp1; typename Msg::_sf2d_type::value_type temp2; typename Msg::_sf3d_type::value_type temp3; m.sf1d.resize(8); - for(std::size_t i = 0; i < 8; ++i) { + for (std::size_t i = 0; i < 8; ++i) + { stream.next(temp1); m.sf1d.push_back(temp1); } m.sf2d.resize(8); - for(std::size_t i = 0; i < 8; ++i) { + for (std::size_t i = 0; i < 8; ++i) + { stream.next(temp2); m.sf2d.push_back(temp2); } m.sf3d.resize(8); - for(std::size_t i = 0; i < 8; ++i) { + for (std::size_t i = 0; i < 8; ++i) + { stream.next(temp3); m.sf3d.push_back(temp3); } } } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 8 + (4 * m.sf1d.size()) + (4 * m.sf2d.size()) + (4 * m.sf3d.size()); } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { ros::serialization::OStream stream(data, size); stream.next(m.svid); stream.next(m.how); - for(std::size_t i = 0; i < m.sf1d.size(); ++i) + for (std::size_t i = 0; i < m.sf1d.size(); ++i) ros::serialization::serialize(stream, m.sf1d[i]); - for(std::size_t i = 0; i < m.sf2d.size(); ++i) + for (std::size_t i = 0; i < m.sf2d.size(); ++i) ros::serialization::serialize(stream, m.sf2d[i]); - for(std::size_t i = 0; i < m.sf3d.size(); ++i) + for (std::size_t i = 0; i < m.sf3d.size(); ++i) ros::serialization::serialize(stream, m.sf3d[i]); } }; @@ -762,13 +811,13 @@ struct Serializer > /// optional block. /// template -struct Serializer > { - typedef boost::call_traits > - CallTraits; +struct Serializer > +{ + typedef boost::call_traits > CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.timeTag); stream.next(m.flags); stream.next(m.id); @@ -777,28 +826,30 @@ struct Serializer > { int data_size = (count - (calib_valid ? 12 : 8)) / 4; // Repeating block m.data.resize(data_size); - for(std::size_t i = 0; i < data_size; ++i) + for (std::size_t i = 0; i < data_size; ++i) ros::serialization::deserialize(stream, m.data[i]); // Optional block - if(calib_valid) { + if (calib_valid) + { m.calibTtag.resize(1); ros::serialization::deserialize(stream, m.calibTtag[0]); } } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 4 + 8 * m.data.size() + 4 * m.calibTtag.size(); } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { ros::serialization::OStream stream(data, size); stream.next(m.timeTag); stream.next(m.flags); stream.next(m.id); - for(std::size_t i = 0; i < m.data.size(); ++i) + for (std::size_t i = 0; i < m.data.size(); ++i) ros::serialization::serialize(stream, m.data[i]); - for(std::size_t i = 0; i < m.calibTtag.size(); ++i) + for (std::size_t i = 0; i < m.calibTtag.size(); ++i) ros::serialization::serialize(stream, m.calibTtag[i]); } }; @@ -807,31 +858,31 @@ struct Serializer > { /// @brief Serializes the EsfRAW message which has a repeated block. /// template -struct Serializer > { - typedef boost::call_traits > - CallTraits; +struct Serializer > +{ + typedef boost::call_traits > CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.reserved0); m.blocks.clear(); int num_blocks = (count - 4) / 8; m.blocks.resize(num_blocks); - for(std::size_t i = 0; i < num_blocks; ++i) + for (std::size_t i = 0; i < num_blocks; ++i) ros::serialization::deserialize(stream, m.blocks[i]); } - - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 4 + 8 * m.blocks.size(); } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { ros::serialization::OStream stream(data, size); stream.next(m.reserved0); - for(std::size_t i = 0; i < m.blocks.size(); ++i) + for (std::size_t i = 0; i < m.blocks.size(); ++i) ros::serialization::serialize(stream, m.blocks[i]); } }; @@ -840,30 +891,33 @@ struct Serializer > { /// @brief Serializes the EsfSTATUS message which has a repeated block. /// template -struct Serializer > { +struct Serializer > +{ typedef ublox_msgs::EsfSTATUS_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { - ros::serialization::IStream stream(const_cast(data), count); + static void read(const uint8_t* data, uint32_t count, typename CallTraits::reference m) + { + ros::serialization::IStream stream(const_cast(data), count); stream.next(m.iTOW); stream.next(m.version); stream.next(m.fusionMode); stream.next(m.reserved2); stream.next(m.numSens); m.sens.resize(m.numSens); - for(std::size_t i = 0; i < m.sens.size(); ++i) + for (std::size_t i = 0; i < m.sens.size(); ++i) ros::serialization::deserialize(stream, m.sens[i]); } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(typename CallTraits::param_type m) + { return 16 + 4 * m.numSens; } - static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.sens.size() != m.numSens) { + static void write(uint8_t* data, uint32_t size, typename CallTraits::param_type m) + { + if (m.sens.size() != m.numSens) + { ROS_ERROR("Writing EsfSTATUS message: numSens must equal size of sens"); } ros::serialization::OStream stream(data, size); @@ -872,12 +926,11 @@ struct Serializer > { stream.next(m.fusionMode); stream.next(m.reserved2); stream.next(static_cast(m.sens.size())); - for(std::size_t i = 0; i < m.sens.size(); ++i) + for (std::size_t i = 0; i < m.sens.size(); ++i) ros::serialization::serialize(stream, m.sens[i]); } }; +} // namespace ublox -} // namespace ublox - -#endif // UBLOX_SERIALIZATION_UBLOX_MSGS_H +#endif // UBLOX_SERIALIZATION_UBLOX_MSGS_H diff --git a/ublox_msgs/include/ublox_msgs/ublox_msgs.h b/ublox_msgs/include/ublox_msgs/ublox_msgs.h index d640e56a..006dc0b5 100644 --- a/ublox_msgs/include/ublox_msgs/ublox_msgs.h +++ b/ublox_msgs/include/ublox_msgs/ublox_msgs.h @@ -14,9 +14,9 @@ // 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 +// 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 BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; @@ -111,153 +111,167 @@ #include -namespace ublox_msgs { - -namespace Class { - static const uint8_t NAV = 0x01; //!< Navigation Result Messages: Position, - //!< Speed, Time, Acceleration, Heading, +namespace ublox_msgs +{ +namespace Class +{ +static const uint8_t NAV = 0x01; //!< Navigation Result Messages: Position, + //!< Speed, Time, Acceleration, Heading, //!< DOP, SVs used - static const uint8_t RXM = 0x02; //!< Receiver Manager Messages: +static const uint8_t RXM = 0x02; //!< Receiver Manager Messages: //!< Satellite Status, RTC Status - static const uint8_t INF = 0x04; //!< Information Messages: +static const uint8_t INF = 0x04; //!< Information Messages: //!< Printf-Style Messages, with IDs such as //!< Error, Warning, Notice - static const uint8_t ACK = 0x05; //!< Ack/Nack Messages: Acknowledge or Reject +static const uint8_t ACK = 0x05; //!< Ack/Nack Messages: Acknowledge or Reject //!< messages to CFG input messages - static const uint8_t CFG = 0x06; //!< Configuration Input Messages: Set - //!< Dynamic Model, Set DOP Mask, Set Baud +static const uint8_t CFG = 0x06; //!< Configuration Input Messages: Set + //!< Dynamic Model, Set DOP Mask, Set Baud //!< Rate, etc. - static const uint8_t UPD = 0x09; //!< Firmware Update Messages: i.e. - //!< Memory/Flash erase/write, Reboot, Flash +static const uint8_t UPD = 0x09; //!< Firmware Update Messages: i.e. + //!< Memory/Flash erase/write, Reboot, Flash //!< identification, etc. - //!< Used to update the firmware and identify + //!< Used to update the firmware and identify //!< any attached flash device - static const uint8_t MON = 0x0A; //!< Monitoring Messages: Communication - //!< Status, CPU Load, Stack Usage, +static const uint8_t MON = 0x0A; //!< Monitoring Messages: Communication + //!< Status, CPU Load, Stack Usage, //!< Task Status - static const uint8_t AID = 0x0B; //!< AssistNow Aiding Messages: Ephemeris, +static const uint8_t AID = 0x0B; //!< AssistNow Aiding Messages: Ephemeris, //!< Almanac, other A-GPS data input - static const uint8_t TIM = 0x0D; //!< Timing Messages: Timepulse Output, +static const uint8_t TIM = 0x0D; //!< Timing Messages: Timepulse Output, //!< Timemark Results - static const uint8_t ESF = 0x10; //!< External Sensor Fusion Messages: - //!< External sensor measurements and status +static const uint8_t ESF = 0x10; //!< External Sensor Fusion Messages: + //!< External sensor measurements and status //!< information - static const uint8_t MGA = 0x13; //!< Multiple GNSS Assistance Messages: +static const uint8_t MGA = 0x13; //!< Multiple GNSS Assistance Messages: //!< Assistance data for various GNSS - static const uint8_t LOG = 0x21; //!< Logging Messages: Log creation, +static const uint8_t LOG = 0x21; //!< Logging Messages: Log creation, //!< deletion, info and retrieval - static const uint8_t SEC = 0x27; //!< Security Feature Messages - static const uint8_t HNR = 0x28; //!< High Rate Navigation Results Messages: +static const uint8_t SEC = 0x27; //!< Security Feature Messages +static const uint8_t HNR = 0x28; //!< High Rate Navigation Results Messages: //!< High rate time, position, speed, heading - static const uint8_t RTCM = 0xF5; //!< RTCM Configuration Messages -} +static const uint8_t RTCM = 0xF5; //!< RTCM Configuration Messages +} // namespace Class -namespace Message { - namespace NAV { - static const uint8_t ATT = NavATT::MESSAGE_ID; - static const uint8_t CLOCK = NavCLOCK::MESSAGE_ID; - static const uint8_t DGPS = NavDGPS::MESSAGE_ID; - static const uint8_t DOP = NavDOP::MESSAGE_ID; - static const uint8_t HPPOSECEF = NavHPPOSECEF::MESSAGE_ID; - static const uint8_t HPPOSLLH = NavHPPOSLLH::MESSAGE_ID; - static const uint8_t POSECEF = NavPOSECEF::MESSAGE_ID; - static const uint8_t POSLLH = NavPOSLLH::MESSAGE_ID; - static const uint8_t RELPOSNED = NavRELPOSNED::MESSAGE_ID; - static const uint8_t RELPOSNED9 = NavRELPOSNED9::MESSAGE_ID; - static const uint8_t SBAS = NavSBAS::MESSAGE_ID; - static const uint8_t SOL = NavSOL::MESSAGE_ID; - static const uint8_t PVT = NavPVT::MESSAGE_ID; - static const uint8_t SAT = NavSAT::MESSAGE_ID; - static const uint8_t STATUS = NavSTATUS::MESSAGE_ID; - static const uint8_t SVINFO = NavSVINFO::MESSAGE_ID; - static const uint8_t SVIN = NavSVIN::MESSAGE_ID; - static const uint8_t TIMEGPS = NavTIMEGPS::MESSAGE_ID; - static const uint8_t TIMEUTC = NavTIMEUTC::MESSAGE_ID; - static const uint8_t VELECEF = NavVELECEF::MESSAGE_ID; - static const uint8_t VELNED = NavVELNED::MESSAGE_ID; - } +namespace Message +{ +namespace NAV +{ +static const uint8_t ATT = NavATT::MESSAGE_ID; +static const uint8_t CLOCK = NavCLOCK::MESSAGE_ID; +static const uint8_t DGPS = NavDGPS::MESSAGE_ID; +static const uint8_t DOP = NavDOP::MESSAGE_ID; +static const uint8_t HPPOSECEF = NavHPPOSECEF::MESSAGE_ID; +static const uint8_t HPPOSLLH = NavHPPOSLLH::MESSAGE_ID; +static const uint8_t POSECEF = NavPOSECEF::MESSAGE_ID; +static const uint8_t POSLLH = NavPOSLLH::MESSAGE_ID; +static const uint8_t RELPOSNED = NavRELPOSNED::MESSAGE_ID; +static const uint8_t RELPOSNED9 = NavRELPOSNED9::MESSAGE_ID; +static const uint8_t SBAS = NavSBAS::MESSAGE_ID; +static const uint8_t SOL = NavSOL::MESSAGE_ID; +static const uint8_t PVT = NavPVT::MESSAGE_ID; +static const uint8_t SAT = NavSAT::MESSAGE_ID; +static const uint8_t STATUS = NavSTATUS::MESSAGE_ID; +static const uint8_t SVINFO = NavSVINFO::MESSAGE_ID; +static const uint8_t SVIN = NavSVIN::MESSAGE_ID; +static const uint8_t TIMEGPS = NavTIMEGPS::MESSAGE_ID; +static const uint8_t TIMEUTC = NavTIMEUTC::MESSAGE_ID; +static const uint8_t VELECEF = NavVELECEF::MESSAGE_ID; +static const uint8_t VELNED = NavVELNED::MESSAGE_ID; +} // namespace NAV - namespace RXM { - static const uint8_t ALM = RxmALM::MESSAGE_ID; - static const uint8_t EPH = RxmEPH::MESSAGE_ID; - static const uint8_t RAW = RxmRAW::MESSAGE_ID; - static const uint8_t RAWX = RxmRAWX::MESSAGE_ID; - static const uint8_t RTCM = RxmRTCM::MESSAGE_ID; - static const uint8_t SFRB = RxmSFRB::MESSAGE_ID; - static const uint8_t SFRBX = RxmSFRBX::MESSAGE_ID; - static const uint8_t SVSI = RxmSVSI::MESSAGE_ID; - } +namespace RXM +{ +static const uint8_t ALM = RxmALM::MESSAGE_ID; +static const uint8_t EPH = RxmEPH::MESSAGE_ID; +static const uint8_t RAW = RxmRAW::MESSAGE_ID; +static const uint8_t RAWX = RxmRAWX::MESSAGE_ID; +static const uint8_t RTCM = RxmRTCM::MESSAGE_ID; +static const uint8_t SFRB = RxmSFRB::MESSAGE_ID; +static const uint8_t SFRBX = RxmSFRBX::MESSAGE_ID; +static const uint8_t SVSI = RxmSVSI::MESSAGE_ID; +} // namespace RXM - namespace INF { - static const uint8_t ERROR = 0x00; - static const uint8_t WARNING = 0x01; - static const uint8_t NOTICE = 0x02; - static const uint8_t TEST = 0x03; - static const uint8_t DEBUG = 0x04; - } +namespace INF +{ +static const uint8_t ERROR = 0x00; +static const uint8_t WARNING = 0x01; +static const uint8_t NOTICE = 0x02; +static const uint8_t TEST = 0x03; +static const uint8_t DEBUG = 0x04; +} // namespace INF - namespace ACK { - static const uint8_t NACK = 0x00; - static const uint8_t ACK = 0x01; - } +namespace ACK +{ +static const uint8_t NACK = 0x00; +static const uint8_t ACK = 0x01; +} // namespace ACK - namespace AID { - static const uint8_t ALM = AidALM::MESSAGE_ID; - static const uint8_t EPH = AidEPH::MESSAGE_ID; - static const uint8_t HUI = AidHUI::MESSAGE_ID; - } +namespace AID +{ +static const uint8_t ALM = AidALM::MESSAGE_ID; +static const uint8_t EPH = AidEPH::MESSAGE_ID; +static const uint8_t HUI = AidHUI::MESSAGE_ID; +} // namespace AID - namespace CFG { - static const uint8_t ANT = CfgANT::MESSAGE_ID; - static const uint8_t CFG = CfgCFG::MESSAGE_ID; - static const uint8_t DAT = CfgDAT::MESSAGE_ID; - static const uint8_t GNSS = CfgGNSS::MESSAGE_ID; - static const uint8_t HNR = CfgHNR::MESSAGE_ID; - static const uint8_t INF = CfgINF::MESSAGE_ID; - static const uint8_t DGNSS = CfgDGNSS::MESSAGE_ID; - static const uint8_t MSG = CfgMSG::MESSAGE_ID; - static const uint8_t NAV5 = CfgNAV5::MESSAGE_ID; - static const uint8_t NAVX5 = CfgNAVX5::MESSAGE_ID; - static const uint8_t NMEA = CfgNMEA::MESSAGE_ID; - static const uint8_t PRT = CfgPRT::MESSAGE_ID; - static const uint8_t RATE = CfgRATE::MESSAGE_ID; - static const uint8_t RST = CfgRST::MESSAGE_ID; - static const uint8_t SBAS = CfgSBAS::MESSAGE_ID; - static const uint8_t TMODE3 = CfgTMODE3::MESSAGE_ID; - static const uint8_t USB = CfgUSB::MESSAGE_ID; - } +namespace CFG +{ +static const uint8_t ANT = CfgANT::MESSAGE_ID; +static const uint8_t CFG = CfgCFG::MESSAGE_ID; +static const uint8_t DAT = CfgDAT::MESSAGE_ID; +static const uint8_t GNSS = CfgGNSS::MESSAGE_ID; +static const uint8_t HNR = CfgHNR::MESSAGE_ID; +static const uint8_t INF = CfgINF::MESSAGE_ID; +static const uint8_t DGNSS = CfgDGNSS::MESSAGE_ID; +static const uint8_t MSG = CfgMSG::MESSAGE_ID; +static const uint8_t NAV5 = CfgNAV5::MESSAGE_ID; +static const uint8_t NAVX5 = CfgNAVX5::MESSAGE_ID; +static const uint8_t NMEA = CfgNMEA::MESSAGE_ID; +static const uint8_t PRT = CfgPRT::MESSAGE_ID; +static const uint8_t RATE = CfgRATE::MESSAGE_ID; +static const uint8_t RST = CfgRST::MESSAGE_ID; +static const uint8_t SBAS = CfgSBAS::MESSAGE_ID; +static const uint8_t TMODE3 = CfgTMODE3::MESSAGE_ID; +static const uint8_t USB = CfgUSB::MESSAGE_ID; +} // namespace CFG - namespace UPD { - //! SOS and SOS_Ack have the same message ID, but different lengths - static const uint8_t SOS = UpdSOS::MESSAGE_ID; - } - - namespace MON { - static const uint8_t GNSS = MonGNSS::MESSAGE_ID; - static const uint8_t HW = MonHW::MESSAGE_ID; - static const uint8_t VER = MonVER::MESSAGE_ID; - } +namespace UPD +{ +//! SOS and SOS_Ack have the same message ID, but different lengths +static const uint8_t SOS = UpdSOS::MESSAGE_ID; +} // namespace UPD - namespace ESF { - static const uint8_t INS = EsfINS::MESSAGE_ID; - static const uint8_t MEAS = EsfMEAS::MESSAGE_ID; - static const uint8_t RAW = EsfRAW::MESSAGE_ID; - static const uint8_t STATUS = EsfSTATUS::MESSAGE_ID; - } +namespace MON +{ +static const uint8_t GNSS = MonGNSS::MESSAGE_ID; +static const uint8_t HW = MonHW::MESSAGE_ID; +static const uint8_t VER = MonVER::MESSAGE_ID; +} // namespace MON - namespace MGA { - static const uint8_t GAL = MgaGAL::MESSAGE_ID; - } +namespace ESF +{ +static const uint8_t INS = EsfINS::MESSAGE_ID; +static const uint8_t MEAS = EsfMEAS::MESSAGE_ID; +static const uint8_t RAW = EsfRAW::MESSAGE_ID; +static const uint8_t STATUS = EsfSTATUS::MESSAGE_ID; +} // namespace ESF - namespace HNR { - static const uint8_t PVT = HnrPVT::MESSAGE_ID; - } +namespace MGA +{ +static const uint8_t GAL = MgaGAL::MESSAGE_ID; +} + +namespace HNR +{ +static const uint8_t PVT = HnrPVT::MESSAGE_ID; +} - namespace TIM { - static const uint8_t TM2 = TimTM2::MESSAGE_ID; - } +namespace TIM +{ +static const uint8_t TM2 = TimTM2::MESSAGE_ID; } +} // namespace Message -} //!< namespace ublox_msgs +} // namespace ublox_msgs -#endif //!< UBLOX_MSGS_H +#endif //!< UBLOX_MSGS_H diff --git a/ublox_msgs/src/ublox_msgs.cpp b/ublox_msgs/src/ublox_msgs.cpp index 7a6b90d3..ca2abd3a 100644 --- a/ublox_msgs/src/ublox_msgs.cpp +++ b/ublox_msgs/src/ublox_msgs.cpp @@ -14,9 +14,9 @@ // 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 +// 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 BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; @@ -29,176 +29,95 @@ #include template -std::vector > ublox::Message::keys_; - -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::ATT, - ublox_msgs, NavATT); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::CLOCK, - ublox_msgs, NavCLOCK); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::DGPS, - ublox_msgs, NavDGPS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::DOP, - ublox_msgs, NavDOP); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::HPPOSECEF, - ublox_msgs, NavHPPOSECEF); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::HPPOSLLH, - ublox_msgs, NavHPPOSLLH); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::POSECEF, - ublox_msgs, NavPOSECEF); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::POSLLH, - ublox_msgs, NavPOSLLH); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, - ublox_msgs::Message::NAV::RELPOSNED, - ublox_msgs, - NavRELPOSNED); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, - ublox_msgs::Message::NAV::RELPOSNED9, - ublox_msgs, - NavRELPOSNED9); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SBAS, - ublox_msgs, NavSBAS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SOL, - ublox_msgs, NavSOL); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::PVT, - ublox_msgs, NavPVT); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::PVT, - ublox_msgs, NavPVT7); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SAT, - ublox_msgs, NavSAT); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::STATUS, - ublox_msgs, NavSTATUS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SVIN, - ublox_msgs, NavSVIN); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SVINFO, - ublox_msgs, NavSVINFO); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::TIMEGPS, - ublox_msgs, NavTIMEGPS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::TIMEUTC, - ublox_msgs, NavTIMEUTC); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::VELECEF, - ublox_msgs, NavVELECEF); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::VELNED, - ublox_msgs, NavVELNED); - -// ACK messages are declared differently because they both have the same +std::vector > ublox::Message::keys_; + +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::ATT, ublox_msgs, NavATT); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::CLOCK, ublox_msgs, NavCLOCK); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::DGPS, ublox_msgs, NavDGPS); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::DOP, ublox_msgs, NavDOP); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::HPPOSECEF, ublox_msgs, NavHPPOSECEF); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::HPPOSLLH, ublox_msgs, NavHPPOSLLH); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::POSECEF, ublox_msgs, NavPOSECEF); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::POSLLH, ublox_msgs, NavPOSLLH); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::RELPOSNED, ublox_msgs, NavRELPOSNED); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::RELPOSNED9, ublox_msgs, NavRELPOSNED9); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SBAS, ublox_msgs, NavSBAS); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SOL, ublox_msgs, NavSOL); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::PVT, ublox_msgs, NavPVT); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::PVT, ublox_msgs, NavPVT7); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SAT, ublox_msgs, NavSAT); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::STATUS, ublox_msgs, NavSTATUS); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SVIN, ublox_msgs, NavSVIN); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SVINFO, ublox_msgs, NavSVINFO); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::TIMEGPS, ublox_msgs, NavTIMEGPS); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::TIMEUTC, ublox_msgs, NavTIMEUTC); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::VELECEF, ublox_msgs, NavVELECEF); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::VELNED, ublox_msgs, NavVELNED); + +// ACK messages are declared differently because they both have the same // protocol, so only 1 ROS message is used -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ACK, ublox_msgs::Message::ACK::NACK, - ublox_msgs, Ack); -DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::ACK, ublox_msgs::Message::ACK::ACK, - ublox_msgs, Ack, ACK); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ACK, ublox_msgs::Message::ACK::NACK, ublox_msgs, Ack); +DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::ACK, ublox_msgs::Message::ACK::ACK, ublox_msgs, Ack, ACK); -// INF messages are declared differently because they all have the same +// INF messages are declared differently because they all have the same // protocol, so only 1 ROS message is used. DECLARE_UBLOX_MESSAGE can only // be called once, and DECLARE_UBLOX_MESSAGE_ID is called for the following // messages -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::INF, ublox_msgs::Message::INF::ERROR, - ublox_msgs, Inf); -DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, - ublox_msgs::Message::INF::WARNING, - ublox_msgs, Inf, WARNING); -DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, - ublox_msgs::Message::INF::NOTICE, - ublox_msgs, Inf, NOTICE); -DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, - ublox_msgs::Message::INF::TEST, - ublox_msgs, Inf, TEST); -DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, - ublox_msgs::Message::INF::DEBUG, - ublox_msgs, Inf, DEBUG); - -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::ALM, - ublox_msgs, RxmALM); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::EPH, - ublox_msgs, RxmEPH); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::RAW, - ublox_msgs, RxmRAW); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::RAWX, - ublox_msgs, RxmRAWX); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::RTCM, - ublox_msgs, RxmRTCM); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::SFRB, - ublox_msgs, RxmSFRB); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::SFRBX, - ublox_msgs, RxmSFRBX); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::SVSI, - ublox_msgs, RxmSVSI); - -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::ANT, - ublox_msgs, CfgANT); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::CFG, - ublox_msgs, CfgCFG); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::DAT, - ublox_msgs, CfgDAT); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::DGNSS, - ublox_msgs, CfgDGNSS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::GNSS, - ublox_msgs, CfgGNSS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::HNR, - ublox_msgs, CfgHNR); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::INF, - ublox_msgs, CfgINF); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::MSG, - ublox_msgs, CfgMSG); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NAV5, - ublox_msgs, CfgNAV5); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NAVX5, - ublox_msgs, CfgNAVX5); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NMEA, - ublox_msgs, CfgNMEA); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NMEA, - ublox_msgs, CfgNMEA6); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NMEA, - ublox_msgs, CfgNMEA7); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::PRT, - ublox_msgs, CfgPRT); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::RATE, - ublox_msgs, CfgRATE); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::RST, - ublox_msgs, CfgRST); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::TMODE3, - ublox_msgs, CfgTMODE3); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::USB, - ublox_msgs, CfgUSB); - -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::UPD, ublox_msgs::Message::UPD::SOS, - ublox_msgs, UpdSOS); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::INF, ublox_msgs::Message::INF::ERROR, ublox_msgs, Inf); +DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, ublox_msgs::Message::INF::WARNING, ublox_msgs, Inf, WARNING); +DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, ublox_msgs::Message::INF::NOTICE, ublox_msgs, Inf, NOTICE); +DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, ublox_msgs::Message::INF::TEST, ublox_msgs, Inf, TEST); +DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, ublox_msgs::Message::INF::DEBUG, ublox_msgs, Inf, DEBUG); + +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::ALM, ublox_msgs, RxmALM); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::EPH, ublox_msgs, RxmEPH); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::RAW, ublox_msgs, RxmRAW); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::RAWX, ublox_msgs, RxmRAWX); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::RTCM, ublox_msgs, RxmRTCM); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::SFRB, ublox_msgs, RxmSFRB); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::SFRBX, ublox_msgs, RxmSFRBX); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::SVSI, ublox_msgs, RxmSVSI); + +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::ANT, ublox_msgs, CfgANT); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::CFG, ublox_msgs, CfgCFG); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::DAT, ublox_msgs, CfgDAT); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::DGNSS, ublox_msgs, CfgDGNSS); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::GNSS, ublox_msgs, CfgGNSS); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::HNR, ublox_msgs, CfgHNR); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::INF, ublox_msgs, CfgINF); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::MSG, ublox_msgs, CfgMSG); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NAV5, ublox_msgs, CfgNAV5); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NAVX5, ublox_msgs, CfgNAVX5); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NMEA, ublox_msgs, CfgNMEA); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NMEA, ublox_msgs, CfgNMEA6); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NMEA, ublox_msgs, CfgNMEA7); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::PRT, ublox_msgs, CfgPRT); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::RATE, ublox_msgs, CfgRATE); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::RST, ublox_msgs, CfgRST); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::TMODE3, ublox_msgs, CfgTMODE3); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::USB, ublox_msgs, CfgUSB); + +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::UPD, ublox_msgs::Message::UPD::SOS, ublox_msgs, UpdSOS); // SOS and SOS_Ack have the same message ID, but different lengths -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::UPD, ublox_msgs::Message::UPD::SOS, - ublox_msgs, UpdSOS_Ack); - -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::GNSS, - ublox_msgs, MonGNSS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::HW, - ublox_msgs, MonHW); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::HW, - ublox_msgs, MonHW6); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::VER, - ublox_msgs, MonVER); - -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::ALM, - ublox_msgs, AidALM); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::EPH, - ublox_msgs, AidEPH); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::HUI, - ublox_msgs, AidHUI); - -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::INS, - ublox_msgs, EsfINS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::MEAS, - ublox_msgs, EsfMEAS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::RAW, - ublox_msgs, EsfRAW); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::STATUS, - ublox_msgs, EsfSTATUS); - - -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MGA, ublox_msgs::Message::MGA::GAL, - ublox_msgs, MgaGAL); - -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::HNR, ublox_msgs::Message::HNR::PVT, - ublox_msgs, HnrPVT); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::UPD, ublox_msgs::Message::UPD::SOS, ublox_msgs, UpdSOS_Ack); -// TIM messages -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::TIM, ublox_msgs::Message::TIM::TM2, - ublox_msgs, TimTM2); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::GNSS, ublox_msgs, MonGNSS); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::HW, ublox_msgs, MonHW); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::HW, ublox_msgs, MonHW6); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::VER, ublox_msgs, MonVER); + +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::ALM, ublox_msgs, AidALM); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::EPH, ublox_msgs, AidEPH); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::HUI, ublox_msgs, AidHUI); + +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::INS, ublox_msgs, EsfINS); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::MEAS, ublox_msgs, EsfMEAS); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::RAW, ublox_msgs, EsfRAW); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::STATUS, ublox_msgs, EsfSTATUS); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MGA, ublox_msgs::Message::MGA::GAL, ublox_msgs, MgaGAL); + +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::HNR, ublox_msgs::Message::HNR::PVT, ublox_msgs, HnrPVT); + +// TIM messages +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::TIM, ublox_msgs::Message::TIM::TM2, ublox_msgs, TimTM2); diff --git a/ublox_serialization/include/ublox/checksum.h b/ublox_serialization/include/ublox/checksum.h index 38041390..5b271129 100644 --- a/ublox_serialization/include/ublox/checksum.h +++ b/ublox_serialization/include/ublox/checksum.h @@ -14,9 +14,9 @@ // 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 +// 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 BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; @@ -31,21 +31,20 @@ #include -namespace ublox { - +namespace ublox +{ /** * @brief calculate the checksum of a u-blox_message - * @param data the start of the u-blox message + * @param data the start of the u-blox message * @param data the size of the u-blox message * @param ck_a the checksum a output * @param ck_b the checksum b output */ -static inline void calculateChecksum(const uint8_t *data, - uint32_t size, - uint8_t &ck_a, - uint8_t &ck_b) { - ck_a = 0; ck_b = 0; - for(uint32_t i = 0; i < size; ++i) +static inline void calculateChecksum(const uint8_t* data, uint32_t size, uint8_t& ck_a, uint8_t& ck_b) +{ + ck_a = 0; + ck_b = 0; + for (uint32_t i = 0; i < size; ++i) { ck_a = ck_a + data[i]; ck_b = ck_b + ck_a; @@ -54,19 +53,18 @@ static inline void calculateChecksum(const uint8_t *data, /** * @brief calculate the checksum of a u-blox_message. - * @param data the start of the u-blox message + * @param data the start of the u-blox message * @param data the size of the u-blox message * @param checksum the checksum output * @return the checksum */ -static inline uint16_t calculateChecksum(const uint8_t *data, - uint32_t size, - uint16_t &checksum) { - uint8_t *byte = reinterpret_cast(&checksum); +static inline uint16_t calculateChecksum(const uint8_t* data, uint32_t size, uint16_t& checksum) +{ + uint8_t* byte = reinterpret_cast(&checksum); calculateChecksum(data, size, byte[0], byte[1]); return checksum; } -} // namespace ublox +} // namespace ublox -#endif // UBLOX_MSGS_CHECKSUM_H +#endif // UBLOX_MSGS_CHECKSUM_H diff --git a/ublox_serialization/include/ublox/serialization.h b/ublox_serialization/include/ublox/serialization.h index d664d626..30ffd914 100644 --- a/ublox_serialization/include/ublox/serialization.h +++ b/ublox_serialization/include/ublox/serialization.h @@ -14,9 +14,9 @@ // 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 +// 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 BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; @@ -39,62 +39,59 @@ /// /// This file defines the Serializer template class which encodes and decodes -/// specific message types. -/// The Reader class decodes messages and from a buffer and the Writer class +/// specific message types. +/// The Reader class decodes messages and from a buffer and the Writer class /// encodes messages and writes them to a buffer. /// It also declares macros for declaring Messages. The Message class /// maps ROS messages types to class and message ID(s). /// - /** * @namespace ublox * This namespace is for u-blox message serialization. */ -namespace ublox { - +namespace ublox +{ //! u-blox message Sync A char -static const uint8_t DEFAULT_SYNC_A = 0xB5; +static const uint8_t DEFAULT_SYNC_A = 0xB5; //! u-blox message Sync B char -static const uint8_t DEFAULT_SYNC_B = 0x62; +static const uint8_t DEFAULT_SYNC_B = 0x62; //! Maximum payload length static const uint32_t kMaxPayloadLength = 8184; // == (buffer size - header length - checksum length) //! Number of bytes in a message header (Sync chars + class ID + message ID) -static const uint8_t kHeaderLength = 6; +static const uint8_t kHeaderLength = 6; //! Number of checksum bytes in the u-blox message -static const uint8_t kChecksumLength = 2; +static const uint8_t kChecksumLength = 2; /** * @brief Encodes and decodes messages. */ template -struct Serializer { +struct Serializer +{ /** * @brief Decode the message payload from the data buffer. * @param data a pointer to the start of the message payload * @param count the number of bytes in the message payload * @param message the output message */ - static void read(const uint8_t *data, uint32_t count, - typename boost::call_traits::reference message); + static void read(const uint8_t* data, uint32_t count, typename boost::call_traits::reference message); /** * @brief Get the length of the message payload in bytes. - * + * * @details The payload does not include the header or checksum. * @param message the message to get the length of * @return the length of the message in bytes. */ - static uint32_t serializedLength( - typename boost::call_traits::param_type message); - + static uint32_t serializedLength(typename boost::call_traits::param_type message); + /** * @brief Encode the message payload as a byte array. * @param data a buffer to fill with the message payload bytes * @param size the length of the buffer * @param message the output message */ - static void write(uint8_t *data, uint32_t size, - typename boost::call_traits::param_type message); + static void write(uint8_t* data, uint32_t size, typename boost::call_traits::param_type message); }; /** @@ -102,86 +99,98 @@ struct Serializer { * message type. */ template -class Message { - public: +class Message +{ +public: /** * @brief Can this message type decode a u-blox message with the given ID? * @param class_id the class ID of the u-blox message * @param message_id the message ID of the u-blox message * @return whether or not this message type decode the u-blox message */ - static bool canDecode(uint8_t class_id, uint8_t message_id) { - return std::find(keys_.begin(), keys_.end(), - std::make_pair(class_id, message_id)) != keys_.end(); + static bool canDecode(uint8_t class_id, uint8_t message_id) + { + return std::find(keys_.begin(), keys_.end(), std::make_pair(class_id, message_id)) != keys_.end(); } - + /** - * @brief Indicate that this message type can decode u-blox messages with the + * @brief Indicate that this message type can decode u-blox messages with the * given ID * @param class_id the class ID of the u-blox message * @param message_id the message ID of the u-blox message */ - static void addKey(uint8_t class_id, uint8_t message_id) { + static void addKey(uint8_t class_id, uint8_t message_id) + { keys_.push_back(std::make_pair(class_id, message_id)); } struct StaticKeyInitializer { - StaticKeyInitializer(uint8_t class_id, uint8_t message_id) { - Message::addKey(class_id, message_id); + StaticKeyInitializer(uint8_t class_id, uint8_t message_id) + { + Message::addKey(class_id, message_id); } }; - private: - static std::vector > keys_; +private: + static std::vector > keys_; }; /** * @brief Options for the Reader and Writer for encoding and decoding messages. */ -struct Options { +struct Options +{ /** * The default options for a u-blox message. */ - Options() : sync_a(DEFAULT_SYNC_A), sync_b(DEFAULT_SYNC_B), - max_payload_length(kMaxPayloadLength), - header_length(kHeaderLength), checksum_length(kChecksumLength) {} + Options() + : sync_a(DEFAULT_SYNC_A) + , sync_b(DEFAULT_SYNC_B) + , max_payload_length(kMaxPayloadLength) + , header_length(kHeaderLength) + , checksum_length(kChecksumLength) + { + } //! The sync_a byte value identifying the start of a message - uint8_t sync_a; + uint8_t sync_a; //! The sync_b byte value identifying the start of a message - uint8_t sync_b; + uint8_t sync_b; //! The maximum payload length. uint32_t max_payload_length; //! The length of the message header in bytes (everything before the payload) - uint8_t header_length; + uint8_t header_length; //! The length of the checksum in bytes - uint8_t checksum_length; - + uint8_t checksum_length; + /** * @brief Get the number of bytes in the header and footer. * @return the number of bytes in the header and footer */ - int wrapper_length() { - return header_length + checksum_length; + int wrapper_length() + { + return header_length + checksum_length; } }; -/** +/** * @brief Decodes byte messages into u-blox ROS messages. */ -class Reader { - public: +class Reader +{ +public: /** * @param data a buffer containing u-blox messages * @param count the size of the buffer - * @param options A struct containing the parameters sync_a and sync_b + * @param options A struct containing the parameters sync_a and sync_b * which represent the sync bytes indicating the beginning of the message */ - Reader(const uint8_t *data, uint32_t count, - const Options &options = Options()) : - data_(data), count_(count), found_(false), options_(options) {} + Reader(const uint8_t* data, uint32_t count, const Options& options = Options()) + : data_(data), count_(count), found_(false), options_(options) + { + } - typedef const uint8_t *iterator; + typedef const uint8_t* iterator; /** * @brief Search the buffer for the beginning of the next u-blox message @@ -189,19 +198,24 @@ class Reader { */ iterator search() { - if (found_) next(); + if (found_) + next(); // Search for a message header - for( ; count_ > 0; --count_, ++data_) { - if (data_[0] == options_.sync_a && - (count_ == 1 || data_[1] == options_.sync_b)) { + for (; count_ > 0; --count_, ++data_) + { + if (data_[0] == options_.sync_a && (count_ == 1 || data_[1] == options_.sync_b)) + { // Ignore messages which exceed the maximum payload length - if (length() > options_.max_payload_length) { + if (length() > options_.max_payload_length) + { // Message exceeds maximum payload length ROS_ERROR("U-Blox message exceeds maximum payload length %u: " - "0x%02x / 0x%02x", options_.max_payload_length, - classId(), messageId()); - continue; + "0x%02x / 0x%02x", + options_.max_payload_length, + classId(), + messageId()); + continue; } break; } @@ -216,31 +230,37 @@ class Reader { */ bool found() { - if (found_) return true; + if (found_) + return true; // Verify message is long enough to have sync chars, id, length & checksum - if (count_ < options_.wrapper_length()) return false; + if (count_ < options_.wrapper_length()) + return false; // Verify the header bits - if (data_[0] != options_.sync_a || data_[1] != options_.sync_b) + if (data_[0] != options_.sync_a || data_[1] != options_.sync_b) return false; // Verify that the buffer length is long enough based on the received // message length - if (count_ < length() + options_.wrapper_length()) return false; + if (count_ < length() + options_.wrapper_length()) + return false; found_ = true; return true; } /** - * @brief Go to the start of the next message based on the received message + * @brief Go to the start of the next message based on the received message * length. * - * @details Warning: Does not go to the correct byte location if the received + * @details Warning: Does not go to the correct byte location if the received * message length is incorrect. If this is the case, search must be called. */ - iterator next() { - if (found()) { + iterator next() + { + if (found()) + { uint32_t size = length() + options_.wrapper_length(); - data_ += size; count_ -= size; + data_ += size; + count_ -= size; } found_ = false; return data_; @@ -250,16 +270,24 @@ class Reader { * @brief Get the current position in the read buffer. * @return the current position of the read buffer */ - iterator pos() { + iterator pos() + { return data_; } - iterator end() { + iterator end() + { return data_ + count_; } - uint8_t classId() { return data_[2]; } - uint8_t messageId() { return data_[3]; } + uint8_t classId() + { + return data_[2]; + } + uint8_t messageId() + { + return data_[3]; + } /** * @brief Get the length of the u-blox message payload. @@ -268,20 +296,25 @@ class Reader { * Determines the length from the header of the u-blox message. * @return the length of the message payload */ - uint32_t length() { - if (count_ < 6) return 0u; + uint32_t length() + { + if (count_ < 6) + return 0u; return (data_[5] << 8) + data_[4]; } - const uint8_t *data() { return data_ + options_.header_length; } - + const uint8_t* data() + { + return data_ + options_.header_length; + } + /** * @brief Get the checksum of the u-blox message. * * @return the checksum of the u-blox message */ - uint16_t checksum() { - return *reinterpret_cast(data_ + options_.header_length + - length()); + uint16_t checksum() + { + return *reinterpret_cast(data_ + options_.header_length + length()); } /** @@ -290,17 +323,20 @@ class Reader { * @param search whether or not to skip to the next message in the buffer */ template - bool read(typename boost::call_traits::reference message, - bool search = false) { - if (search) this->search(); - if (!found()) return false; - if (!Message::canDecode(classId(), messageId())) return false; + bool read(typename boost::call_traits::reference message, bool search = false) + { + if (search) + this->search(); + if (!found()) + return false; + if (!Message::canDecode(classId(), messageId())) + return false; uint16_t chk; - if (calculateChecksum(data_ + 2, length() + 4, chk) != this->checksum()) { + if (calculateChecksum(data_ + 2, length() + 4, chk) != this->checksum()) + { // checksum error - ROS_DEBUG("U-Blox read checksum error: 0x%02x / 0x%02x", classId(), - messageId()); + ROS_DEBUG("U-Blox read checksum error: 0x%02x / 0x%02x", classId(), messageId()); return false; } @@ -310,42 +346,47 @@ class Reader { /** * @brief Can the given message type decode the current message in the buffer? - * @return whether the given message type can decode the current message in + * @return whether the given message type can decode the current message in * the buffer */ - template - bool hasType() { - if (!found()) return false; + template + bool hasType() + { + if (!found()) + return false; return Message::canDecode(classId(), messageId()); } /** * @brief Does the u-blox message have the given class and message ID? - * @return Whether or not the u-blox message has the given class and message + * @return Whether or not the u-blox message has the given class and message * ID */ - bool isMessage(uint8_t class_id, uint8_t message_id) { - if (!found()) return false; + bool isMessage(uint8_t class_id, uint8_t message_id) + { + if (!found()) + return false; return (classId() == class_id && messageId() == message_id); } - private: +private: //! The buffer of message bytes - const uint8_t *data_; + const uint8_t* data_; //! the number of bytes in the buffer, //! decrement as the buffer is read - uint32_t count_; + uint32_t count_; //! Whether or not a message has been found - bool found_; + bool found_; //! Options representing the sync char values, etc. - Options options_; + Options options_; }; -/** +/** * @brief Encodes a u-blox ROS message as a byte array. */ -class Writer { - public: - typedef uint8_t *iterator; +class Writer +{ +public: + typedef uint8_t* iterator; /** * @brief Construct a Writer with the given buffer. @@ -353,8 +394,9 @@ class Writer { * @param size the size of the buffer * @param options options representing the message sync chars, etc. */ - Writer(uint8_t *data, uint32_t size, const Options &options = Options()) : - data_(data), size_(size), options_(options) {} + Writer(uint8_t* data, uint32_t size, const Options& options = Options()) : data_(data), size_(size), options_(options) + { + } /** * @brief Encode the u-blox message. @@ -363,19 +405,18 @@ class Writer { * @param message_id the u-blox message ID, defaults to the message MESSAGE_ID * @return true if the message was encoded correctly, false otherwise */ - template bool write(const T& message, - uint8_t class_id = T::CLASS_ID, - uint8_t message_id = T::MESSAGE_ID) { + template + bool write(const T& message, uint8_t class_id = T::CLASS_ID, uint8_t message_id = T::MESSAGE_ID) + { // Check for buffer overflow uint32_t length = Serializer::serializedLength(message); - if (size_ < length + options_.wrapper_length()) { - ROS_ERROR("u-blox write buffer overflow. Message %u / %u not written", - class_id, message_id); + if (size_ < length + options_.wrapper_length()) + { + ROS_ERROR("u-blox write buffer overflow. Message %u / %u not written", class_id, message_id); return false; } // Encode the message and add it to the buffer - Serializer::write(data_ + options_.header_length, - size_ - options_.header_length, message); + Serializer::write(data_ + options_.header_length, size_ - options_.header_length, message); return write(0, length, class_id, message_id); } @@ -388,11 +429,11 @@ class Writer { * @param message_id the u-blox message ID * @return true if the message was encoded correctly, false otherwise */ - bool write(const uint8_t* message, uint32_t length, uint8_t class_id, - uint8_t message_id) { - if (size_ < length + options_.wrapper_length()) { - ROS_ERROR("u-blox write buffer overflow. Message %u / %u not written", - class_id, message_id); + bool write(const uint8_t* message, uint32_t length, uint8_t class_id, uint8_t message_id) + { + if (size_ < length + options_.wrapper_length()) + { + ROS_ERROR("u-blox write buffer overflow. Message %u / %u not written", class_id, message_id); return false; } iterator start = data_; @@ -407,7 +448,8 @@ class Writer { size_ -= options_.header_length; // write message - if (message) std::copy(message, message + length, data_); + if (message) + std::copy(message, message + length, data_); data_ += length; size_ -= length; @@ -421,40 +463,48 @@ class Writer { return true; } - iterator end() { + iterator end() + { return data_; } - private: +private: //! The buffer of message bytes - iterator data_; + iterator data_; //! The number of remaining bytes in the buffer /*! Decrements as messages are written to the buffer */ - uint32_t size_; + uint32_t size_; //! Options representing the sync char values, etc. - Options options_; + Options options_; }; -} // namespace ublox +} // namespace ublox // Use to declare u-blox messages and message serializers -#define DECLARE_UBLOX_MESSAGE(class_id, message_id, package, message) \ - template class ublox::Serializer; \ - template class ublox::Message; \ - namespace package { namespace { \ - static const ublox::Message::StaticKeyInitializer static_key_initializer_##message(class_id, message_id); \ - } } \ +#define DECLARE_UBLOX_MESSAGE(class_id, message_id, package, message) \ + template class ublox::Serializer; \ + template class ublox::Message; \ + namespace package \ + { \ + namespace \ + { \ + static const ublox::Message::StaticKeyInitializer static_key_initializer_##message(class_id, message_id); \ + } \ + } // Use for messages which have the same structure but different IDs, e.g. INF // Call DECLARE_UBLOX_MESSAGE for the first message and DECLARE_UBLOX_MESSAGE_ID // for following declarations -#define DECLARE_UBLOX_MESSAGE_ID(class_id, message_id, package, message, name) \ - namespace package { namespace { \ - static const ublox::Message::StaticKeyInitializer static_key_initializer_##name(class_id, message_id); \ - } } \ - +#define DECLARE_UBLOX_MESSAGE_ID(class_id, message_id, package, message, name) \ + namespace package \ + { \ + namespace \ + { \ + static const ublox::Message::StaticKeyInitializer static_key_initializer_##name(class_id, message_id); \ + } \ + } // use implementation of class Serializer in "serialization_ros.h" #include "serialization_ros.h" -#endif // UBLOX_SERIALIZATION_H +#endif // UBLOX_SERIALIZATION_H diff --git a/ublox_serialization/include/ublox/serialization_ros.h b/ublox_serialization/include/ublox/serialization_ros.h index 9236ff46..2e3b3c0d 100644 --- a/ublox_serialization/include/ublox/serialization_ros.h +++ b/ublox_serialization/include/ublox/serialization_ros.h @@ -14,9 +14,9 @@ // 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 +// 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 BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; @@ -34,28 +34,28 @@ #include -namespace ublox { - +namespace ublox +{ template -void Serializer::read(const uint8_t *data, uint32_t count, - typename boost::call_traits::reference message) { - ros::serialization::IStream stream(const_cast(data), count); +void Serializer::read(const uint8_t* data, uint32_t count, typename boost::call_traits::reference message) +{ + ros::serialization::IStream stream(const_cast(data), count); ros::serialization::Serializer::read(stream, message); } template -uint32_t Serializer::serializedLength( - typename boost::call_traits::param_type message) { +uint32_t Serializer::serializedLength(typename boost::call_traits::param_type message) +{ return ros::serialization::Serializer::serializedLength(message); } template -void Serializer::write(uint8_t *data, uint32_t size, - typename boost::call_traits::param_type message) { +void Serializer::write(uint8_t* data, uint32_t size, typename boost::call_traits::param_type message) +{ ros::serialization::OStream stream(data, size); ros::serialization::Serializer::write(stream, message); } -} // namespace ublox +} // namespace ublox -#endif // UBLOX_SERIALIZATION_ROS_H +#endif // UBLOX_SERIALIZATION_ROS_H From 4fe569931c0e5b125e487c3c41ffc4491b5b375d Mon Sep 17 00:00:00 2001 From: Jonathan Sanabria Date: Wed, 4 May 2022 16:53:00 -0400 Subject: [PATCH 07/17] cleaning other conflict. --- ublox_gps/src/node.cpp | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 91658e71..ba4313f1 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -1525,7 +1525,6 @@ void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS& m) // t_ref_.header.seq = m.risingEdgeCount; // t_ref_.header.stamp = ros::Time::now(); // t_ref_.header.frame_id = frame_id; -<<<<<<< HEAD // t_ref_.time_ref = ros::Time((m.wnR * 604800 + m.towMsR / 1000), (m.towMsR % 1000) * 1000000 + m.towSubMsR); @@ -1533,15 +1532,6 @@ void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS& m) // src << "TIM" << int(m.ch); // t_ref_.source = src.str(); -======= - - // t_ref_.time_ref = ros::Time((m.wnR * 604800 + m.towMsR / 1000), (m.towMsR % 1000) * 1000000 + m.towSubMsR); - - // std::ostringstream src; - // src << "TIM" << int(m.ch); - // t_ref_.source = src.str(); - ->>>>>>> f6a3af656eafbf24e1a717cdaa6d91b5c2122fd0 t_ref_.header.stamp = ros::Time::now(); // create a new timestamp t_ref_.header.frame_id = frame_id; From 1ed20979023d6e45eb7ad4e8c335c4c7ef1111f8 Mon Sep 17 00:00:00 2001 From: Jonathan Sanabria Date: Wed, 4 May 2022 16:54:12 -0400 Subject: [PATCH 08/17] new line for service. --- ublox_msgs/srv/GetVersionInfo.srv | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ublox_msgs/srv/GetVersionInfo.srv b/ublox_msgs/srv/GetVersionInfo.srv index b0336a66..3e492c0f 100644 --- a/ublox_msgs/srv/GetVersionInfo.srv +++ b/ublox_msgs/srv/GetVersionInfo.srv @@ -4,4 +4,5 @@ string hardware string software string firmware string protocol -string device_model \ No newline at end of file +string device_model + From 2f5061f72a53803d7616b5cdaaaab05b53b7ab0d Mon Sep 17 00:00:00 2001 From: Jonathan Sanabria Date: Wed, 11 May 2022 11:32:44 -0400 Subject: [PATCH 09/17] increment debian for gps version info. --- ublox_gps/debian/changelog | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/ublox_gps/debian/changelog b/ublox_gps/debian/changelog index 0c96a3e7..d580da1a 100644 --- a/ublox_gps/debian/changelog +++ b/ublox_gps/debian/changelog @@ -1,3 +1,9 @@ +ros-noetic-ublox-gps (1.5.0-greenzie-focal2) focal; urgency=medium + + * Creating service for GPS device version info. + + -- Jonathan Sanabria Wed, 11 May 2022 11:30:49 -0400 + ros-noetic-ublox-gps (1.5.0-greenzie-focal1) focal; urgency=medium * Debianize From 713b87dd78a30014b4fc826647a003ab61ce1562 Mon Sep 17 00:00:00 2001 From: Kristian Kabbabe Date: Wed, 11 May 2022 12:20:40 -0400 Subject: [PATCH 10/17] Removed version constrain in debians --- ublox/debian/changelog | 6 ++++++ ublox/debian/control | 8 ++++---- ublox_gps/debian/changelog | 6 ++++++ ublox_gps/debian/control | 8 ++++---- ublox_msg_filters/debian/changelog | 6 ++++++ ublox_msg_filters/debian/control | 4 ++-- ublox_msgs/debian/changelog | 6 ++++++ ublox_msgs/debian/control | 4 ++-- ublox_serialization/debian/changelog | 6 ++++++ 9 files changed, 42 insertions(+), 12 deletions(-) diff --git a/ublox/debian/changelog b/ublox/debian/changelog index 0ee65993..65ddbddd 100644 --- a/ublox/debian/changelog +++ b/ublox/debian/changelog @@ -1,3 +1,9 @@ +ros-noetic-ublox (1.5.0-greenzie-focal2) focal; urgency=medium + + * Removed version constraint + + -- Kristian Kabbabe Wed, 11 May 2022 12:17:33 -0400 + ros-noetic-ublox (1.5.0-greenzie-focal1) focal; urgency=medium * Debianize diff --git a/ublox/debian/control b/ublox/debian/control index 4b64616f..7fbfc6fd 100644 --- a/ublox/debian/control +++ b/ublox/debian/control @@ -11,8 +11,8 @@ Package: ros-noetic-ublox Architecture: any Depends: ${shlibs:Depends}, ${misc:Depends}, - ros-noetic-ublox-gps (= ${binary:Version}), - ros-noetic-ublox-msgs (= ${binary:Version}), - ros-noetic-ublox-msg-filters (= ${binary:Version}), - ros-noetic-ublox-serialization (= ${binary:Version}) + ros-noetic-ublox-gps, + ros-noetic-ublox-msgs, + ros-noetic-ublox-msg-filters, + ros-noetic-ublox-serialization Description: Provides a ublox_gps node for u-blox GPS receivers, messages, and serialization packages for the binary UBX protocol. diff --git a/ublox_gps/debian/changelog b/ublox_gps/debian/changelog index d580da1a..9df98220 100644 --- a/ublox_gps/debian/changelog +++ b/ublox_gps/debian/changelog @@ -1,3 +1,9 @@ +ros-noetic-ublox-gps (1.5.0-greenzie-focal3) focal; urgency=medium + + * Removed version constraint + + -- Kristian Kabbabe Wed, 11 May 2022 12:16:56 -0400 + ros-noetic-ublox-gps (1.5.0-greenzie-focal2) focal; urgency=medium * Creating service for GPS device version info. diff --git a/ublox_gps/debian/control b/ublox_gps/debian/control index ed12bd94..f5e28162 100644 --- a/ublox_gps/debian/control +++ b/ublox_gps/debian/control @@ -9,8 +9,8 @@ Build-Depends: debhelper (>= 9.0.0), ros-noetic-roscpp-serialization, ros-noetic-rtcm-msgs, ros-noetic-tf, - ros-noetic-ublox-msgs (= ${binary:Version}), - ros-noetic-ublox-serialization (= ${binary:Version}) + ros-noetic-ublox-msgs, + ros-noetic-ublox-serialization Homepage: http://ros.org/wiki/ublox Standards-Version: 3.9.2 @@ -23,6 +23,6 @@ Depends: ${shlibs:Depends}, ros-noetic-roscpp-serialization, ros-noetic-rtcm-msgs, ros-noetic-tf, - ros-noetic-ublox-msgs (= ${binary:Version}), - ros-noetic-ublox-serialization (= ${binary:Version}) + ros-noetic-ublox-msgs, + ros-noetic-ublox-serialization Description: Driver for u-blox GPS devices. diff --git a/ublox_msg_filters/debian/changelog b/ublox_msg_filters/debian/changelog index 897c098b..6335ba60 100644 --- a/ublox_msg_filters/debian/changelog +++ b/ublox_msg_filters/debian/changelog @@ -1,3 +1,9 @@ +ros-noetic-ublox-msg-filters (1.5.0-greenzie-focal2) focal; urgency=medium + + * Removed version constraint + + -- Kristian Kabbabe Wed, 11 May 2022 12:17:15 -0400 + ros-noetic-ublox-msg-filters (1.5.0-greenzie-focal1) focal; urgency=medium * Debianize diff --git a/ublox_msg_filters/debian/control b/ublox_msg_filters/debian/control index d4066650..7776619a 100644 --- a/ublox_msg_filters/debian/control +++ b/ublox_msg_filters/debian/control @@ -8,7 +8,7 @@ Build-Depends: debhelper (>= 9.0.0), ros-noetic-roscpp, ros-noetic-roslaunch, ros-noetic-rospy, - ros-noetic-ublox-msgs (= ${binary:Version}) + ros-noetic-ublox-msgs Homepage: http://ros.org/wiki/ublox Standards-Version: 3.9.2 @@ -17,5 +17,5 @@ Architecture: any Depends: ${shlibs:Depends}, ${misc:Depends}, ros-noetic-message-filters, - ros-noetic-ublox-msgs (= ${binary:Version}) + ros-noetic-ublox-msgs Description: Time synchronize multiple uBlox messages to get a single callback diff --git a/ublox_msgs/debian/changelog b/ublox_msgs/debian/changelog index 70c64dcb..ec68ecdc 100644 --- a/ublox_msgs/debian/changelog +++ b/ublox_msgs/debian/changelog @@ -1,3 +1,9 @@ +ros-noetic-ublox-msgs (1.5.0-greenzie-focal2) focal; urgency=medium + + * Removed version constraint + + -- Kristian Kabbabe Wed, 11 May 2022 12:17:21 -0400 + ros-noetic-ublox-msgs (1.5.0-greenzie-focal1) focal; urgency=medium * Debianize diff --git a/ublox_msgs/debian/control b/ublox_msgs/debian/control index 76de3f26..03f15ee9 100644 --- a/ublox_msgs/debian/control +++ b/ublox_msgs/debian/control @@ -7,7 +7,7 @@ Build-Depends: debhelper (>= 9.0.0), ros-noetic-message-generation, ros-noetic-sensor-msgs, ros-noetic-std-msgs, - ros-noetic-ublox-serialization (= ${binary:Version}) + ros-noetic-ublox-serialization Homepage: http://ros.org/wiki/ublox Standards-Version: 3.9.2 @@ -18,5 +18,5 @@ Depends: ${shlibs:Depends}, ros-noetic-message-runtime, ros-noetic-sensor-msgs, ros-noetic-std-msgs, - ros-noetic-ublox-serialization (= ${binary:Version}) + ros-noetic-ublox-serialization Description: ublox_msgs contains raw messages for u-blox GNSS devices. diff --git a/ublox_serialization/debian/changelog b/ublox_serialization/debian/changelog index cf481aff..6b3dd2e5 100644 --- a/ublox_serialization/debian/changelog +++ b/ublox_serialization/debian/changelog @@ -1,3 +1,9 @@ +ros-noetic-ublox-serialization (1.5.0-greenzie-focal2) focal; urgency=medium + + * Removed version constraint + + -- Kristian Kabbabe Wed, 11 May 2022 12:17:26 -0400 + ros-noetic-ublox-serialization (1.5.0-greenzie-focal1) focal; urgency=medium * Debianize From 13b5f120ee846ee95778290d1b27190966d61242 Mon Sep 17 00:00:00 2001 From: Jonathan Sanabria Date: Thu, 16 Jun 2022 20:42:08 -0400 Subject: [PATCH 11/17] updating service name to poll_version. --- ublox_gps/src/node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index ba4313f1..89915931 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -657,7 +657,7 @@ void UbloxNode::initialize() // Params must be set before initializing IO getRosParams(); initializeIo(); - ros::ServiceServer service = nh->advertiseService("poll_gps_version", &UbloxNode::getVersionInfo, this); + ros::ServiceServer service = nh->advertiseService("poll_version", &UbloxNode::getVersionInfo, this); // Must process Mon VER before setting firmware/hardware params processMonVer(); if (protocol_version_ <= 14) From b5a8dd619daadb4d87f42c50f62e436e4bd14492 Mon Sep 17 00:00:00 2001 From: Jonathan Sanabria Date: Thu, 16 Jun 2022 20:43:58 -0400 Subject: [PATCH 12/17] debian bump. --- ublox_gps/debian/changelog | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/ublox_gps/debian/changelog b/ublox_gps/debian/changelog index 9df98220..bec56f10 100644 --- a/ublox_gps/debian/changelog +++ b/ublox_gps/debian/changelog @@ -1,3 +1,9 @@ +ros-noetic-ublox-gps (1.5.0-greenzie-focal4) focal; urgency=medium + + * Updating service name to poll_version. + + -- Jonathan Sanabria Thu, 16 Jun 2022 20:43:35 -0400 + ros-noetic-ublox-gps (1.5.0-greenzie-focal3) focal; urgency=medium * Removed version constraint From 650aa9f884794142c5d2ce5719d9ab98840d8888 Mon Sep 17 00:00:00 2001 From: taylorskalyo Date: Wed, 28 Feb 2024 14:43:53 -0500 Subject: [PATCH 13/17] Add changelog for ublox_msg_filters --- ublox_msg_filters/CHANGELOG.rst | 8 ++++++++ 1 file changed, 8 insertions(+) create mode 100644 ublox_msg_filters/CHANGELOG.rst diff --git a/ublox_msg_filters/CHANGELOG.rst b/ublox_msg_filters/CHANGELOG.rst new file mode 100644 index 00000000..fd889b7f --- /dev/null +++ b/ublox_msg_filters/CHANGELOG.rst @@ -0,0 +1,8 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package ublox_msg_filters +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.0 (2022-02-21) +------------------ +* ublox: first commit +* Contributors: Kevin Hallenbeck From 46a35b9420f32b10f19ce9d410dba5884734f5ab Mon Sep 17 00:00:00 2001 From: taylorskalyo Date: Wed, 28 Feb 2024 14:34:14 -0500 Subject: [PATCH 14/17] Add to CircleCI * Initial circleci.yml file --- .circleci/config.yml | 55 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 55 insertions(+) create mode 100644 .circleci/config.yml diff --git a/.circleci/config.yml b/.circleci/config.yml new file mode 100644 index 00000000..3fe6b4c2 --- /dev/null +++ b/.circleci/config.yml @@ -0,0 +1,55 @@ +version: 2.1 + +workflows: + version: 2 + pushbuild: + jobs: + - debian_build + - deploy: + filters: + branches: + only: + - noetic + requires: + - debian_build + +jobs: + debian_build: + docker: + - image: greenzie/debianize-experimental:focal-noetic + auth: + username: $DOCKERHUB_GREENZIE_USERNAME + password: $DOCKERHUB_GREENZIE_PASSWORD + resource_class: small + steps: + - checkout + - run: + name: "Generate Changelogs" + command: greenzie-release changelog -r "${ROS_DISTRO}" + - run: + name: "Package debs (debuild)" + command: GITHUB_WORKSPACE="$PWD" greenzie-debianize all + - store_artifacts: + path: /debians + - persist_to_workspace: + root: /debians + paths: + - '*' + + deploy: + docker: + - image: greenzie/aptly:3.19.1-focal-noetic + auth: + username: $DOCKERHUB_GREENZIE_USERNAME + password: $DOCKERHUB_GREENZIE_PASSWORD + resource_class: small + steps: + - add_ssh_keys + - attach_workspace: + at: /debians + - run: + name: Deploy to aptly.greenzie.com + environment: + SERVER_HOSTNAME: aptly.greenzie.com + command: | + aptly repo deploy -r experimental -n "${CIRCLE_BUILD_NUM}" -p "/debians" From 9fcd0bf274eff05c3ffcf479483bef16eaf965c3 Mon Sep 17 00:00:00 2001 From: Esteban Martinena Date: Tue, 25 Jun 2024 13:31:21 +0200 Subject: [PATCH 15/17] Last CI template --- .circleci/config.yml | 121 +++++++++++++++++++++++++++---------------- 1 file changed, 76 insertions(+), 45 deletions(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index 3fe6b4c2..2b740e29 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -1,55 +1,86 @@ version: 2.1 +orbs: + greenzie: greenzie/build_and_publish_debs@0.0.7 + +parameters: + manual-bobcat-deploy: + type: boolean + default: false + # Use this parameter to change the branch that the deployment jobs are triggered on + # It's useful for testing the deployment jobs on a different branch before merging + filter-branch: + type: string + default: noetic + workflows: version: 2 pushbuild: + unless: << pipeline.parameters.manual-bobcat-deploy >> jobs: - - debian_build - - deploy: + # Build jobs + - greenzie/debian_build: + name: "amd_debian_build" + platform: amd64 + executor: greenzie/debianize_amd64 + + - greenzie/debian_build: + name: "arm64_debian_build" + platform: arm64 + executor: greenzie/debianize_arm64 + + # Deployment jobs + - greenzie/debian_deploy_experimental: + name: "deploy_amd64_greenzie" + context: GREENZIE + filters: + branches: + only: + - << pipeline.parameters.filter-branch >> + requires: + - amd_debian_build + + - greenzie/debian_deploy_experimental: + name: "deploy_arm64_greenzie" + context: GREENZIE + architecture: arm64 filters: branches: only: - - noetic + - << pipeline.parameters.filter-branch >> + requires: + - arm64_debian_build + + - greenzie/debian_deploy_experimental: + name: "deploy_all_bobcat" + context: GREENZIE + server_hostname: bobcat + filters: + branches: + only: + - << pipeline.parameters.filter-branch >> + requires: + - arm64_debian_build + - amd_debian_build + + manualdeploy: + when: << pipeline.parameters.manual-bobcat-deploy >> + jobs: + # Build jobs + - greenzie/debian_build: + name: "amd_debian_build" + platform: amd64 + executor: greenzie/debianize_amd64 + + - greenzie/debian_build: + name: "arm64_debian_build" + platform: arm64 + executor: greenzie/debianize_arm64 + + - greenzie/debian_deploy_experimental: + context: GREENZIE + name: "manual_deploy_all_bobcat" + server_hostname: bobcat requires: - - debian_build - -jobs: - debian_build: - docker: - - image: greenzie/debianize-experimental:focal-noetic - auth: - username: $DOCKERHUB_GREENZIE_USERNAME - password: $DOCKERHUB_GREENZIE_PASSWORD - resource_class: small - steps: - - checkout - - run: - name: "Generate Changelogs" - command: greenzie-release changelog -r "${ROS_DISTRO}" - - run: - name: "Package debs (debuild)" - command: GITHUB_WORKSPACE="$PWD" greenzie-debianize all - - store_artifacts: - path: /debians - - persist_to_workspace: - root: /debians - paths: - - '*' - - deploy: - docker: - - image: greenzie/aptly:3.19.1-focal-noetic - auth: - username: $DOCKERHUB_GREENZIE_USERNAME - password: $DOCKERHUB_GREENZIE_PASSWORD - resource_class: small - steps: - - add_ssh_keys - - attach_workspace: - at: /debians - - run: - name: Deploy to aptly.greenzie.com - environment: - SERVER_HOSTNAME: aptly.greenzie.com - command: | - aptly repo deploy -r experimental -n "${CIRCLE_BUILD_NUM}" -p "/debians" + - arm64_debian_build + - amd_debian_build From 103592170c02d3ee19e6e8ea30f7a41823e20940 Mon Sep 17 00:00:00 2001 From: Esteban Martinena Date: Tue, 25 Jun 2024 13:37:12 +0200 Subject: [PATCH 16/17] Disable arm64 build and deploy --- .circleci/config.yml | 66 ++++++++++++++++++++++++++++++-------------- 1 file changed, 46 insertions(+), 20 deletions(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index 2b740e29..3dded0b5 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -1,5 +1,31 @@ version: 2.1 +# NOTE: ARM64 build and deployment are intentionally disabled until we fix this error +# ################################################################################ Debianization ################################################################################ +################################################################################################################################################################################ +#Package: 'ublox' +# - Build Depends.................................................................................... [SUCCESS] (6.829 seconds) +# - Build............................................................................................ [SUCCESS] (5.537 seconds) +# - Install.......................................................................................... [FAILED] (1.793 seconds) +#Error Below: +#################################################################################### STDOUT #################################################################################### +#Reading package lists... +#Building dependency tree... +#Reading state information... +#Some packages could not be installed. This may mean that you have +#requested an impossible situation or if you are using the unstable +#distribution that some required packages have not yet been created +#or been moved out of Incoming. +#The following information may help to resolve the situation: +# +#The following packages have unmet dependencies: +# ros-noetic-ublox : Depends: ros-noetic-ublox-msg-filters but it is not installable +#################################################################################### STDERR #################################################################################### +#E: Unable to correct problems, you have held broken packages. +################################################################################################################################################################################ +#CalledProcessError | Command 'apt-get install -y --allow-downgrades -o "DPkg::Lock::Timeout=-1" ./*ublox*.deb' returned non-zero exit status 100. + + orbs: greenzie: greenzie/build_and_publish_debs@0.0.7 @@ -24,10 +50,10 @@ workflows: platform: amd64 executor: greenzie/debianize_amd64 - - greenzie/debian_build: - name: "arm64_debian_build" - platform: arm64 - executor: greenzie/debianize_arm64 +# - greenzie/debian_build: +# name: "arm64_debian_build" +# platform: arm64 +# executor: greenzie/debianize_arm64 # Deployment jobs - greenzie/debian_deploy_experimental: @@ -40,16 +66,16 @@ workflows: requires: - amd_debian_build - - greenzie/debian_deploy_experimental: - name: "deploy_arm64_greenzie" - context: GREENZIE - architecture: arm64 - filters: - branches: - only: - - << pipeline.parameters.filter-branch >> - requires: - - arm64_debian_build +# - greenzie/debian_deploy_experimental: +# name: "deploy_arm64_greenzie" +# context: GREENZIE +# architecture: arm64 +# filters: +# branches: +# only: +# - << pipeline.parameters.filter-branch >> +# requires: +# - arm64_debian_build - greenzie/debian_deploy_experimental: name: "deploy_all_bobcat" @@ -60,7 +86,7 @@ workflows: only: - << pipeline.parameters.filter-branch >> requires: - - arm64_debian_build +# - arm64_debian_build - amd_debian_build manualdeploy: @@ -72,15 +98,15 @@ workflows: platform: amd64 executor: greenzie/debianize_amd64 - - greenzie/debian_build: - name: "arm64_debian_build" - platform: arm64 - executor: greenzie/debianize_arm64 +# - greenzie/debian_build: +# name: "arm64_debian_build" +# platform: arm64 +# executor: greenzie/debianize_arm64 - greenzie/debian_deploy_experimental: context: GREENZIE name: "manual_deploy_all_bobcat" server_hostname: bobcat requires: - - arm64_debian_build +# - arm64_debian_build - amd_debian_build From 57d311d569f864f4c5999f70659d4d3b0dab0950 Mon Sep 17 00:00:00 2001 From: Esteban Martinena Guerrero Date: Wed, 28 Aug 2024 11:25:44 +0200 Subject: [PATCH 17/17] Update orb version --- .circleci/config.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index 3dded0b5..bb890557 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -27,7 +27,7 @@ version: 2.1 orbs: - greenzie: greenzie/build_and_publish_debs@0.0.7 + greenzie: greenzie/build_and_publish_debs@volatile parameters: manual-bobcat-deploy: