fuse -> ROS 2 fuse_core : Messages and Services (#285)

README.md

@@ -86,7 +86,7 @@ out to ROS. In order to publish data to ROS, we derive a `fuse_core::Publisher`
 optimizer. Derived publishers have access to the optimized values of all state variables. The specific publisher
 implementation determines what type of messages are published and at what frequency. For our example system,
 we would like visualize the current pose of the robot in RViz, so we create a `fuse` publisher that finds the most
-recent pose and converts it into a `geometry_msgs::PoseStamped` message, then publishes the message to a topic.
+recent pose and converts it into a `geometry_msgs::msg::PoseStamped` message, then publishes the message to a topic.
 
 ![fuse optimizer](doc/fuse_optimizer_2.png)
 
@@ -117,7 +117,7 @@ from the configured plugins whenever new states are created by the sensor models
 
 Nothing about the `fuse` framework limits you to having a single publisher. What if you want to visualize the entire
 robot trajectory, instead of just the most recent pose? Well, we can create a new derived `fuse_core::Publisher` class
-that publishes all of the robot poses using a `nav_msgs::Path` message.
+that publishes all of the robot poses using a `nav_msgs::msg::Path` message.
 
 ![fuse optimizer](doc/fuse_optimizer_4.png)
 

fuse_core/include/fuse_core/throttled_callback.hpp

@@ -198,11 +198,11 @@ class ThrottledCallback
 /**
  * @brief Throttled callback for ROS messages
  *
- * @tparam M The ROS message type, which should have the M::ConstPtr nested type
+ * @tparam M The ROS message type
  */
 template<class M>
 using ThrottledMessageCallback =
-  ThrottledCallback<std::function<void (const typename M::ConstPtr &)>>;
+  ThrottledCallback<std::function<void (const M &)>>;
 
 }  // namespace fuse_core
 

fuse_core/include/fuse_core/transaction_deserializer.hpp

@@ -75,8 +75,11 @@ class TransactionDeserializer
    * @param[IN]  msg  The SerializedTransaction message to be deserialized
    * @return          A fuse Transaction object
    */
-  fuse_core::Transaction deserialize(const fuse_msgs::msg::SerializedTransaction::SharedPtr msg)
-  const;
+  inline fuse_core::Transaction::UniquePtr deserialize(
+    const fuse_msgs::msg::SerializedTransaction::ConstSharedPtr msg) const
+  {
+    return deserialize(*msg);
+  }
 
   /**
    * @brief Deserialize a SerializedTransaction message into a fuse Transaction object.
@@ -87,7 +90,8 @@ class TransactionDeserializer
    * @param[IN]  msg  The SerializedTransaction message to be deserialized
    * @return          A fuse Transaction object
    */
-  fuse_core::Transaction deserialize(const fuse_msgs::msg::SerializedTransaction & msg) const;
+  fuse_core::Transaction::UniquePtr deserialize(
+    const fuse_msgs::msg::SerializedTransaction & msg) const;
 
 private:
   //! Pluginlib class loader for Variable types

fuse_core/src/async_sensor_model.cpp

@@ -32,6 +32,7 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 #include <functional>
+#include <memory>
 #include <string>
 #include <utility>
 

fuse_core/src/fuse_echo.cpp

@@ -87,7 +87,7 @@ class FuseEcho : public rclcpp::Node
     std::cout << "TRANSACTION:" << std::endl;
     std::cout << "received at: " << this->now().seconds() << std::endl;
     auto transaction = transaction_deserializer_.deserialize(msg);
-    transaction.print();
+    transaction->print();
   }
 };
 

fuse_core/src/transaction_deserializer.cpp

@@ -71,24 +71,18 @@ TransactionDeserializer::TransactionDeserializer()
   }
 }
 
