Merge in open source changes prior to adding new services

.github/ISSUE_TEMPLATE/bug_report.md

@@ -0,0 +1,31 @@
+---
+name: Bug report
+about: Create a report to help us improve
+title: ''
+labels: ''
+assignees: ''
+
+---
+
+**Describe the bug**
+A clear and concise description of what the bug is.
+
+**To Reproduce**
+Steps to reproduce the behavior:
+1. Go to '...'
+2. Click on '....'
+3. Scroll down to '....'
+4. See error
+
+**Expected behavior**
+A clear and concise description of what you expected to happen.
+
+**Screenshots**
+If applicable, add screenshots to help explain your problem.
+
+**Desktop (please complete the following information):**
+ - OS: [e.g. Ubuntu 20.04]
+ - ROS Version [e.g. ROS1 Noetic]
+
+**Additional context**
+Add any other context about the problem here.

.github/ISSUE_TEMPLATE/feature_request.md

@@ -0,0 +1,20 @@
+---
+name: Feature request
+about: Suggest an idea for this project
+title: ''
+labels: ''
+assignees: ''
+
+---
+
+**Is your feature request related to a problem? Please describe.**
+A clear and concise description of what the problem is. Ex. I'm always frustrated when [...]
+
+**Describe the solution you'd like**
+A clear and concise description of what you want to happen.
+
+**Describe alternatives you've considered**
+A clear and concise description of any alternative solutions or features you've considered.
+
+**Additional context**
+Add any other context or screenshots about the feature request here.

.github/stale.yml

@@ -0,0 +1,16 @@
+# Number of days of inactivity before an issue becomes stale
+daysUntilStale: 365
+# Number of days of inactivity before a stale issue is closed
+daysUntilClose: 15
+# Issues with these labels will never be considered stale
+exemptLabels:
+  - pinned
+  - security
+# Label to use when marking an issue as stale
+staleLabel: wontfix
+# Comment to post when marking an issue as stale. Set to `false` to disable
+markComment: >
+  This issue has been automatically marked as stale because it has not had
+  recent activity. It will be closed if no further activity occurs.
+# Comment to post when closing a stale issue. Set to `false` to disable
+closeComment: true

LICENSE

@@ -0,0 +1,29 @@
+BSD 3-Clause License
+
+Copyright (c) 2022, ublox contributors
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+
+3. Neither the name of the copyright holder nor the names of its
+   contributors may be used to endorse or promote products derived from
+   this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

README.md

@@ -185,6 +185,7 @@ To publish a given u-blox message to a ROS topic, set the parameter shown below
 
 ### ESF messages
 * `publish/esf/all`: This is the default value for the `publish/esf/<message>` parameters below. It defaults to `publish/all` for **ADR/UDR devices**. Individual messages can be enabled or disabled by setting the parameters below.
+* `publish/esf/alg`: Topic `~esfalg`
 * `publish/esf/ins`: Topic `~esfins`
 * `publish/esf/meas`: Topic `~esfmeas`
 * `publish/esf/raw`: Topic `~esfraw`

ublox_gps/CMakeLists.txt

@@ -8,12 +8,13 @@ find_package(catkin REQUIRED COMPONENTS
   ublox_serialization
   diagnostic_updater
   rtcm_msgs
+  nmea_msgs
 )
 
 catkin_package(
     INCLUDE_DIRS include
     LIBRARIES ${PROJECT_NAME}
-    CATKIN_DEPENDS tf roscpp ublox_msgs ublox_serialization rtcm_msgs)
+    CATKIN_DEPENDS tf roscpp ublox_msgs ublox_serialization rtcm_msgs nmea_msgs)
 
 # include boost
 find_package(Boost REQUIRED COMPONENTS system regex thread)

ublox_gps/config/zed_f9k.yaml

@@ -0,0 +1,26 @@
+# Configuration Settings for ZED-F9K device
+# Please change these settings according as necessary
+device: /dev/ttyACM0
+frame_id: gps
+uart1:
+  baudrate: 115200
+config_on_startup: true
+rate: 20                     # in Hz
+nav_rate: 1                  # [# of measurement cycles], recommended 1 Hz, may
+dgnss_mode: 3                # Fixed mode
+dynamic_model: automotive    # automative mode
+use_adr: true                # Whether to enable dead reckoning
+
+inf:
+  all: false                 # Whether to display all INF messages in console
+
+# Enable necessary u-blox message publishers
+publish:
+  all: false
+  esf:
+    meas: true
+  nav:
+    status: true
+    pvt: true
+    posecef: false
+    hpposllh: true

ublox_gps/debian/control

@@ -10,7 +10,8 @@ Build-Depends: debhelper (>= 9.0.0),
     ros-noetic-rtcm-msgs,
     ros-noetic-tf,
     ros-noetic-ublox-msgs,
-    ros-noetic-ublox-serialization
+    ros-noetic-ublox-serialization,
+    ros-noetic-nmea-msgs
 Homepage: http://ros.org/wiki/ublox
 Standards-Version: 3.9.2
 
