Robotics StackExchange | Archived questions

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" << std::endl;
  listen(serverSock,1);
  std::cout << "Listen complete" << std::endl;
  clientSock=accept(serverSock,(struct sockaddr*)&clientAddr, &sin_size);
  std::cout << "Socket connected" << std::endl;
  std::cout << "Inside main function now" << std::endl;
  std::cout << "Inside main function now" << std::endl;
  rclcpp::init(argc, argv);
  std::cout << "RCLCPP init complete" << std::endl;
  rclcpp::spin(std::make_shared<MinimalSubscriber>());
  std::cout << "RCLCPP spin line executed" << std::endl;
  rclcpp::shutdown();
  std::cout << "RCLCPP shutdown completed" << std::endl;
  return 0;
}

After running this c++ executable, I run the below Python script with which c++ subscriber callback was working initially until I pressed CTRL+C on both terminals to test them again. After that, they won't run until I restart my computer.

from logging.handlers import BaseRotatingHandler
from threading import local
import time
import math
import subprocess


from geometry_msgs.msg import Pose, PoseStamped, Twist
from geometry_msgs.msg import PoseWithCovarianceStamped
from sensor_msgs.msg import NavSatFix, Imu, PointCloud2
from sensor_msgs.msg import MagneticField
from nav_msgs.msg import Odometry
from lifecycle_msgs.srv import GetState
from nav2_msgs.action import NavigateThroughPoses, NavigateToPose
import sys
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
from rclpy.qos import QoSProfile
from rclpy.duration import Duration


from pathlib import Path
from launch.substitutions import PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare

import socket
import struct
import errno
import sys

HOST = "127.0.0.1"  # The server's hostname or IP address
PORT = 65432  # The port used by the server

class CustomNavigator(Node):
    def __init__(self):
        super().__init__(node_name='custom_navigator')


        self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.s.connect((HOST, PORT))
        self.s.setblocking(False)
        print("Socket connected")


        self.gps_callback_done = 0
        self.gps_subscription = self.create_subscription(NavSatFix,'gps/data',self.gps_callback,10)
        self.gps_subscription  # prevent unused variable warning

        self.imu_subscription = self.create_subscription(Imu,'imu/data',self.imu_callback,10)
        self.imu_subscription  # prevent unused variable warning

        self.odom_subscription = self.create_subscription(Odometry,'odom',self.odom_callback,10)
        self.odom_subscription  # prevent unused variable warning

        self.velocity_publisher_ = self.create_publisher(Twist, 'husky_velocity_controller/cmd_vel_unstamped', 10)
        timer_period = 1/100  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)
        print("Entered in class")

    def imu_callback(self, msg):
        #Some code

    def gps_callback(self, msg):
        #Some code



    def odom_callback(self, msg):
        #Some code


    def timer_callback(self):
        #Some code           

def main(argv=sys.argv[1:]):
    rclpy.init()
    custom_navigator = CustomNavigator()
    rclpy.spin(custom_navigator)    

if __name__ == '__main__':
  main()

With the combination of above two scripts, c++ subscriber call function does not get called always. If I restart my computer, it does.

However, If I run c++ executable along with the below client code for socket connection, the subscriber callback for velodyne_points works each time!

from base64 import decode
import socket
import errno
import sys

HOST = "127.0.0.1"  # The server's hostname or IP address
PORT = 65432  # The port used by the server
buffer_size = 1
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.connect((HOST, PORT)) #Script waits here
print("Socket connected")
s.setblocking(False)

while(1==1):

    try:
        data = s.recv(4)
    except socket.error as e:
        err = e.args[0]
        if err == errno.EAGAIN or err == errno.EWOULDBLOCK:
            continue
        else:
            print("Real error occured: ", e)
            sys.exit(1)

I am not able to find out why is this happening? Please suggest.

Thanks, Dev

Asked by dvy on 2022-10-11 12:47:08 UTC

Comments

Answers