ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Publish message on destructor works only on rclpy

asked 2020-10-23 08:43:29 -0500

Erysme gravatar image

Hi everyone,

In a test node I just have one publisher that publishes the node state once on the constructor and once on the destructor of my node class. I figured out that this works on Python3 (rclpy) and not on C++ (rclcpp).

import rclpy
from rclpy.node import Node
import time

from std_msgs.msg import String


class Test(Node):

    def __init__(self):
        super().__init__('test')

        self.publisher = self.create_publisher(String, 'state_test', 10)
        msg = String()
        msg.data = "Node started"
        self.publisher.publish(msg)

    def __del__(self):
        msg = String()
        msg.data = "Node crashed"
        self.publisher.publish(msg)

def main(args=None):
    rclpy.init(args=args)

    node_test = Test()
    rclpy.spin(node_test)

    node_test.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

C++ code:

#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;

class Test : public rclcpp::Node
{
  public:
    Test()
    : Node("test"), count_(0)
    {
      pub_state = this->create_publisher<std_msgs::msg::String>("state_test", 10);
      auto message = std_msgs::msg::String();
      message.data = "Node started";
      pub_state->publish(message);
    }

    ~Test()
    {
      auto message = std_msgs::msg::String();
      message.data = "Node crashed";
      pub_state->publish(message);
    }

  private:
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_state;
    size_t count_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<Test>());
  rclcpp::shutdown();
  return 0;
}

Why is the result different on those two examples ?

edit retag flag offensive close merge delete

Comments

1

Isn't that obvious? In python you are explicitly calling the destruction before calling shutdown. In C++ you don't do that, so the destructor will only be called when the variable goes out of scope, i.e., after the shutdown when it can't publish anymore.

chfritz gravatar image chfritz  ( 2020-10-23 12:44:59 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-10-23 13:07:34 -0500

updated 2020-10-26 10:15:48 -0500

As pointed out in the comment, in your C++ code the object is only destroyed at the end of the program when the scope for the shared pointed ends. At that time ros has already shut down and you can no longer publish. However, you can force the destruction of the shared pointer before shutdown, but limiting its scope a bit more. I haven't tested this, but the following should do the trick:

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  {
    auto node = std::make_shared<Test>();
    rclcpp::spin(node);
  }
  rclcpp::spin_some(); // need to let ros spin a bit more to process the publish queued by the destructor
  rclcpp::shutdown();
  return 0;
}
edit flag offensive delete link more

Comments

Thanks for your answer, I agree that in python the destructor is "manually" called. Your script does not work for me. I am still searching a way to publish a message when the node is destroyed.

Erysme gravatar image Erysme  ( 2020-10-26 09:52:57 -0500 )edit

Can you say more about why it doesn't work? Does it not compile/run, or does it still fail to call the destructor?

chfritz gravatar image chfritz  ( 2020-10-26 09:54:41 -0500 )edit
1

It enters the ~Test() destructor and goes out (I can print something in), but the message is not published

Erysme gravatar image Erysme  ( 2020-10-26 10:11:22 -0500 )edit

ah, right, there was something missing from my answer. You need to call spin_some (or some other spin method) once more after the destructor in order for the queued up publication to go out. I've updated the answer. Could you try this?

chfritz gravatar image chfritz  ( 2020-10-26 10:18:01 -0500 )edit

Yeah thats a nice idea. I tested it. I got compilation error : spin_some require an argument. I tryed rclcpp::spin_some(node); but there were an arror because node have been destroyed.

Edit : rclcpp::on_shutdown(linked_function); seems to be the way to do what I want. I test different implementation and will post a code

Erysme gravatar image Erysme  ( 2020-10-26 10:27:59 -0500 )edit

I tryed rclcpp::on_shutdown() to link a fonction that is called on shutdown. Unfortunately rclcpp::ok() return False in this function.

Ros communications seems to be closed just after the exit of the spin function.

Does somebody have an idea ?

Erysme gravatar image Erysme  ( 2020-10-28 12:27:18 -0500 )edit

I think you've tried this approach long enough. I would just create a manual destroy method in the class and call that manually after the spin.

chfritz gravatar image chfritz  ( 2020-10-29 15:19:47 -0500 )edit

Question Tools

2 followers

Stats

Asked: 2020-10-23 08:43:29 -0500

Seen: 1,498 times

Last updated: Oct 28 '20