How to use subscriber values in a service server? ROS1 C++

Course Support ROS Basics In 5 Days (C++)
, ,
1

I am working on the services section (finding the wall) of the Rosject related to the ROS Basics in 5 Days C++ course. I am trying to get laser values from a subscriber and utilize them within a service server. However, it seems the subscriber is only called once and not continuously. How can I integrate a subscriber and publisher into my service callback?

#include "ros/duration.h"
#include "ros/rate.h"
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "rosject/FindWall.h"
#include <bits/stdc++.h>
#include <sensor_msgs/LaserScan.h>

class controller {

    public:
        controller(){
            pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1000);
             sub = nh.subscribe("/scan", 1, &controller::laserCallback, this);
            my_service = nh.advertiseService("/find_wall", &controller::find_Callback, this); 
        }

        void laserCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
        {
            // Identify which laser ray is the shortest one. Assume that this is the one pointing to a wall.
            laser_readings.clear();

            // creating vector of laser distances
            for (int i = 0; i < msg -> ranges.size(); i++) {
                laser_readings.push_back(msg -> ranges[i]);
            }
                
            // finding index of min value in vector
            laser_min = distance(laser_readings.begin(), min_element(laser_readings.begin(),laser_readings.end()));
        }

        bool find_Callback(rosject::FindWall::Request  &req,
                           rosject::FindWall::Response &res)
        {  
            ROS_INFO("find_Callback has been called"); // We print an string whenever the Service gets called

            vel.angular.z = 0.05;
            pub.publish(vel);

            while(laser_min != 360){
                ROS_INFO("%d\n Min Element = ", laser_min);
                ROS_INFO("%f\n laser_readings[laser_min] = ", laser_readings[laser_min]);
                ros::Duration(0.1).sleep();
            }

            vel.angular.z = 0;
            pub.publish(vel);

            ROS_INFO("find_Callback completed");
            res.wallfound = true;
            return true;
        }

    private:
        ros::Subscriber sub;
        ros::Publisher pub;
        ros::NodeHandle nh;
        ros::ServiceServer my_service;

        std::string status_update;

        geometry_msgs::Twist vel;

        std::vector<float> laser_readings;
        int laser_min;
        
};

int main(int argc, char **argv)
{
  ros::init(argc, argv, "find_wall");
  controller ctrl;
  ros::spin();

  return 0;
}
2

Hi @IMSETraining ,

Your laserCallback(...) is actually fine. Your subscriber declaration is also defined fine (but has indentation mistake - easy fix).

The reason why you get only one laser message is that your service callback has a while loop and you are not spinning inside the while loop.

Solution:
Inside the while loop of the find_Callback function, add ros::spinOnce(); line just before the while loop ends.

while(laser_min != 360){
  ROS_INFO("%d\n Min Element = ", laser_min);
  ROS_INFO("%f\n laser_readings[laser_min] = ", laser_readings[laser_min]);
  ros::Duration(0.1).sleep();
  ros::spinOnce(); // <--- add this line here
}

This should fix your problem. Let me know if this does not work.

Regards,
Girish

1 Like
3

Hi @girishkumar.kannan! Thank you for your response.

Adding the ros::spinOnce(); line in each of my while loops worked great. I would recommend anyone having similar issues check out the ROS Wiki on Callbacks and Spinning - it helped me better understand why the issue was occurring.

Prior to your response, I found a solution by adding two lines to my main function enabling multithreading. I believe this set the subscriber callback on one thread and the service callback on another which allowed them to work simultaneously.

// alternate solution
int main(int argc, char **argv)
{
  ros::init(argc, argv, "find_wall");
  controller ctrl;

  ros::MultiThreadedSpinner spinner(2); // <--- added lines
  spinner.spin(); // <--- added lines
  ros::spin(); //<--- removed lines

  return 0;
}
4

Hi @IMSETraining ,

Glad to know that you have fixed your problem.

Also good to see that you did your research on other alternatives. I did not mention Spinner class to you because you do not need to have the laser scanner callback running always. Especially in your case, you would not require to have the laser scanner running from the beginning of the program, that is, you must have it initialized but not running continuously. You just need it when the service callback is called.

Also, there are two types of spinners: MultiThreadedSpinner and AsyncSpinner. The first type works in a blocking way but the second one does not. You need to choose carefully and wisely.

Regards,
Girish

1 Like