@@ -24,5 +25,6 @@ Depends: ${shlibs:Depends},
     ros-noetic-rtcm-msgs,
     ros-noetic-tf,
     ros-noetic-ublox-msgs,
-    ros-noetic-ublox-serialization
+    ros-noetic-ublox-serialization,
+    ros-noetic-nmea-msgs
 Description: Driver for u-blox GPS devices.

ublox_gps/include/ublox_gps/callback.h

@@ -166,6 +166,15 @@ class CallbackHandlers
         std::make_pair(std::make_pair(T::CLASS_ID, message_id), boost::shared_ptr<CallbackHandler>(handler)));
   }
 
+  /**
+   * @brief Add a callback handler for nmea messages
+   * @param callback the callback handler for the message
+   */
+  void set_nmea_callback(boost::function<void(const std::string&)> callback) {
+    boost::mutex::scoped_lock lock(callback_mutex_);
+    callback_nmea_ = callback;
+  }
+
   /**
    * @brief Calls the callback handler for the message in the reader.
    * @param reader a reader containing a u-blox message
@@ -180,6 +189,27 @@ class CallbackHandlers
       callback->second->handle(reader);
   }
 
+  /**
+   * @brief Calls the callback handler for the nmea messages in the reader.
+   * @param reader a reader containing an nmea message
+   */
+  void handle_nmea(ublox::Reader& reader) {
+    boost::mutex::scoped_lock lock(callback_mutex_);
+    if(callback_nmea_.empty())
+        return;
+
+    const std::string& buffer = reader.getUnusedData();
+    size_t nmea_start = buffer.find('$', 0);
+    size_t nmea_end = buffer.find('\n', nmea_start);
+    while(nmea_start != std::string::npos && nmea_end != std::string::npos) {
+        std::string sentence = buffer.substr(nmea_start, nmea_end - nmea_start + 1);
+        callback_nmea_(sentence);
+
+        nmea_start = buffer.find('$', nmea_end+1);
+        nmea_end = buffer.find('\n', nmea_start);
+    }
+  }
+
   /**
    * @brief Read a u-blox message of the given type.
    * @param message the received u-blox message
@@ -233,6 +263,7 @@ class CallbackHandlers
 
       handle(reader);
     }
+    handle_nmea(reader);
 
     // delete read bytes from ASIO input buffer
     std::copy(reader.pos(), reader.end(), data);
@@ -245,6 +276,9 @@ class CallbackHandlers
   // Call back handlers for u-blox messages
   Callbacks callbacks_;
   boost::mutex callback_mutex_;
+
+  //! Callback handler for nmea messages
+  boost::function<void(const std::string&)> callback_nmea_;
 };
 
 }  // namespace ublox_gps

ublox_gps/include/ublox_gps/gps.h

@@ -343,6 +343,12 @@ class Gps
   template <typename T>
   void subscribe(typename CallbackHandler_<T>::Callback callback);
 
+  /**
+   * @brief Subscribe to the given Ublox message.
+   * @param the callback handler for the message
+   */
+  void subscribe_nmea(boost::function<void(const std::string&)> callback) { callbacks_.set_nmea_callback(callback); }
+
   /**
    * @brief Subscribe to the message with the given ID. This is used for
    * messages which have the same format but different message IDs,

ublox_gps/include/ublox_gps/node.h

@@ -51,6 +51,7 @@
 #include <sensor_msgs/TimeReference.h>
 #include <sensor_msgs/Imu.h>
 #include <std_msgs/UInt8MultiArray.h>
+#include <nmea_msgs/Sentence.h>
 // Other U-Blox package includes
 #include <ublox_msgs/ublox_msgs.h>
 #include <ublox_msgs/GetVersionInfo.h>
@@ -440,6 +441,16 @@ void publish(const MessageT& m, const std::string& topic)
   publisher.publish(m);
 }
 
+void publish_nmea(const std::string& sentence, const std::string& topic) {
+  static ros::Publisher publisher = nh->advertise<nmea_msgs::Sentence>(topic,
+                                                            kROSQueueSize);
+  nmea_msgs::Sentence m;
+  m.header.stamp = ros::Time::now();
+  m.header.frame_id = frame_id;
+  m.sentence = sentence;
+  publisher.publish(m);
+}
+
 /**
  * @param gnss The string representing the GNSS. Refer MonVER message protocol.
  * i.e. GPS, GLO, GAL, BDS, QZSS, SBAS, IMES

ublox_gps/package.xml

@@ -16,6 +16,7 @@
 
   <depend>ublox_serialization</depend>
   <depend>ublox_msgs</depend>
+  <depend>nmea_msgs</depend>
   <depend>roscpp</depend>
   <depend>roscpp_serialization</depend>
   <depend>tf</depend>

ublox_gps/src/node.cpp

@@ -155,15 +155,16 @@ void UbloxNode::addProductInterface(std::string product_category, std::string re
     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 ||
+           product_category.compare("LAP") == 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)
     ROS_WARN("Product category %s %s from MonVER message not recognized %s",
-             product_category.c_str(),
-             ref_rov.c_str(),
-             "options are HPG REF, HPG ROV, HPG #.#, HDG #.#, TIM, ADR, UDR, FTS, SPG");
+             product_category.c_str(), ref_rov.c_str(),
+             "options are HPG REF, HPG ROV, HPG #.#, HDG #.#, TIM, ADR, UDR, LAP, FTS, SPG");
 }
 
 void UbloxNode::getRosParams()
@@ -321,6 +322,10 @@ void UbloxNode::subscribe()
   if (enabled["nav_clock"])
     gps.subscribe<ublox_msgs::NavCLOCK>(boost::bind(publish<ublox_msgs::NavCLOCK>, _1, "navclock"), kSubscribeRate);
 
+  nh->param("publish/nmea", enabled["nmea"], false);
+  if (enabled["nmea"])
+    gps.subscribe_nmea(boost::bind(publish_nmea, _1, "nmea"));
+
   // INF messages
   nh->param("inf/debug", enabled["inf_debug"], false);
   if (enabled["inf_debug"])
@@ -1234,8 +1239,11 @@ void UbloxFirmware8::getRosParams()
 
     std::vector<uint8_t> bdsTalkerId;
     getRosUint("nmea/bds_talker_id", bdsTalkerId);
-    cfg_nmea_.bdsTalkerId[0] = bdsTalkerId[0];
-    cfg_nmea_.bdsTalkerId[1] = bdsTalkerId[1];
+    if(bdsTalkerId.size() >= 2) {
+      cfg_nmea_.bdsTalkerId[0] = bdsTalkerId[0];
+      cfg_nmea_.bdsTalkerId[1] = bdsTalkerId[1];
+    }
+
   }
 }
 
@@ -1434,6 +1442,12 @@ void AdrUdrProduct::subscribe()
   if (enabled["nav_att"])
     gps.subscribe<ublox_msgs::NavATT>(boost::bind(publish<ublox_msgs::NavATT>, _1, "navatt"), kSubscribeRate);
 
+  // Subscribe to ESF ALG messages
+  nh->param("publish/esf/alg", enabled["esf_alg"], enabled["esf"]);
+  if (enabled["esf_alg"])
+    gps.subscribe<ublox_msgs::EsfALG>(boost::bind(
+        publish<ublox_msgs::EsfALG>, _1, "esfalg"), kSubscribeRate);
+
   // Subscribe to ESF INS messages
   nh->param("publish/esf/ins", enabled["esf_ins"], enabled["esf"]);
   if (enabled["esf_ins"])

ublox_msg_filters/scripts/talker.py

@@ -2,13 +2,14 @@
 import rospy
 from ublox_msgs.msg import NavHPPOSLLH, NavRELPOSNED9, NavVELNED
 
+
 class Talker(object):
     def __init__(self):
         self.itow = 0
-        self.pub1 = rospy.Publisher('msg1', NavHPPOSLLH, queue_size=2)
-        self.pub2 = rospy.Publisher('msg2', NavRELPOSNED9, queue_size=2)
-        self.pub3 = rospy.Publisher('msg3', NavVELNED, queue_size=2)
-        self.timer = rospy.Timer(rospy.Duration(0.2),self.publish)
+        self.pub1 = rospy.Publisher("msg1", NavHPPOSLLH, queue_size=2)
+        self.pub2 = rospy.Publisher("msg2", NavRELPOSNED9, queue_size=2)
+        self.pub3 = rospy.Publisher("msg3", NavVELNED, queue_size=2)
+        self.timer = rospy.Timer(rospy.Duration(0.2), self.publish)
 
     def publish(self, event):
         rospy.loginfo("TX %u", self.itow)
@@ -27,11 +28,13 @@ def publish(self, event):
 
         self.itow += 1
 
+
 def main():
-    rospy.init_node('talker')
+    rospy.init_node("talker")
     talker = Talker()
     rospy.loginfo("Ready to transmit")
     rospy.spin()
 
-if __name__ == '__main__':
+
+if __name__ == "__main__":
     main()

ublox_msgs/include/ublox/serialization/ublox_msgs.h

@@ -900,6 +900,7 @@ struct Serializer<ublox_msgs::EsfSTATUS_<ContainerAllocator> >
   {
     ros::serialization::IStream stream(const_cast<uint8_t*>(data), count);
     stream.next(m.iTOW);
+    stream.next(m.reserved1);
     stream.next(m.version);
     stream.next(m.fusionMode);
     stream.next(m.reserved2);
@@ -923,6 +924,7 @@ struct Serializer<ublox_msgs::EsfSTATUS_<ContainerAllocator> >
     ros::serialization::OStream stream(data, size);
     stream.next(m.iTOW);
     stream.next(m.version);
+    stream.next(m.reserved1);
     stream.next(m.fusionMode);
     stream.next(m.reserved2);
     stream.next(static_cast<typename Msg::_numSens_type>(m.sens.size()));