Advice for accessing laser scan callback from service server - ROS2 Basics C++ Course

[sploeger]

I am working on Part II of the ROS2 Basics in 5 Days (C++) course, Create a ROS service that looks for the wall. I am looking for some general advice on how to go about it.

I know that I will need at least two callbacks, 1 for actually running the service once it is called, and a 2nd for getting laser scan data from the /scan topic.

Both of these callbacks will need to be run at the same time, as the laser scan data will need to be updated as the wall find server executes its steps (find shortest distance, point at wall, drive to wall, rotate until the side of the robot is pointed at the wall).

In unit 4, we learned about executors and callbacks. Since I want 2 callbacks executing in the same node, we will need to use callback groups. I have some code prepared where I use two callback groups in a MutuallyExclusiveCallbackGroup. However, I think that since my service callback group has blocking loops in it, it is still stopping the laser scan data from updating. Am I understanding callback groups correctly, and is it possible that my blocking wallfind_callback is blocking the scan_callback from running? What are other ways to solve this problem? Thanks!

Here is the first part of my wallfind_callback that runs when the service is called. As you can see here (at the bottom), there is a for loop that runs until the front of the robot is facing the wall. Even though this callback is in a MutuallyExclusiveCallbackGroup, I think this blocking part of the code stops the laser scan callback from updating the scan values.

void wallfind_callback(const std::shared_ptr<FindWall::Request> request,
                         std::shared_ptr<FindWall::Response> response) {

    auto message = geometry_msgs::msg::Twist();

    while (scan_data_.ranges.empty()) {
      RCLCPP_INFO(this->get_logger(), "Waiting");
    }
    float min_distance = 99;
    size_t min_distance_index = 0;

    RCLCPP_INFO(this->get_logger(), "Finding shortest distance");
    for (size_t i = 1; i < scan_data_.ranges.size();
         ++i) { // find the minimum distance
      if (scan_data_.ranges[i] < min_distance) {
        min_distance = scan_data_.ranges[i];
        min_distance_index = i;
      }
    }
    RCLCPP_INFO(this->get_logger(), "Shortest distance: %f at range %f",
                min_distance, min_distance_index);

    RCLCPP_INFO(this->get_logger(), "Rotating until front points at wall");
    // rotate until ray[0] is pointing at the wall
    while (scan_data_.ranges[0] > (min_distance - buffer)) {
      message.linear.x = 0.0;
      message.angular.z = 0.25;
      publisher_->publish(message);
      RCLCPP_INFO(this->get_logger(), "Shortest distance: %f", min_distance);
    }
... rest of code continues

Here is the laser scan callback:

void scan_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg) {
    scan_data_ = *msg;
    RCLCPP_INFO(this->get_logger(),
                "Hello from laser callback. Front distance: %f",
                scan_data_.ranges[0]);
  }

[roalgoal]

Hi, if you want your callbacks to not block each other, you can run them in different threads using rclcpp::executors::MultiThreadedExecutor.

Check out our live class where we explain it to see how to implement them:

Multithreading in ROS2 (C++) - hands-on Open Class | The Construct

[sploeger]

Thanks @roalgoal
I am using rclcpp::executors::MultiThreadedExecutor but I suspect there is still something wrong with my callback. However, is it possible that the while loop in my service callback stops scan_callback function is not being called because the executor is not spinning. This means that no new LaserScan messages are being processed and scan_data_ is not being updated?

The loop in question within my service callback that I think is blocking:

 RCLCPP_INFO(this->get_logger(), "Rotating until front points at wall");
    // rotate until ray[0] is pointing at the wall
    while (scan_data_.ranges[0] > (min_distance - buffer)) {
      message.linear.x = 0.0;
      message.angular.z = 0.25;
      publisher_->publish(message);
      RCLCPP_INFO(this->get_logger(), "Shortest distance: %f at range %f",
                  min_distance, min_index);
    }

Here is my callback group:

    callback_group_1 =
        this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);

As you can see, the service and the scan callback are in the same Reentrant callback group.

    rclcpp::SubscriptionOptions options;
    options.callback_group = callback_group_1;

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

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

    // create laser scan sub
    scan_subscriber_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
        "/scan", 10,
        std::bind(&ServerNode::scan_callback, this, std::placeholders::_1),
        options);
  }

Here is the scan callback that I think is never running/being blocked:

 void scan_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg) {
    scan_data_ = *msg;
    RCLCPP_INFO(this->get_logger(),
                "Hello from laser callback. Front distance: %f",
                scan_data_.ranges[0]);
  }

In my main, I add the node to an executor as you suggested:

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

  rclcpp::executors::MultiThreadedExecutor executor;
  executor.add_node(node);
  executor.spin();

  rclcpp::shutdown();
  return 0;
}