fuse -> ROS 2 fuse_publishers : Linting

fuse_core/include/fuse_core/parameter.hpp

@@ -277,8 +277,14 @@ inline fuse_core::Loss::SharedPtr loadLossConfig(
   const std::string & name
 )
 {
+  if (!interfaces.get_node_parameters_interface()->has_parameter(
+      name + ".type"))
+  {
+    return {};
+  }
+
   std::string loss_type;
-  getParamRequired(interfaces, name + "/type", loss_type);
+  getParamRequired(interfaces, name + ".type", loss_type);
 
   auto loss = fuse_core::createUniqueLoss(loss_type);
   loss->initialize(interfaces, interfaces.get_node_base_interface()->get_fully_qualified_name());

fuse_core/include/fuse_core/util.hpp

@@ -38,6 +38,7 @@
 #include <Eigen/Core>
 
 #include <cmath>
+#include <string>
 
 #include <rclcpp/logging.hpp>
 
@@ -149,6 +150,33 @@ Eigen::Matrix<T, 2, 2, Eigen::RowMajor> rotationMatrix2D(const T angle)
   return rotation;
 }
 
+/**
+ * @brief Create a compound ROS topic name from two components
+ *
+ * @param[in] a The first element
+ * @param[in] b The second element
+ * @return      The joined topic name
+ */
+inline
+std::string joinTopicName(std::string a, std::string b)
+{
+  if (a.empty()) {
+    return b;
+  }
+  if (b.empty()) {
+    return a;
+  }
+
+  if (a.back() == '/') {
+    a.pop_back();
+  }
+  if (b.front() == '/') {
+    b.erase(b.begin());
+  }
+
+  return a + "/" + b;
+}
+
 }  // namespace fuse_core
 
 #endif  // FUSE_CORE__UTIL_HPP_

fuse_core/test/test_util.cpp

@@ -69,4 +69,13 @@ TEST(Util, wrapAngle2D)
     const double angle = 0.5;
     EXPECT_EQ(angle, fuse_core::wrapAngle2D(angle - 3.0 * 2.0 * M_PI));
   }
+
+  // Join topic names
+  {
+    EXPECT_EQ("a/b", fuse_core::joinTopicName("a", "b"));
+    EXPECT_EQ("/a/b", fuse_core::joinTopicName("/a", "b"));
+    EXPECT_EQ("a/b", fuse_core::joinTopicName("a/", "b"));
+    EXPECT_EQ("a/b", fuse_core::joinTopicName("a", "/b"));
+    EXPECT_EQ("a/b", fuse_core::joinTopicName("a/", "/b"));
+  }
 }

fuse_models/include/fuse_models/odometry_2d_publisher.h

@@ -42,7 +42,7 @@
 #include <fuse_core/transaction.hpp>
 #include <fuse_core/uuid.hpp>
 #include <fuse_core/variable.hpp>
-#include <fuse_publishers/stamped_variable_synchronizer.h>
+#include <fuse_publishers/stamped_variable_synchronizer.hpp>
 
 #include <geometry_msgs/AccelWithCovarianceStamped.h>
 #include <nav_msgs/Odometry.h>

fuse_publishers/include/fuse_publishers/path_2d_publisher.h

@@ -1,7 +1,7 @@
 /*
  * Software License Agreement (BSD License)
  *
- *  Copyright (c) 2018, Locus Robotics
+ *  Copyright (c) 2022, Locus Robotics
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -31,86 +31,12 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  */
-#ifndef FUSE_PUBLISHERS_PATH_2D_PUBLISHER_H
-#define FUSE_PUBLISHERS_PATH_2D_PUBLISHER_H
 
-#include <fuse_core/async_publisher.hpp>
-#include <fuse_core/graph.hpp>
-#include <fuse_core/fuse_macros.hpp>
-#include <fuse_core/transaction.hpp>
-#include <fuse_core/uuid.hpp>
-#include <rclcpp/rclcpp.hpp>
+#ifndef FUSE_PUBLISHERS__PATH_2D_PUBLISHER_H_
+#define FUSE_PUBLISHERS__PATH_2D_PUBLISHER_H_
 
-#include <nav_msgs/msg/path.hpp>
-#include <geometry_msgs/msg/pose_array.hpp>
-#include <geometry_msgs/msg/pose_stamped.hpp>
+#warning This header is obsolete, please include fuse_publishers/path_2d_publisher.hpp instead
 
-#include <string>
+#include <fuse_publishers/path_2d_publisher.hpp>
 
