Refine changes and fix graph lifetime bug

Signed-off-by: methylDragon <methylDragon@gmail.com>
locusrobotics · Jan 26, 2023 · b96524e · b96524e
1 parent 3cba177
commit b96524e
Show file tree

Hide file tree

Showing 7 changed files with 13 additions and 265 deletions.
diff --git a/fuse_core/include/fuse_core/callback_wrapper.hpp b/fuse_core/include/fuse_core/callback_wrapper.hpp
@@ -179,7 +179,6 @@ class CallbackAdapter : public rclcpp::Waitable
 
   std::shared_ptr<void> take_data() override;
 
-  // TODO(CH3): check this against the threading model of the multi-threaded executor.
   void execute(std::shared_ptr<void> & data) override;
 
   void addCallback(const std::shared_ptr<CallbackWrapperBase> & callback);

diff --git a/fuse_models/include/fuse_models/graph_ignition.h b/fuse_models/include/fuse_models/graph_ignition.h
@@ -151,7 +151,7 @@ class GraphIgnition : public fuse_core::AsyncSensorModel
    * @param[in] graph - The graph
    * @param[in] stamp - The graph stamp
    */
-  void sendGraph(const fuse_core::Graph& graph, const rclcpp::Time& stamp);
+  void sendGraph(fuse_core::Graph& graph, const rclcpp::Time& stamp);
 
   fuse_core::node_interfaces::NodeInterfaces<
     fuse_core::node_interfaces::Base,

diff --git a/fuse_models/src/graph_ignition.cpp b/fuse_models/src/graph_ignition.cpp
@@ -163,7 +163,10 @@ void GraphIgnition::process(
   }
 
   // Deserialize the graph message
-  const auto graph = graph_deserializer_.deserialize(msg);
+  // NOTE(methylDragon): We convert the Graph::UniquePtr to a shared pointer so it can be passed
+  //                     as a copyable object to the deferred service call's std::function<> arg
+  //                     to satisfy the requirement that std::function<> arguments are copyable.
+  auto graph = std::shared_ptr<fuse_core::Graph>(std::move(graph_deserializer_.deserialize(msg)));
 
   // Validate the requested graph before we do anything
   if (boost::empty(graph->getConstraints()))
@@ -191,11 +194,12 @@ void GraphIgnition::process(
     // Don't block the executor.
     // It needs to be free to handle the response to this service call.
     // Have a callback do the rest of the work when a response comes.
-    auto result_future = reset_client_->async_send_request(srv,
-      [this, post_process, &graph, msg](rclcpp::Client<std_srvs::srv::Empty>::SharedFuture result){
+    auto result_future = reset_client_->async_send_request(
+      srv,
+      [this, post_process, captured_graph = std::move(graph), msg](rclcpp::Client<std_srvs::srv::Empty>::SharedFuture result) {
         (void)result;
         // Now that the optimizer has been reset, actually send the initial state constraints to the optimizer
-        sendGraph(*graph, msg.header.stamp);
+        sendGraph(*captured_graph, msg.header.stamp);
         if (post_process) {
           post_process();
         }
@@ -208,7 +212,7 @@ void GraphIgnition::process(
   }
 }
 
-void GraphIgnition::sendGraph(const fuse_core::Graph& graph, const rclcpp::Time& stamp)
+void GraphIgnition::sendGraph(fuse_core::Graph& graph, const rclcpp::Time& stamp)
 {
   // Create a transaction equivalent to the graph
   auto transaction = fuse_core::Transaction::make_shared();

diff --git a/fuse_optimizers/src/optimizer.cpp b/fuse_optimizers/src/optimizer.cpp
@@ -41,8 +41,6 @@
 #include <fuse_optimizers/optimizer.h>
 #include <fuse_graphs/hash_graph.hpp>
 
-//#include <XmlRpcValue.h>
-
 #include <chrono>
 #include <functional>
 #include <numeric>
@@ -55,7 +53,6 @@
 namespace fuse_optimizers
 {
 
-// XXX pass a rclcpp::Context, and a CallbackGroup
 Optimizer::Optimizer(
   fuse_core::node_interfaces::NodeInterfaces<ALL_FUSE_CORE_NODE_INTERFACES> interfaces,
   fuse_core::Graph::UniquePtr graph

diff --git a/fuse_optimizers/test/CMakeLists.txt b/fuse_optimizers/test/CMakeLists.txt
@@ -1,14 +1,6 @@
 # CORE GTESTS ======================================================================================
-set(TEST_TARGETS
-  # test_fixed_lag_ignition
-  # test_optimizer
-  test_variable_stamp_index
-)
-
-foreach(test_name ${TEST_TARGETS})
-  ament_add_gtest("${test_name}" "${test_name}.cpp")
-  target_link_libraries("${test_name}" ${PROJECT_NAME})
-endforeach()
+ament_add_gtest(test_variable_stamp_index "test_variable_stamp_index.cpp")
+target_link_libraries(test_variable_stamp_index ${PROJECT_NAME})
 
 
 # ROS TESTS (WITH LAUNCH) ==========================================================================

diff --git a/fuse_optimizers/test/launch_tests/test_parameters.cpp b/fuse_optimizers/test/launch_tests/test_parameters.cpp
diff --git a/fuse_publishers/src/pose_2d_publisher.cpp b/fuse_publishers/src/pose_2d_publisher.cpp
@@ -178,7 +178,7 @@ void Pose2DPublisher::onInit()
         rclcpp::Duration::from_seconds(tf_cache_time)
         .to_chrono<std::chrono::nanoseconds>()
         // , interfaces_  // NOTE(methylDragon): This one is pending a change on tf2_ros/buffer.h
-        // TODO(methylDragon): See above ^
+        //                   TODO(methylDragon): See above ^                            // NOLINT
       );
       tf_listener_ = std::make_unique<tf2_ros::TransformListener>(
         *tf_buffer_,