make rcl_lifecyle_com_interface optional in lifecycle nodes (#1507)

Karsten1987 · fujitatomoya · web-flow · commit 052596c971c1 · 2021-03-23T11:26:35.000-07:00
* make rcl_com_interface optional

Signed-off-by: Karsten Knese &lt;Karsten1987@users.noreply.github.com&gt;

* Update rclcpp_lifecycle/test/mocking_utils/patch.hpp

Co-authored-by: Tomoya Fujita &lt;Tomoya.Fujita@sony.com&gt;

* line break

Signed-off-by: Karsten Knese &lt;Karsten1987@users.noreply.github.com&gt;

* use state_machine_options

Signed-off-by: Karsten Knese &lt;Karsten1987@users.noreply.github.com&gt;

Co-authored-by: Tomoya Fujita &lt;Tomoya.Fujita@sony.com&gt;
diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp
@@ -127,23 +127,27 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface,
   /**
    * \param[in] node_name Name of the node.
    * \param[in] options Additional options to control creation of the node.
+   * \param[in] enable_communication_interface Deciding whether the communication interface of the underlying rcl_lifecycle_node shall be enabled.
    */
   RCLCPP_LIFECYCLE_PUBLIC
   explicit LifecycleNode(
     const std::string & node_name,
-    const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+    const rclcpp::NodeOptions & options = rclcpp::NodeOptions(),
+    bool enable_communication_interface = true);
 
   /// Create a node based on the node name and a rclcpp::Context.
   /**
    * \param[in] node_name Name of the node.
    * \param[in] namespace_ Namespace of the node.
    * \param[in] options Additional options to control creation of the node.
+   * \param[in] enable_communication_interface Deciding whether the communication interface of the underlying rcl_lifecycle_node shall be enabled.
    */
   RCLCPP_LIFECYCLE_PUBLIC
   LifecycleNode(
     const std::string & node_name,
     const std::string & namespace_,
-    const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+    const rclcpp::NodeOptions & options = rclcpp::NodeOptions(),
+    bool enable_communication_interface = true);
 
   RCLCPP_LIFECYCLE_PUBLIC
   virtual ~LifecycleNode();
diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp
@@ -47,17 +47,20 @@ namespace rclcpp_lifecycle
 
 LifecycleNode::LifecycleNode(
   const std::string & node_name,
-  const rclcpp::NodeOptions & options)
+  const rclcpp::NodeOptions & options,
+  bool enable_communication_interface)
 : LifecycleNode(
     node_name,
     "",
-    options)
+    options,
+    enable_communication_interface)
 {}
 
 LifecycleNode::LifecycleNode(
   const std::string & node_name,
   const std::string & namespace_,
-  const rclcpp::NodeOptions & options)
+  const rclcpp::NodeOptions & options,
+  bool enable_communication_interface)
 : node_base_(new rclcpp::node_interfaces::NodeBase(
       node_name,
       namespace_,
@@ -105,7 +108,7 @@ LifecycleNode::LifecycleNode(
   node_options_(options),
   impl_(new LifecycleNodeInterfaceImpl(node_base_, node_services_))
 {
-  impl_->init();
+  impl_->init(enable_communication_interface);
 
   register_on_configure(
     std::bind(
diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp
@@ -63,10 +63,11 @@ class LifecycleNode::LifecycleNodeInterfaceImpl
 
   ~LifecycleNodeInterfaceImpl()
   {
+    fprintf(stderr, "oh yeah destructuro\n");
     rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle();
-    const rcl_node_options_t * node_options = rcl_node_get_options(node_handle);
-    auto ret = rcl_lifecycle_state_machine_fini(
-      &state_machine_, node_handle, &node_options->allocator);
+    fprintf(stderr, "oh yeah end destructuro\n");
+    auto ret = rcl_lifecycle_state_machine_fini(&state_machine_, node_handle);
+    fprintf(stderr, "oh yeah end destructuro222\n");
     if (ret != RCL_RET_OK) {
       RCUTILS_LOG_FATAL_NAMED(
         "rclcpp_lifecycle",
@@ -75,12 +76,16 @@ class LifecycleNode::LifecycleNodeInterfaceImpl
   }
 
   void
-  init()
+  init(bool enable_communication_interface = true)
   {
     rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle();
     const rcl_node_options_t * node_options =
       rcl_node_get_options(node_base_interface_->get_rcl_node_handle());
     state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine();
+    auto state_machine_options = rcl_lifecycle_get_default_state_machine_options();
+    state_machine_options.enable_com_interface = enable_communication_interface;
+    state_machine_options.allocator = node_options->allocator;
+
     // The call to initialize the state machine takes
     // currently five different typesupports for all publishers/services
     // created within the RCL_LIFECYCLE structure.
@@ -96,94 +101,96 @@ class LifecycleNode::LifecycleNodeInterfaceImpl
       rosidl_typesupport_cpp::get_service_type_support_handle<GetAvailableStatesSrv>(),
       rosidl_typesupport_cpp::get_service_type_support_handle<GetAvailableTransitionsSrv>(),
       rosidl_typesupport_cpp::get_service_type_support_handle<GetAvailableTransitionsSrv>(),
-      true,
-      &node_options->allocator);
+      &state_machine_options);
     if (ret != RCL_RET_OK) {
+      fprintf(stderr, "oh yeah %s\n", node_base_interface_->get_name());
       throw std::runtime_error(
               std::string("Couldn't initialize state machine for node ") +
               node_base_interface_->get_name());
     }
 
-    {  // change_state
-      auto cb = std::bind(
-        &LifecycleNodeInterfaceImpl::on_change_state, this,
-        std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
-      rclcpp::AnyServiceCallback<ChangeStateSrv> any_cb;
-      any_cb.set(std::move(cb));
-
-      srv_change_state_ = std::make_shared<rclcpp::Service<ChangeStateSrv>>(
-        node_base_interface_->get_shared_rcl_node_handle(),
-        &state_machine_.com_interface.srv_change_state,
-        any_cb);
-      node_services_interface_->add_service(
-        std::dynamic_pointer_cast<rclcpp::ServiceBase>(srv_change_state_),
-        nullptr);
-    }
+    if (enable_communication_interface) {
+      { // change_state
+        auto cb = std::bind(
+          &LifecycleNodeInterfaceImpl::on_change_state, this,
+          std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
+        rclcpp::AnyServiceCallback<ChangeStateSrv> any_cb;
+        any_cb.set(std::move(cb));
+
+        srv_change_state_ = std::make_shared<rclcpp::Service<ChangeStateSrv>>(
+          node_base_interface_->get_shared_rcl_node_handle(),
+          &state_machine_.com_interface.srv_change_state,
+          any_cb);
+        node_services_interface_->add_service(
+          std::dynamic_pointer_cast<rclcpp::ServiceBase>(srv_change_state_),
+          nullptr);
+      }
 
-    {  // get_state
-      auto cb = std::bind(
-        &LifecycleNodeInterfaceImpl::on_get_state, this,
-        std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
-      rclcpp::AnyServiceCallback<GetStateSrv> any_cb;
-      any_cb.set(std::move(cb));
-
-      srv_get_state_ = std::make_shared<rclcpp::Service<GetStateSrv>>(
-        node_base_interface_->get_shared_rcl_node_handle(),
-        &state_machine_.com_interface.srv_get_state,
-        any_cb);
-      node_services_interface_->add_service(
-        std::dynamic_pointer_cast<rclcpp::ServiceBase>(srv_get_state_),
-        nullptr);
-    }
+      { // get_state
+        auto cb = std::bind(
+          &LifecycleNodeInterfaceImpl::on_get_state, this,
+          std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
+        rclcpp::AnyServiceCallback<GetStateSrv> any_cb;
+        any_cb.set(std::move(cb));
+
+        srv_get_state_ = std::make_shared<rclcpp::Service<GetStateSrv>>(
+          node_base_interface_->get_shared_rcl_node_handle(),
+          &state_machine_.com_interface.srv_get_state,
+          any_cb);
+        node_services_interface_->add_service(
+          std::dynamic_pointer_cast<rclcpp::ServiceBase>(srv_get_state_),
+          nullptr);
+      }
 
-    {  // get_available_states
-      auto cb = std::bind(
-        &LifecycleNodeInterfaceImpl::on_get_available_states, this,
-        std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
-      rclcpp::AnyServiceCallback<GetAvailableStatesSrv> any_cb;
-      any_cb.set(std::move(cb));
-
-      srv_get_available_states_ = std::make_shared<rclcpp::Service<GetAvailableStatesSrv>>(
-        node_base_interface_->get_shared_rcl_node_handle(),
-        &state_machine_.com_interface.srv_get_available_states,
-        any_cb);
-      node_services_interface_->add_service(
-        std::dynamic_pointer_cast<rclcpp::ServiceBase>(srv_get_available_states_),
-        nullptr);
-    }
+      { // get_available_states
+        auto cb = std::bind(
+          &LifecycleNodeInterfaceImpl::on_get_available_states, this,
+          std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
+        rclcpp::AnyServiceCallback<GetAvailableStatesSrv> any_cb;
+        any_cb.set(std::move(cb));
+
+        srv_get_available_states_ = std::make_shared<rclcpp::Service<GetAvailableStatesSrv>>(
+          node_base_interface_->get_shared_rcl_node_handle(),
+          &state_machine_.com_interface.srv_get_available_states,
+          any_cb);
+        node_services_interface_->add_service(
+          std::dynamic_pointer_cast<rclcpp::ServiceBase>(srv_get_available_states_),
+          nullptr);
+      }
 
-    {  // get_available_transitions
-      auto cb = std::bind(
-        &LifecycleNodeInterfaceImpl::on_get_available_transitions, this,
-        std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
-      rclcpp::AnyServiceCallback<GetAvailableTransitionsSrv> any_cb;
-      any_cb.set(std::move(cb));
-
-      srv_get_available_transitions_ =
-        std::make_shared<rclcpp::Service<GetAvailableTransitionsSrv>>(
-        node_base_interface_->get_shared_rcl_node_handle(),
-        &state_machine_.com_interface.srv_get_available_transitions,
-        any_cb);
-      node_services_interface_->add_service(
-        std::dynamic_pointer_cast<rclcpp::ServiceBase>(srv_get_available_transitions_),
-        nullptr);
-    }
+      { // get_available_transitions
+        auto cb = std::bind(
+          &LifecycleNodeInterfaceImpl::on_get_available_transitions, this,
+          std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
+        rclcpp::AnyServiceCallback<GetAvailableTransitionsSrv> any_cb;
+        any_cb.set(std::move(cb));
+
+        srv_get_available_transitions_ =
+          std::make_shared<rclcpp::Service<GetAvailableTransitionsSrv>>(
+          node_base_interface_->get_shared_rcl_node_handle(),
+          &state_machine_.com_interface.srv_get_available_transitions,
+          any_cb);
+        node_services_interface_->add_service(
+          std::dynamic_pointer_cast<rclcpp::ServiceBase>(srv_get_available_transitions_),
+          nullptr);
+      }
 
-    {  // get_transition_graph
-      auto cb = std::bind(
-        &LifecycleNodeInterfaceImpl::on_get_transition_graph, this,
-        std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
-      rclcpp::AnyServiceCallback<GetAvailableTransitionsSrv> any_cb;
-      any_cb.set(std::move(cb));
-
-      srv_get_transition_graph_ =
-        std::make_shared<rclcpp::Service<GetAvailableTransitionsSrv>>(
-        node_base_interface_->get_shared_rcl_node_handle(),
-        &state_machine_.com_interface.srv_get_transition_graph,
-        any_cb);
-      node_services_interface_->add_service(
-        std::dynamic_pointer_cast<rclcpp::ServiceBase>(srv_get_transition_graph_),
-        nullptr);
+      { // get_transition_graph
+        auto cb = std::bind(
+          &LifecycleNodeInterfaceImpl::on_get_transition_graph, this,
+          std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
+        rclcpp::AnyServiceCallback<GetAvailableTransitionsSrv> any_cb;
+        any_cb.set(std::move(cb));
+
+        srv_get_transition_graph_ =
+          std::make_shared<rclcpp::Service<GetAvailableTransitionsSrv>>(
+          node_base_interface_->get_shared_rcl_node_handle(),
+          &state_machine_.com_interface.srv_get_transition_graph,
+          any_cb);
+        node_services_interface_->add_service(
+          std::dynamic_pointer_cast<rclcpp::ServiceBase>(srv_get_transition_graph_),
+          nullptr);
+      }
     }
   }
 
diff --git a/rclcpp_lifecycle/test/mocking_utils/patch.hpp b/rclcpp_lifecycle/test/mocking_utils/patch.hpp
@@ -234,7 +234,27 @@ struct PatchTraits<ID, ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5)>
     mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5);
 };
 
-/// Traits specialization for void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7)
+/// Traits specialization for void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5)
+/// free functions.
+/**
+ * Necessary for Mimick macros to adjust accordingly when the return
+ * type is `void`.
+ *
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID,
+  typename ArgT0, typename ArgT1,
+  typename ArgT2, typename ArgT3,
+  typename ArgT4, typename ArgT5>
+struct PatchTraits<ID, void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5)>
+{
+  mmk_mock_define(
+    mock_type, void, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5);
+};
+
+/// Traits specialization for
+/// ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8)
 /// free functions.
 /**
  * Necessary for Mimick macros to adjust accordingly when the return
@@ -248,15 +268,15 @@ template<size_t ID, typename ReturnT,
   typename ArgT2, typename ArgT3,
   typename ArgT4, typename ArgT5,
   typename ArgT6, typename ArgT7,
-  typename ArgT8, typename ArgT9>
+  typename ArgT8>
 struct PatchTraits<ID, ReturnT(
-    ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8, ArgT9)>
+    ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8)>
 {
   mmk_mock_define(
-    mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8, ArgT9);
+    mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8);
 };
 
-/// Traits specialization for void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5)
+/// Traits specialization for void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7)
 /// free functions.
 /**
  * Necessary for Mimick macros to adjust accordingly when the return
@@ -265,14 +285,17 @@ struct PatchTraits<ID, ReturnT(
  * \tparam ID Numerical identifier of the patch. Ought to be unique.
  * \tparam ArgTx Argument types.
  */
-template<size_t ID,
+template<size_t ID, typename ReturnT,
   typename ArgT0, typename ArgT1,
   typename ArgT2, typename ArgT3,
-  typename ArgT4, typename ArgT5>
-struct PatchTraits<ID, void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5)>
+  typename ArgT4, typename ArgT5,
+  typename ArgT6, typename ArgT7,
+  typename ArgT8, typename ArgT9>
+struct PatchTraits<ID, ReturnT(
+    ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8, ArgT9)>
 {
   mmk_mock_define(
-    mock_type, void, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5);
+    mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8, ArgT9);
 };
 
 /// Generic trampoline to wrap generalized callables in plain functions.
diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp
@@ -152,7 +152,7 @@ TEST_F(TestDefaultStateMachine, empty_initializer) {
 
 TEST_F(TestDefaultStateMachine, empty_initializer_rcl_errors) {
   {
-    auto patch = mocking_utils::patch_and_return(
+    auto patch = mocking_utils::inject_on_return(
       "lib:rclcpp_lifecycle", rcl_lifecycle_state_machine_init, RCL_RET_ERROR);
     EXPECT_THROW(
       std::make_shared<EmptyLifecycleNode>("testnode").reset(),

Original file line number	Diff line number	Diff line change
`@@ -152,7 +152,7 @@ TEST_F(TestDefaultStateMachine, empty_initializer) {`
`152`	`152`
`153`	`153`	`TEST_F(TestDefaultStateMachine, empty_initializer_rcl_errors) {`
`154`	`154`	`{`
`155`		`- auto patch = mocking_utils::patch_and_return(`
	`155`	`+ auto patch = mocking_utils::inject_on_return(`
`156`	`156`	`"lib:rclcpp_lifecycle", rcl_lifecycle_state_machine_init, RCL_RET_ERROR);`
`157`	`157`	`EXPECT_THROW(`
`158`	`158`	`std::make_shared<EmptyLifecycleNode>("testnode").reset(),`