[ros-users] [Discourse.ros.org] How to shutdown and reinitialize a publisher node in ROS 2

William Woodall ros.discourse at gmail.com
Sat Mar 3 00:06:02 UTC 2018



I reformatted you code to be easier to read and added some missing includes that prevent it from compiling:

```c++
#include <iostream>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"

#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;

int main(int argc, char * argv[] )
{
  auto node = rclcpp::Node::make_shared("talker1");
  auto Pub = node->create_publisher<std_msgs::msg::String>("chatter1", rmw_qos_profile_default);
  rclcpp::WallRate loop_rate(1);
  size_t count = 1;
  auto msg = std::make_shared<std_msgs::msg::String>();

  while(1) {
    rclcpp::init(argc, argv);

    msg->data = "Hello World" + std::to_string(count++);

    std::cout << msg->data << ":msg_no:" << count << std::endl;
    Pub->publish(msg); 
    loop_rate.sleep();
  }

  rclcpp::shutdown();
  return 0;
}
```

The first issue I see (and the reason for your error) is that you are trying to create a node before `rclcpp::init()` has been called, which is implied in the error message though it could be more specific to rclcpp that it currently is:

> what():  failed to create interrupt guard condition: rcl_init() has not been called, at ...

But even if you move the node creation into the while loop, and after the `rclcpp::init()`, the next issue I see is that you are actually calling init many times, while only calling shutdown once. This will definitely be an issue, and I modified your example to see what would happen :slight_smile:

```
#include <iostream>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"

#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;

int main(int argc, char * argv[] )
{
  rclcpp::WallRate loop_rate(1);
  size_t count = 1;
  auto msg = std::make_shared<std_msgs::msg::String>();

  while(1) {
    rclcpp::init(argc, argv);

    auto node = rclcpp::Node::make_shared("talker1");
    auto Pub = node->create_publisher<std_msgs::msg::String>("chatter1", rmw_qos_profile_default);

    msg->data = "Hello World" + std::to_string(count++);

    std::cout << msg->data << ":msg_no:" << count << std::endl;
    Pub->publish(msg); 
    loop_rate.sleep();
  }

  rclcpp::shutdown();
  return 0;
}
```

When I run it I get:

```
% /Users/william/ros2_ws/install_release/lib/demo_nodes_cpp/talker
Hello World1:msg_no:2
libc++abi.dylib: terminating with uncaught exception of type std::runtime_error: failed to initialize rmw implementation: rcl_init called while already initialized, at /Users/william/ros2_ws/src/ros2/rcl/rcl/src/rcl/rcl.c:68
```

The important thing being:

> rcl_init called while already initialized

Which again could be more `rclcpp` explicit, but should also be enough of an indication as to what the issue is.

Finally, however, I'd like to ask why you think you need to init and shutdown repeatedly? It's potentially a valid use case, but I get from your example why that's necessary.

Assuming for a second that's actually what you want to do, I modified your example one more time to move the shutdown into the while loop as well:

```c++
#include <iostream>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"

#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;

int main(int argc, char * argv[] )
{
  rclcpp::WallRate loop_rate(1);
  size_t count = 1;
  auto msg = std::make_shared<std_msgs::msg::String>();

  while(1) {
    rclcpp::init(argc, argv);

    auto node = rclcpp::Node::make_shared("talker1");
    auto Pub = node->create_publisher<std_msgs::msg::String>("chatter1", rmw_qos_profile_default);

    msg->data = "Hello World" + std::to_string(count++);

    std::cout << msg->data << ":msg_no:" << count << std::endl;
    Pub->publish(msg); 
    loop_rate.sleep();

    rclcpp::shutdown();
  }

  return 0;
}
```

But, that results in this still:

> terminating with uncaught exception of type std::runtime_error: failed to initialize rmw implementation: rcl_init called while already initialized

Unfortunately doing `init-shutdown-init` does not currently work in rclcpp and is a known issue, see:

- https://github.com/ros2/rclcpp/pull/152
- https://github.com/ros2/rclcpp/issues/154

This does work one layer down in `rcl` (which is used by both `rclcpp` and `rclpy`), but specifically in `rclcpp`, there was some disagreement about how to deterministically shutdown in a multi-threaded environment, and so it was not resolved yet. It can be and should be but it's not the case at the moment.

So if you truly do need to init and shutdown repeatedly in the same process we'll have to first fix `rclcpp` to support that case, which would be a great contribution if you or anyone else has time to help with it :smile:.





---
[Visit Topic](https://discourse.ros.org/t/how-to-shutdown-and-reinitialize-a-publisher-node-in-ros-2/4090/2) or reply to this email to respond.




More information about the ros-users mailing list