-
-namespace fuse_publishers
-{
-
-/**
- * @brief Publisher plugin that publishes all of the stamped 2D poses as a nav_msgs::msg::Path message.
- *
- * Parameters:
- *  - device_id (uuid string, default: 00000000-0000-0000-0000-000000000000) The device/robot ID to publish
- *  - device_name (string) Used to generate the device/robot ID if the device_id is not provided
- *  - frame_id (string, default: map)  Name for the robot's map frame
- */
-class Path2DPublisher : public fuse_core::AsyncPublisher
-{
-public:
-  FUSE_SMART_PTR_DEFINITIONS(Path2DPublisher)
-
-  /**
-   * @brief Constructor
-   */
-  Path2DPublisher();
-
-  /**
-   * @brief Destructor
-   */
-  virtual ~Path2DPublisher() = default;
-
-  /**
-   * @brief Shadowing extension to the AsyncPublisher::initialize call
-   */
-  void initialize(
-    fuse_core::node_interfaces::NodeInterfaces<ALL_FUSE_CORE_NODE_INTERFACES> interfaces,
-    const std::string & name) override;
-
-  /**
-   * @brief Perform any required post-construction initialization, such as advertising publishers or reading from the
-   * parameter server.
-   */
-  void onInit() override;
-
-  /**
-   * @brief Notify the publisher about variables that have been added or removed
-   *
-   * @param[in] transaction A Transaction object, describing the set of variables that have been added and/or removed
-   * @param[in] graph       A read-only pointer to the graph object, allowing queries to be performed whenever needed
-   */
-  void notifyCallback(
-    fuse_core::Transaction::ConstSharedPtr transaction,
-    fuse_core::Graph::ConstSharedPtr graph) override;
-
-protected:
-  fuse_core::node_interfaces::NodeInterfaces<
-    fuse_core::node_interfaces::Base,
-    fuse_core::node_interfaces::Parameters,
-    fuse_core::node_interfaces::Topics,
-    fuse_core::node_interfaces::Waitables
-  > interfaces_;  //!< Shadows AsyncPublisher interfaces_
-
-  fuse_core::UUID device_id_;  //!< The UUID of the device to be published
-  std::string frame_id_;  //!< The name of the frame for this path
-  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_publisher_;  //!< The publisher that sends the entire robot trajectory as a path
-  rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr pose_array_publisher_;  //!< The publisher that sends the entire robot trajectory as a pose array
-};
-
-}  // namespace fuse_publishers
-
-#endif  // FUSE_PUBLISHERS_PATH_2D_PUBLISHER_H
+#endif  // FUSE_PUBLISHERS__PATH_2D_PUBLISHER_H_

fuse_publishers/include/fuse_publishers/path_2d_publisher.hpp

@@ -0,0 +1,124 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2018, Locus Robotics
+ *  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 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.
+ */
+#ifndef FUSE_PUBLISHERS__PATH_2D_PUBLISHER_HPP_
+#define FUSE_PUBLISHERS__PATH_2D_PUBLISHER_HPP_
+
+#include <string>
+
+#include <fuse_core/async_publisher.hpp>
+#include <fuse_core/graph.hpp>
+#include <fuse_core/fuse_macros.hpp>
+#include <fuse_core/transaction.hpp>
+#include <fuse_core/uuid.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <nav_msgs/msg/path.hpp>
+#include <geometry_msgs/msg/pose_array.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+
+
+namespace fuse_publishers
+{
+
+/**
+ * @brief Publisher plugin that publishes all of the stamped 2D poses as a nav_msgs::msg::Path
+ *        message.
+ *
+ * Parameters:
+ *  - device_id (uuid string, default: 00000000-0000-0000-0000-000000000000) The device/robot ID to
+ *    publish
+ *  - device_name (string) Used to generate the device/robot ID if the device_id is not provided
+ *  - frame_id (string, default: map)  Name for the robot's map frame
+ */
+class Path2DPublisher : public fuse_core::AsyncPublisher
+{
+public:
+  FUSE_SMART_PTR_DEFINITIONS(Path2DPublisher)
+
+  /**
+   * @brief Constructor
+   */
+  Path2DPublisher();
+
+  /**
+   * @brief Destructor
+   */
+  virtual ~Path2DPublisher() = default;
+
+  /**
+   * @brief Shadowing extension to the AsyncPublisher::initialize call
+   */
+  void initialize(
+    fuse_core::node_interfaces::NodeInterfaces<ALL_FUSE_CORE_NODE_INTERFACES> interfaces,
+    const std::string & name) override;
+
+  /**
+   * @brief Perform any required post-construction initialization, such as advertising publishers or
+   *        reading from the parameter server.
+   */
+  void onInit() override;
+
+  /**
+   * @brief Notify the publisher about variables that have been added or removed
+   *
+   * @param[in] transaction A Transaction object, describing the set of variables that have been
+   *                        added and/or removed
+   * @param[in] graph       A read-only pointer to the graph object, allowing queries to be
+   *                        performed whenever needed
+   */
+  void notifyCallback(
+    fuse_core::Transaction::ConstSharedPtr transaction,
+    fuse_core::Graph::ConstSharedPtr graph) override;
+
+protected:
+  fuse_core::node_interfaces::NodeInterfaces<
+    fuse_core::node_interfaces::Base,
+    fuse_core::node_interfaces::Parameters,
+    fuse_core::node_interfaces::Topics,
+    fuse_core::node_interfaces::Waitables
+  > interfaces_;  //!< Shadows AsyncPublisher interfaces_
+
+  fuse_core::UUID device_id_;  //!< The UUID of the device to be published
+  std::string frame_id_;  //!< The name of the frame for this path
+
+  //!< The publisher that sends the entire robot trajectory as a path
+  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_publisher_;
+
+  //!< The publisher that sends the entire robot trajectory as a pose array
+  rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr pose_array_publisher_;
+};
+
+}  // namespace fuse_publishers
+
+#endif  // FUSE_PUBLISHERS__PATH_2D_PUBLISHER_HPP_