diff --git a/README.md b/README.md index c96aea166..3908121b1 100644 --- a/README.md +++ b/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) diff --git a/fuse_core/include/fuse_core/throttled_callback.hpp b/fuse_core/include/fuse_core/throttled_callback.hpp index 050e9b7a2..5afa1bbea 100644 --- a/fuse_core/include/fuse_core/throttled_callback.hpp +++ b/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 using ThrottledMessageCallback = - ThrottledCallback>; + ThrottledCallback>; } // namespace fuse_core diff --git a/fuse_core/include/fuse_core/transaction_deserializer.hpp b/fuse_core/include/fuse_core/transaction_deserializer.hpp index cf6575e14..634129b21 100644 --- a/fuse_core/include/fuse_core/transaction_deserializer.hpp +++ b/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 diff --git a/fuse_core/src/async_sensor_model.cpp b/fuse_core/src/async_sensor_model.cpp index 9da3c619e..bbebc2494 100644 --- a/fuse_core/src/async_sensor_model.cpp +++ b/fuse_core/src/async_sensor_model.cpp @@ -32,6 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ #include +#include #include #include diff --git a/fuse_core/src/fuse_echo.cpp b/fuse_core/src/fuse_echo.cpp index 11629897f..0286700b0 100644 --- a/fuse_core/src/fuse_echo.cpp +++ b/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(); } }; diff --git a/fuse_core/src/transaction_deserializer.cpp b/fuse_core/src/transaction_deserializer.cpp index b69136a61..b2c1f82d4 100644 --- a/fuse_core/src/transaction_deserializer.cpp +++ b/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 stream(msg.data); { BinaryInputArchive archive(stream); - transaction.deserialize(archive); + transaction->deserialize(archive); } return transaction; } diff --git a/fuse_core/test/test_throttled_callback.cpp b/fuse_core/test/test_throttled_callback.cpp index a3a411bf7..8548a45fd 100644 --- a/fuse_core/test/test_throttled_callback.cpp +++ b/fuse_core/test/test_throttled_callback.cpp @@ -96,8 +96,8 @@ class PointPublisher : public rclcpp::Node } private: - rclcpp::Publisher::SharedPtr publisher_; //!< The publisher - double frequency_{10.0}; //!< The publish rate frequency + rclcpp::Publisher::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( "point", 10, std::bind( - &PointThrottledCallback::callback, + &PointThrottledCallback::callback, &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,10 +179,10 @@ 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(msg); } /** @@ -190,8 +190,8 @@ class PointSensorModel : public rclcpp::Node * * @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