ROS2 foxy C++ subscriber callback function not called after CTRL+C until computer restart

asked 2022-10-11 12:47:08 -0500

dvy gravatar image

updated 2022-12-14 13:02:06 -0500

System: ROS2 foxy on Ubuntu 20.04

I have two scripts in a package: one python (one publisher + 3 subscriber), one c++ (1 publisher (not called through a timer, publishes only when I want from inside the subscriber callback function + 1 subscriber). They both communicate to each other through sockets. Server is implemented on the c++ script and client on python. First, I run the c++ script, it waits for the client to run, once socket connection is established, both create publisher/ subscriber node and should process the data on topics by running their respective callback functions. Both scripts were working fine until I pressed CTRL+C for both to stop the execution and start again but then the c++ subscriber callback function is not ran (confirmed through std::cout) while the python's subscriber work fine and socket connection is established as well. However, if I create another python script containing just the socket client and run it along with c++ code, its subscriber works fine (weird!)

C++ code

//#pragma once
#include <memory>
#include <iostream>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"

#include <fstream>
#include <sstream>
#include <string>
#include <math.h>
#include <vector>

// Include files for socket
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h>
#include <unistd.h>
#include <arpa/inet.h>
#include <string.h>
#include <stdio.h>

#define SERVER_PORT htons(65432)
char socket_buffer[1000];
int serverSock;
int clientSock;
socklen_t sin_size=sizeof(struct sockaddr_in);
sockaddr_in clientAddr;



class MinimalSubscriber : public rclcpp::Node
{

  struct Config {
        int    num;
      };
  Config config;

  public:

    int detect_obstacle(float xmin, float ymin, float xmax, float ymax, float *angle_individual_radian_ptr) const
    {
      // some function
    }

    MinimalSubscriber()    //Constructor which has the same name as that of class followed by a parentheses. A constructor in C++ is a special method that is automatically called when an object of a class is created. 
    : Node("minimal_subscriber")
    {
      std::cout << "Reached start of Public" << std::endl;


      std::ifstream config_file_path("/config/file/path");
      std::string line;
      while (std::getline(config_file_path, line)) {
          std::istringstream sin(line.substr(line.find("=") + 1));
          if (line.find("num") == 0)
              sin >> config.num;
      }

      publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("filtered_cloud", 10);

      subscription_ = this->create_subscription<sensor_msgs::msg::PointCloud2>
      ("velodyne_points", rclcpp::SensorDataQoS(), std::bind(&MinimalSubscriber::topic_callback, this, std::placeholders::_1));

      std::cout << "Reached end of Public" << std::endl;      
    }

  private:


    void topic_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg_ptr) const
    {
        std::cout << "Hello" << std::endl;
        // some code in callback function
          }

    }
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher_;

};

int main(int argc, char * argv[])
{

  int serverSock=socket(AF_INET, SOCK_STREAM, 0);
  sockaddr_in serverAddr;
  serverAddr.sin_family = AF_INET;
  serverAddr.sin_port = SERVER_PORT;
  serverAddr.sin_addr.s_addr = inet_addr("127.0.0.1");
  /* bind (this socket, local address, address length)
     bind server socket (serverSock) to server address (serverAddr).  
     Necessary so that server can use a specific port */ 
  bind(serverSock, (struct sockaddr*)&serverAddr, sizeof(struct sockaddr));
  // wait for a client
  /* listen (this socket, request queue length) */
  std::cout << "Waiting to listen on server socket ...
(more)
edit retag flag offensive close merge delete