How to CTRL-C to end cleanly multithread node

Course Support ROS2 Basics in 5 Days Humble (C++)
, ,
1

Hello there,

Can someone explain me why when I do CTRL-C I have the error below?
On the real robot the robot continues to move and even stopping with teleop it doesn’t stop until some time.

Thank you in advance for your help.

Pablo

user:~/ros2_ws$ ros2 launch wall_following_pkg find_wall.launch.py
[INFO] [launch]: All log files can be found below /home/user/.ros/log/2023-06-30-07-43-56-920300-2_xterm-5348
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [find_wall_service-1]: process started with pid [5350]
[find_wall_service-1] [INFO] [1688111037.068505519] [find_wall_service]: Robot simulated = 1
[find_wall_service-1] [INFO] [1688111042.532605931] [find_wall_service]: starting to wait for message
[find_wall_service-1] [INFO] [1688111042.533416142] [find_wall_service]: still waiting for message
[find_wall_service-1] [INFO] [1688111043.533600894] [find_wall_service]: message found
[find_wall_service-1] [INFO] [1688111043.533682551] [find_wall_service]: min value is at index 3
[find_wall_service-1] [INFO] [1688111043.533700081] [find_wall_service]: wall behind
^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
[find_wall_service-1] [INFO] [1688111050.101255646] [rclcpp]: signal_handler(signal_value=2)
[ERROR] [find_wall_service-1]: process[find_wall_service-1] failed to terminate '5' seconds after receiving 'SIGINT', escalating to 'SIGTERM'
[INFO] [find_wall_service-1]: sending signal 'SIGTERM' to process[find_wall_service-1]
[ERROR] [find_wall_service-1]: process has died [pid 5350, exit code-15, cmd '/home/user/ros2_ws/install/wall_following_pkg/lib/wall_following_pkg/find_wall_service --ros-args -r __node:=find_wall_service'].

#include "geometry_msgs/msg/twist.hpp"
#include "rcl/event.h"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/subscription_base.hpp"
#include "rclcpp/subscription_options.hpp"
#include "rclcpp/utilities.hpp"
#include "rclcpp/wait_for_message.hpp"
#include "sensor_msgs/msg/detail/laser_scan__struct.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "wall_following_pkg/srv/find_wall.hpp"

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

using FindWall = wall_following_pkg::srv::FindWall;
using std::placeholders::_1;
using std::placeholders::_2;
using namespace std::chrono_literals;

const bool SIMULATION = true;

class ServerNode : public rclcpp::Node {
public:
  ServerNode() : Node("find_wall_server") {
    RCLCPP_INFO(this->get_logger(), "Robot simulated = %d", SIMULATION);

    callback_group_1_ = this->create_callback_group(
        rclcpp::CallbackGroupType::MutuallyExclusive);
    callback_group_2_ = this->create_callback_group(
        rclcpp::CallbackGroupType::MutuallyExclusive);

    srv_ = create_service<FindWall>(
        "find_wall", std::bind(&ServerNode::find_wall_callback, this, _1, _2),
        rmw_qos_profile_services_default, callback_group_1_);

    publisher_ =
        this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1);

    this->laser_left = 0.0;
    this->laser_right = 0.0;
    this->laser_forward = 0.0;
    this->laser_backward = 0.0;
  }

