Problems with ROS2 C++ Project Part 2

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

Hello

I am currently at part 2 of the ros2 c++ project.

I have somehow issues to run the subscriber callback and the service server callback simultanously.

This means. That when I run my code.
The service callback gets called. But the subscriber callback gets not called. Which means I don’t get the logs which should appear from the subscriber callback, while as my service callback gets stucked in the while loop, because it waits for a valid feedback of the subscriber.

I don’t know what the issue could be, since in the Topics_Quiz it has worked fine to call the subscriber callback.

This is my code:

{
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "my_ros2ject/srv/find_wall.hpp"

#include <chrono>
#include <memory>
#include <thread>

using FindWall = my_ros2ject::srv::FindWall;
using std::placeholders::_1;
using std::placeholders::_2;
using namespace std::this_thread; // sleep_for, sleep_until
using namespace std::chrono;      // nanoseconds, system_clock, seconds

class Find_Wall : public rclcpp::Node
{
public:
  Find_Wall() : Node("find_wall")
  {
    // Create a subscriber to the "scan" topic with a queue size of 10
    subscriber_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
      "scan", 10, std::bind(&Find_Wall::laserscan_callback, this, _1));

    // Create a publisher to the "cmd_vel" topic with a queue size of 10
    publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 10);

    // Create a service named "find_wall" with a callback function
    srv_ = create_service<FindWall>(
        "find_wall", std::bind(&Find_Wall::moving_callback, this, _1, _2));

    // Initialize laserdata
    for (int i = 0; i < 360; i++) {
      laserdata_[i] = 10;
    }

  }

private:
  void laserscan_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg)
  {

    // laserscan callback triggered
    RCLCPP_INFO(this->get_logger(), "Laserscan callback triggered. Number of ranges: %zu", msg->ranges.size());

    // Log the front laser value
    RCLCPP_INFO(this->get_logger(), "Front Laser Value is : '%f'", msg->ranges[180]);

    // Store the laser data in the laserdata_ array
    std::copy(msg->ranges.begin(), msg->ranges.end(), laserdata_.begin());
  }

    // Declare the subscriber, publisher, service and Twist message as private member variables
    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr subscriber_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;
    rclcpp::Service<FindWall>::SharedPtr srv_;
    geometry_msgs::msg::Twist vel_;
    // Create laserdata_ float array
    std::array<float, 360> laserdata_;

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

    RCLCPP_INFO(this->get_logger(), "server_callback started with {}", request.get());

    while (laserdata_[0] == 10) { // Wait until laserdata gets realistic values
        RCLCPP_INFO(this->get_logger(), "laser data not ready");
        sleep_for(1s);
    }

    // Initialize values
    double min_laser = 100000;
    int min_index = 0;

    // Find the minimum laser value and its index
    for (int i = 0; i < 360; i++) {
        if (laserdata_[i] < min_laser) {
            min_laser = laserdata_[i];
            min_index = i;
        }
    }

    // Calculate the desired angle (in degrees) for the 180th value to align with the minimum value
    double desired_angle_deg = 180 - min_index;

    // Calculate the time needed for rotation to achieve the desired angle
    double angular_vel = 0.2;
    double rot_angle_rad = desired_angle_deg * 0.01745; // Convert to radians
    double rot_time = rot_angle_rad / angular_vel;
    if (rot_angle_rad < 0) {
      angular_vel = -angular_vel;
      rot_time = -rot_time;
    }
    RCLCPP_INFO(this->get_logger(), "rot time is : '%f'", rot_time);
    RCLCPP_INFO(this->get_logger(), "desired angle is : '%f'", desired_angle_deg);
    RCLCPP_INFO(this->get_logger(), "rot angle rad is : '%f'", rot_angle_rad);
    RCLCPP_INFO(this->get_logger(), "angular vel is : '%f'", angular_vel);

    // Rotate the robot
    vel_.angular.z = angular_vel;
    publisher_->publish(vel_);
    sleep_for(milliseconds(static_cast<int>(rot_time * 1000)));
    vel_.angular.z = 0;
    publisher_->publish(vel_);
    RCLCPP_INFO(this->get_logger(), "robot has rotated");
    sleep_for(1s);


    if (laserdata_[180] < min_laser + 0.1 && laserdata_[180] > min_laser - 0.1) {
      // Set the response success variable to true
      response->wallfound = true;
    } else {
      response->wallfound = false;
    }
    
  }
};

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

  // Create an instance of the Find_Wall class
  auto node = std::make_shared<Find_Wall>();

  // Create a loop rate of 1 Hz
  rclcpp::WallRate loop_rate(1);

  while (rclcpp::ok()) {
    // Spin once to process any callbacks
    rclcpp::spin_some(node);

    // Sleep to maintain the loop rate
    loop_rate.sleep();
  }

  rclcpp::shutdown();
  return 0;
}

