Refine msg usage

Signed-off-by: methylDragon <methylDragon@gmail.com>

Author: methylDragon

fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp

@@ -36,7 +36,6 @@
 #include <fuse_core/eigen_gtest.hpp>
 #include <fuse_core/uuid.hpp>
 #include <fuse_variables/orientation_3d_stamped.h>
-#include <geometry_msgs/msg/Quaternion.hpp>
 
 #include <ceres/covariance.h>
 #include <ceres/problem.h>

fuse_core/include/fuse_core/graph_deserializer.hpp

@@ -74,7 +74,7 @@ class GraphDeserializer
    * @return          A unique_ptr to a derived Graph object
    */
   inline fuse_core::Graph::UniquePtr deserialize(
-    const fuse_msgs::msg::SerializedGraph::ConstSharedPtr msg) const
+    const fuse_msgs::msg::SerializedGraph::SharedPtr msg) const
   {
     return deserialize(*msg);
   }

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::SharedPtr 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/package.xml

@@ -14,6 +14,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>boost</depend>
   <depend>eigen</depend>
   <depend>libceres-dev</depend>
   <depend>libgoogle-glog-dev</depend>

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::ConstSharedPtr 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,26 @@ 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_ = std::move(msg);
+
+    if (!last_kept_message_) {
+      last_kept_message_ = std::make_shared<geometry_msgs::msg::Point>();
+    }
+
+    last_kept_message_->x = msg.x;
+    last_kept_message_->y = msg.y;
+    last_kept_message_->z = msg.z;
   }
 
   /**
    * @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 +210,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

fuse_models/include/fuse_models/imu_2d.h

@@ -141,7 +141,7 @@ class Imu2D : public fuse_core::AsyncSensorModel
 
   ParameterType params_;
 
-  std::unique_ptr<geometry_msgs::msg::PoseWithCovarianceStamped> previous_pose_;
+  geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr previous_pose_;
 
   tf2_ros::Buffer tf_buffer_;
 

fuse_models/include/fuse_models/odometry_2d.h

@@ -137,7 +137,7 @@ class Odometry2D : public fuse_core::AsyncSensorModel
 
   ParameterType params_;
 
-  std::unique_ptr<geometry_msgs::msg::PoseWithCovarianceStamped> previous_pose_;
+  geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr previous_pose_;
 
   tf2_ros::Buffer tf_buffer_;