private:
  rclcpp::Service<FindWall>::SharedPtr srv_;
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::CallbackGroup::SharedPtr callback_group_1_;
  rclcpp::CallbackGroup::SharedPtr callback_group_2_;
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;
  rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr subscription_;
  geometry_msgs::msg::Twist twist_msg;
  float laser_left;
  float laser_right;
  float laser_forward;
  float laser_backward;
  bool scan_found;

  void find_wall_callback(const std::shared_ptr<FindWall::Request> request,
                          const std::shared_ptr<FindWall::Response> response) {
    // non intrusive function to avoid unused value warning
    typeid(request).name();

    auto scan_message = sensor_msgs::msg::LaserScan();
    twist_msg.linear.x = 0.0;
    twist_msg.angular.z = 0.0;

    scan_found = false;
    RCLCPP_INFO(this->get_logger(), "starting to wait for message");
    start_scan_sub();
    while (!scan_found) {
      RCLCPP_INFO(this->get_logger(), "still waiting for message");
      rclcpp::sleep_for(1s);
    }
    RCLCPP_INFO(this->get_logger(), "message found");
    /*RCLCPP_INFO(this->get_logger(), "starting to wait for message");
    while (!scan_found) {
      scan_found =
          rclcpp::wait_for_message(scan_message, this->shared_from_this(),
                                   "/scan", std::chrono::seconds(1));
      //RCLCPP_INFO(this->get_logger(), "still waiting for message");
    }
    RCLCPP_INFO(this->get_logger(), "scan_found = %d", scan_found);
    this->laser_left = scan_message.ranges[90];
    this->laser_right = scan_message.ranges[270];
    this->laser_forward = scan_message.ranges[0];
    this->laser_backward = scan_message.ranges[180];
    */

    std::vector<float> laser_values = {this->laser_left, this->laser_right,
                                       this->laser_forward,
                                       this->laser_backward};

    int min_index = find_min_index(laser_values);
    RCLCPP_INFO(this->get_logger(), "min value is at index %d", min_index);
    switch (min_index) {
    case 0:
      RCLCPP_INFO(this->get_logger(), "wall on the left");
      twist_msg.angular.z = 0.2;
      break;
    case 1:
      RCLCPP_INFO(this->get_logger(), "wall on the right");
      if (laser_right < 0.3) {
        RCLCPP_INFO(this->get_logger(), "already in position, end of service");
        response->wallfound = true;
        return;
      }
      twist_msg.angular.z = -0.2;
      break;
    case 2:
      RCLCPP_INFO(this->get_logger(), "wall in front");
      twist_msg.linear.x = 0.05;
      break;
    case 3:
      RCLCPP_INFO(this->get_logger(), "wall behind");
      twist_msg.angular.z = 0.2;
      break;
    }

    // start_scan_sub();

    while (laser_forward > laser_backward || laser_forward > laser_left ||
           laser_forward > laser_right) {
      publisher_->publish(twist_msg);
      /*RCLCPP_INFO(this->get_logger(), "[FORWARD] = '%f'",
      this->laser_forward); RCLCPP_INFO(this->get_logger(), "[LEFT] = '%f'",
      this->laser_left); RCLCPP_INFO(this->get_logger(), "[RIGHT] = '%f'",
      this->laser_right); RCLCPP_INFO(this->get_logger(), "[FORWARD] = '%f'",
      this->laser_forward); RCLCPP_INFO(this->get_logger(), "[BACKWARD] = '%f'",
                  this->laser_backward);
      */
      // rclcpp::sleep_for(1s);
    }
    this->twist_msg.linear.x = 0;
    this->twist_msg.angular.z = 0;
    publisher_->publish(twist_msg);
    RCLCPP_INFO(this->get_logger(), "facing the wall, going to approach");
    this->twist_msg.linear.x = 0.05;
    this->twist_msg.angular.z = 0;
    while (laser_forward > 0.3) {
      publisher_->publish(twist_msg);
    }
    this->twist_msg.linear.x = 0;
    this->twist_msg.angular.z = 0;
    publisher_->publish(twist_msg);
    RCLCPP_INFO(this->get_logger(),
                "close to the wall, rotating to start position");
    this->twist_msg.linear.x = 0;
    this->twist_msg.angular.z = 0.2;
    while (laser_right > laser_backward || laser_right > laser_left ||
           laser_forward < laser_right) {
      publisher_->publish(twist_msg);
      /*RCLCPP_INFO(this->get_logger(), "[FORWARD] = '%f'",
      this->laser_forward); RCLCPP_INFO(this->get_logger(), "[LEFT] = '%f'",
      this->laser_left); RCLCPP_INFO(this->get_logger(), "[RIGHT] = '%f'",
      this->laser_right); RCLCPP_INFO(this->get_logger(), "[FORWARD] = '%f'",
      this->laser_forward); RCLCPP_INFO(this->get_logger(), "[BACKWARD] = '%f'",
                  this->laser_backward);
      */
      rclcpp::sleep_for(1s);
    }
    this->twist_msg.linear.x = 0;
    this->twist_msg.angular.z = 0;
    publisher_->publish(twist_msg);
    subscription_.reset();
    RCLCPP_INFO(this->get_logger(), "in position, end of service");
    response->wallfound = true;
  }

  int find_min_index(std::vector<float> laser_values) {
    int length = laser_values.size();
    // RCLCPP_INFO(this->get_logger(), "length= %d", length);
    float min = laser_values[0];
    int index = 0;
    for (int i = 0; i < length; i++) {
      // RCLCPP_INFO(this->get_logger(), " %f\n", laser_values[i]);
      if (laser_values[i] < min) {
        min = laser_values[i];
        index = i;
      }
    }
    return index;
  }

  void start_scan_sub() {
    rclcpp::SubscriptionOptions sub_options;
    sub_options.callback_group = callback_group_2_;
    subscription_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
        "/scan", rclcpp::SensorDataQoS(),
        std::bind(&ServerNode::scan_callback, this, _1), sub_options);
  }

  void scan_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg) {
    //RCLCPP_INFO(this->get_logger(), "SCAN CALLBACK");
    if (SIMULATION) {
      // RCLCPP_INFO(this->get_logger(), "number of angle values = '%d'",
      // msg->ranges.size());
      //-------------------------SIM SCAN VALUES 360 values
      this->laser_left = msg->ranges[90];
      this->laser_right = msg->ranges[270];
      this->laser_forward = msg->ranges[0];
      this->laser_backward = msg->ranges[180];
    } else {
      //---------------real turtle bot values 720 values angle_min:
      //-3.1241390705108643 angle_max: 3.1415927410125732
      this->laser_left = msg->ranges[540];
      this->laser_right = msg->ranges[180];
      this->laser_forward = msg->ranges[360];
      this->laser_backward = msg->ranges[0];
    }
    scan_found = true;
    /*
    RCLCPP_INFO(this->get_logger(), "[LEFT] = '%f'", this->laser_left);
    RCLCPP_INFO(this->get_logger(), "[RIGHT] = '%f'", this->laser_right);
    RCLCPP_INFO(this->get_logger(), "[FORWARD] = '%f'", this->laser_forward);
    RCLCPP_INFO(this->get_logger(), "[BACKWARD] = '%f'", this->laser_backward);
    */
  }
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);

  std::shared_ptr<ServerNode> find_wall_server = std::make_shared<ServerNode>();

  rclcpp::executors::MultiThreadedExecutor executor;

  executor.add_node(find_wall_server);
  executor.spin();

  rclcpp::shutdown();
  return 0;
}
2

Hello @pabled91 ,

Try removing the node from the executor before shutting down:

executor.add_node(find_wall_server);
executor.spin();
executor.remove_node(find_wall_server);
rclcpp::shutdown();
3

Hello @albertoezquerro,

I tryed it and it didn’t work for me.

4

Hello @pabled91 ,

I was testing your code but couldn’t reproduce the issue. Could you please send your package to my e-mail (aezquerro@theconstructsim.com) and tell me what commands should I execute in order to reproduce the issue?

5

This problem seems to happen for example when there is a while:
here in the code it is when the scan is not already available.

I already had this kind of message in other exercices where a while was used to make compute data.

I’l sending you the email.

6

This topic was automatically closed 5 days after the last reply. New replies are no longer allowed.