ROS2 foxy C++ subscriber callback function not called after CTRL+C until computer restart
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 ...