logging in on_shutdown callback

edit

Hello, noob here!

I'm using ROS2 foxy on Ubuntu 20.04.

I'm trying to log to INFO with the RCLCPP_INFO macro in a on_shutdown() callback function. When exiting the program with ctrl + c I am getting this error message: Failed to publish log message to rosout: publisher's context is invalid

I'm guessing the context is invalid because it has been invalidated with the start of the shutdown procedure? Is it possible to log something in the on_shutdown callback anyway?

Here's my code:

(Side question: Is it good practice in ROS to exit the program with exit()?)

#include "cmd_vel_subscriber.h"

#define DEFAULT_SERVER_PORT 3333

using std::placeholders::_1;


CmdVelSubscriber::CmdVelSubscriber() : Node("cmd_vel_subscriber") {
    this->declare_parameter<std::string>("turtlename", "turtle1");
    this->get_parameter("turtlename", turtlename_);

    //Todo make server_port a parameter
    this->declare_parameter<int>("tcp_port", DEFAULT_SERVER_PORT);
    this->get_parameter("tcp_port", socket_port_);

    std::stringstream ss;
    ss << "/" << turtlename_ << "/cmd_vel";

    socketfd_ = connect_socket();

    subscription_ = this->create_subscription<geometry_msgs::msg::Twist>(ss.str(), 10, std::bind(&CmdVelSubscriber::topic_callback, this, _1));

    rclcpp::on_shutdown(std::bind( &CmdVelSubscriber::test_shutdown, this));
}

void CmdVelSubscriber::test_shutdown() {
  RCLCPP_INFO(this->get_logger(), "Closing connection");
  close(socketfd_);
}

void CmdVelSubscriber::topic_callback(const geometry_msgs::msg::Twist::SharedPtr msg) const {
  RCLCPP_INFO(this->get_logger(), "Sending: '%f'", msg->linear.x);

  char msg_buffer[32];
  sprintf(msg_buffer, "velocity: %f", 25*(msg->linear.x));

  write(socketfd_, msg_buffer, strlen(msg_buffer));

}

int CmdVelSubscriber::connect_socket() {
  struct sockaddr_in sa;
    int socketfd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);

    if (socketfd == -1) {
      perror("cannot create socket");
      exit(EXIT_FAILURE);
    }

    memset(&sa, 0, sizeof sa);

    sa.sin_family = AF_INET;
    sa.sin_port = htons(socket_port_);
    sa.sin_addr.s_addr = htonl(INADDR_ANY);

    if (bind(socketfd,(struct sockaddr *)&sa, sizeof sa) == -1) {
      perror("bind failed");
      close(socketfd);
      exit(EXIT_FAILURE);
    }

    if (listen(socketfd, 1) == -1) {
      perror("listen failed");
      close(socketfd);
      exit(EXIT_FAILURE);
    }

    int connectfd = accept(socketfd, NULL, NULL);

    if (connectfd == -1) {
      perror("accept failed");
      close(socketfd);
      close(connectfd);
      exit(EXIT_FAILURE);
    }

    close(socketfd);

    RCLCPP_INFO(this->get_logger(), "Connection established.");

    return connectfd;
}


int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  auto cmd_node = std::make_shared<CmdVelSubscriber>();

  rclcpp::spin(cmd_node);

  rclcpp::shutdown();
  return 0;
}