{

Does somebody have an idea, what the issue is? It is probably something really silly, which I am overlooking.

Would be very thankful for any help.

Cheers

2

Hi @Dkae ,

The solution for your problem is very simple. You need to implement Executors and Callback Groups.

In your program you have two callbacks that needs to run in parallel - laser callback and service callback. So classify your callbacks under ReentrantCallbackGroup with MultiTheadedExecutor.

That should fix your problem.

Regards,
Girish

3

Hi @girishkumar.kannan

Thanks for the advice. Yeah I was thinking about this.

Apperently the create_subscription does’nt have the option to add it to a callback group, so I could just add the create_service

So I implemented it like this, and now it works.

{
.....

class Find_Wall : public rclcpp::Node {
public:
  Find_Wall() : Node("find_wall") {
    // Initialize one Reentrant callback group object
    callback_group_ =
        this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);

    // Create a subscriber to the "scan" topic with a queue size of 10
    subscriber_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
        "scan", 10,
        std::bind(&Find_Wall::laserscan_callback, this, _1));

    // Create a publisher to the "cmd_vel" topic with a queue size of 10
    publisher_ =
        this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 10);

    // Create a service named "find_wall" with a callback function
    srv_ = this->create_service<FindWall>(
        "find_wall", std::bind(&Find_Wall::moving_callback, this, _1, _2),
        rmw_qos_profile_services_default, callback_group_);

    // Initialize laserdata
    for (int i = 0; i < 360; i++) {
      laserdata_[i] = 10;
    }
  }

private:
  void laserscan_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg) {

   ......

  }

  // Declare the subscriber, publisher, service and Twist message as private
  // member variables
  rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr subscriber_;
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;
  rclcpp::Service<FindWall>::SharedPtr srv_;
  geometry_msgs::msg::Twist vel_;
  rclcpp::CallbackGroup::SharedPtr callback_group_;
  // Create laserdata_ float array
  std::array<float, 360> laserdata_;

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

    ......

  }
};

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

  // Create an instance of the Find_Wall class
  auto node = std::make_shared<Find_Wall>();

  // Initialize one MultiThreadedExecutor object
  rclcpp::executors::MultiThreadedExecutor executor;
  executor.add_node(node);
  executor.spin();

  rclcpp::shutdown();
  return 0;
}

{

Thanks a lot for your help!

4

Hi @Dkae ,

create_subscription does have a callback group option. You can do it this way:

// library includes:
#include "rclcpp/qos.hpp"
#include "rmw/qos_profiles.h"
#include "rclcpp/callback_group.hpp"

// main code - subscriber with callback group defined
auto scan_sub_qos = rclcpp::QoS(rclcpp::KeepLast(10), rmw_qos_profile_sensor_data);
rclcpp::SubscriptionOptions scan_sub_options;
scan_sub_options.callback_group = create_callback_group(rclcpp::CallbackGroupType::Reentrant);
scan_sub = this->create_subscription<LaserScan>(
           "/scan", scan_sub_qos,
           std::bind(&WallFollowerRobot::scan_callback, this, std::placeholders::_1),
           scan_sub_options);

Check the documentation for more info: rclcpp: rclcpp::Node Class Reference

Regards,
Girish

1 Like
5

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