-fuse_core::Transaction TransactionDeserializer::deserialize(
-  const fuse_msgs::msg::SerializedTransaction::SharedPtr msg) const
-{
-  return deserialize(*msg);
-}
-
-fuse_core::Transaction TransactionDeserializer::deserialize(
+fuse_core::Transaction::UniquePtr TransactionDeserializer::deserialize(
   const fuse_msgs::msg::SerializedTransaction & msg) const
 {
   // The Transaction object is not a plugin and has no derived types. That makes it much easier to
   // use.
-  auto transaction = fuse_core::Transaction();
+  auto transaction = fuse_core::Transaction::UniquePtr();
   // Deserialize the msg.data field into the transaction.
   // This will throw if something goes wrong in the deserialization.
   boost::iostreams::stream<fuse_core::MessageBufferStreamSource> stream(msg.data);
   {
     BinaryInputArchive archive(stream);
-    transaction.deserialize(archive);
+    transaction->deserialize(archive);
   }
   return transaction;
 }

fuse_core/test/test_throttled_callback.cpp

@@ -96,8 +96,8 @@ class PointPublisher : public rclcpp::Node
   }
 
 private:
-  rclcpp::Publisher<geometry_msgs::msg::Point>::SharedPtr publisher_;     //!< The publisher
-  double frequency_{10.0};       //!< The publish rate frequency
+  rclcpp::Publisher<geometry_msgs::msg::Point>::SharedPtr publisher_;  //!< The publisher
+  double frequency_{10.0};  //!< The publish rate frequency
 };
 
 /**
@@ -127,7 +127,7 @@ class PointSensorModel : public rclcpp::Node
     subscription_ = this->create_subscription<geometry_msgs::msg::Point>(
       "point", 10,
       std::bind(
-        &PointThrottledCallback::callback<const geometry_msgs::msg::Point::ConstSharedPtr &>,
+        &PointThrottledCallback::callback<const geometry_msgs::msg::Point &>,
         &throttled_callback_, std::placeholders::_1)
     );
   }
@@ -167,7 +167,7 @@ class PointSensorModel : public rclcpp::Node
    *
    * @return The last message kept. It would be nullptr if no message has been kept so far
    */
-  const geometry_msgs::msg::Point::ConstSharedPtr getLastKeptMessage() const
+  const geometry_msgs::msg::Point::SharedPtr getLastKeptMessage() const
   {
     return last_kept_message_;
   }
@@ -179,19 +179,19 @@ class PointSensorModel : public rclcpp::Node
    *
    * @param[in] msg A geometry_msgs::msg::Point message
    */
-  void keepCallback(const geometry_msgs::msg::Point::ConstSharedPtr & msg)
+  void keepCallback(const geometry_msgs::msg::Point & msg)
   {
     ++kept_messages_;
-    last_kept_message_ = msg;
+    last_kept_message_ = std::make_shared<geometry_msgs::msg::Point>(msg);
   }
 
   /**
    * @brief Drop callback, that counts the number of times it has been called
    *
    * @param[in] msg A geometry_msgs::msg::Point message (not used)
    */
-  // NOTE(CH3): The ConstSharedPtr here is necessary to allow binding the throttled callback
-  void dropCallback(const geometry_msgs::msg::Point::ConstSharedPtr & /*msg*/)
+  // NOTE(CH3): The msg arg here is necessary to allow binding the throttled callback
+  void dropCallback(const geometry_msgs::msg::Point & /*msg*/)
   {
     ++dropped_messages_;
   }
@@ -203,7 +203,9 @@ class PointSensorModel : public rclcpp::Node
 
   size_t kept_messages_{0};                           //!< Messages kept
   size_t dropped_messages_{0};                        //!< Messages dropped
-  geometry_msgs::msg::Point::ConstSharedPtr last_kept_message_;  //!< The last message kept
+
+  // We use a SharedPtr to check for nullptr just for this test
+  geometry_msgs::msg::Point::SharedPtr last_kept_message_;  //!< The last message kept
 };
 
 class TestThrottledCallback : public ::testing::Test