How to use rclcpp::wait_for_message() in a node class

Course Support ROS2 in 5 Days C++
,
1

Hello there,

can somebody explain me how to use rclcpp::wait_for_message(), and more specifically inside of a node class please?

I want to make a service node that, when called, reads one time a scan topic and does something depending on what was read. I tries to read the scan during 1sec and restarts trying.

while (!rclcpp::wait_for_message(message, node_, "scan",1)) {
      RCLCPP_INFO(this->get_logger(), "still waiting for message");
}

I tried tu use it but i didn’t get to make it work and the coding assistance didn’t help.
I didn’t found a lot of examples.

Thank you in advance
Pablo

2

You could provide more context here. Which exercise are you trying to solve exactly?

What setup do you have (for the service and the laser scan callback)?

In any case, maybe you need to solve your problem using another approach, because it seems that there is an issue with the wait-for-message implementation.

3

Hello,

I’m trying to do the part 2 of the ros2 cpp project.
Here is my code, for the moment I’m just trying to get only one scan value:

#include "geometry_msgs/msg/twist.hpp"
#include "rclcpp/rclcpp.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 <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);
    srv_ = create_service<FindWall>( "find_wall", std::bind(&ServerNode::find_wall_callback, this, _1, _2));
    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_;
  geometry_msgs::msg::Twist twist;
  rclcpp::TimerBase::SharedPtr timer_;
  float laser_left;
  float laser_right;
  float laser_forward;
  float laser_backward;

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

    

    auto message = sensor_msgs::msg::LaserScan();

    RCLCPP_INFO(this->get_logger(), "starting to wait for message");
    bool scan_found = false;
    while (!scan_found) {
      scan_found = rclcpp::wait_for_message(message, this->shared_from_this(),"/scan",std::chrono::seconds(1));
      RCLCPP_INFO(this->get_logger(), "scan_found = %d", scan_found);
      RCLCPP_INFO(this->get_logger(), "still waiting for message");
    }

      this->laser_left = message.ranges[90];
      this->laser_right = message.ranges[270];
      this->laser_forward = message.ranges[0];
      this->laser_backward = message.ranges[180];
      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);

    
    response->wallfound = true;
  }
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<ServerNode>());
  rclcpp::shutdown();
  return 0;
}

What is strange is that rqt_graph shows that I subscribed to the topic but it doesn’t get any value:

image
image1327×632 69.8 KB

I may have missed something.

-----EDIT------
It appears that my last problem was due to the fact that for some reason the turtle bot didn’t publish values on the topic.
This code worked for me.

1 Like
4

Hello,

It’s a little bit strange it works very well when it wait for message gets the message but sometimes even if i see the scan topic being published with ros2 topic echo /scan my function seems to not get the message…
That’s a little bit problematic because I’m blocked waiting to get a message sometimes so it’s a little bit complicated to debbug sometimes. I dont want to use directly a subscriber because I want to make sur to get a real message before starting to move.

Can someone tell me what I’m doing wrong? @bayodesegun maybe?

thank you in advance.

// Improvement axis:
// start scan subscriber after wait for msg
// and at the end of service callback sub.shutdown();

#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;

  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;
    
    //start_scan_sub();
    
    RCLCPP_INFO(this->get_logger(), "starting to wait for message");
    bool scan_found = false;
    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.2;
      break;
    case 3:
      RCLCPP_INFO(this->get_logger(), "wall behind");
      twist_msg.linear.x = 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];
    }
    /*
    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;
}
5

My advice is to use a subscriber because, as I mentioned earlier, there are some quirks with the wait-for-message implementation in ROS2.

Messages you get from a subscriber are as real as what you get from wait-for-message, even better (because it’s real-time). You can revise your logic such that the robot does not move until you get the first message from the subscriber. Indeed, for real-time applications, the logic for moving the robot should be based on the subscriber callback.

6

Hello,

That works in simulation without using wait_for_message

// Improvement axis:
// start scan subscriber after wait for msg
// and at the end of service callback sub.shutdown();

#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;
}
1 Like
7

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