Project - how to get the laser scan data in the service?

I’m working on the second part of the project for the ROS Basics in 5 Days C++ course. It asks to create a service server to find a wall. To do this, you need to get the laser scan data. This would use a subscriber. I’m not sure how to use the subscriber to get the scan data within the service. It seems like there are two callbacks, one for the subscriber and one for the service, and I need to use the subscriber multiple times within the service.

I’ve tried the code below. The subscriber constantly prints the correct scan values, Ft_dist and Rt_dist. However, when I call the service (manually from the terminal) the values for Ft_dist and Rt_dist are zero.


#include “ros/ros.h”

#include <geometry_msgs/Twist.h>

#include <sensor_msgs/LaserScan.h>

#include <server_turtle_wall/FindWall.h>

ros::Publisher pub;

geometry_msgs::Twist vel;

float Fr_dist;

float Lt_dist;

float Rt_dist;

void laserCallback(const sensor_msgs::LaserScan::ConstPtr &msg) {

ROS_INFO(“Running”);

float Fr_dist = msg->ranges[180];

float Lt_dist = msg->ranges[270];

float Rt_dist = msg->ranges[90];

ROS_INFO(“Distance to Front: %f”, Fr_dist);

ROS_INFO(“Distance to Right: %f”, Rt_dist);

}

// We define the callback function of the service

bool my_callback(server_turtle_wall::FindWall::Request &req,

             server_turtle_wall::FindWall::Response &res) {

// res.some_variable = req.some_variable + req.other_variable;

ROS_INFO(“Distance to Front: %f”, Fr_dist);

ROS_INFO(“Distance to Right: %f”, Rt_dist);

ROS_INFO(“Finding Wall…”);

res.wallfound = true;

return true;

}

int main(int argc, char **argv) {

ros::init(argc, argv, “find_wall_server”);

ros::NodeHandle nh;

pub = nh.advertise<geometry_msgs::Twist>(“cmd_vel”, 1000);

ros::Subscriber sub = nh.subscribe("/scan", 1000, laserCallback);

ros::ServiceServer my_service =

  nh.advertiseService("/find_wall", my_callback);

ros::spin();

return 0;

}


Have you tried subscribing to /scan outside of the service callback? That way you always have the values.

Thanks for the help. That was the first thing that I tried. Below is the code. The output when I call the service is that the front and right distance are both 0. I just don’t know the proper syntax to access subscriber message data outside the subscriber callback.

#include “ros/ros.h”
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/LaserScan.h>
#include <server_turtle_wall/FindWall.h>
ros::Publisher pub;
geometry_msgs::Twist vel;
float Fr_dist;
float Lt_dist;
float Rt_dist;
void laserCallback(const sensor_msgs::LaserScan::ConstPtr &msg) {
ROS_INFO(“Running”);
float Fr_dist = msg->ranges[180];
float Lt_dist = msg->ranges[270];
float Rt_dist = msg->ranges[90];
ROS_INFO(“Distance to Front: %f”, Fr_dist);
ROS_INFO(“Distance to Right: %f”, Rt_dist);
}
// We define the callback function of the service
bool my_callback(server_turtle_wall::FindWall::Request &req,
server_turtle_wall::FindWall::Response &res) {
// res.some_variable = req.some_variable + req.other_variable;
ROS_INFO(“Finding Wall…”);
ROS_INFO(“Distance to Front: %f”, Fr_dist);
ROS_INFO(“Distance to Right: %f”, Rt_dist);
res.wallfound = true;
return true;
}
int main(int argc, char **argv) {
ros::init(argc, argv, “find_wall_server”);
ros::NodeHandle nh;
pub = nh.advertise<geometry_msgs::Twist>(“cmd_vel”, 1000);
ros::ServiceServer my_service =
nh.advertiseService("/find_wall", my_callback);
ros::Subscriber sub = nh.subscribe("/scan", 1000, laserCallback);
ros::spin();
return 0;
}

See if this thread helps

You might have to find a similar multi-thread code for python.
I am surprised more people didn’t encounter the same issue.

Thanks for the help, but I don’t think all of that should be necessary. The ROSin5Days course doesn’t even cover classes until after this part of the project. It seems like it should be doable without classes. I’d like to know what the intended method is.

I’m not using ROS2. Any idea how it would be different in ROS Noetic? It seems like this should be doable with only the info taught in the class so far.

Yes, it’s doable without classes, you can use global variables. Here is what you need to do (I’m leaving you to figure the implementation out):

  1. Create a global variable, say msg, that will store the laser scan message.
  2. In the subscriber callback, update the global variable.
  3. Based on 1 and 2 above, the global variable msg will always contain the latest laser scan data, and you can access it within your service callback or anywhere else.

You can get some hints on using global variables in this post: Solution to Exercise 6.2 solution missing global

Thanks for the help. That is what I’m trying to do. The problem is that once I call the service, the subscriber callback never runs again until the service is complete, so I can’t get updated sensor data in the service callback. The question is how do I get the subscriber callback to run and update the sensor data while running the service. I think that is required based on the task given because we need to turn the robot until it is pointing at the wall and then drive a certain distance to the wall. That implies that I need to be able to access updating sensor data while running the service. Here’s the code:

#include “ros/ros.h”
#include <geometry_msgs/Twist.h>
#include
#include <sensor_msgs/LaserScan.h>
#include <server_turtle_wall/FindWall.h>
using namespace std;

ros::Publisher pub;
geometry_msgs::Twist vel;
vector rays;
int count = 0;
int min_ct = 10000;
float min_ray = 10000;
bool is_min = 0;

void laserCallback(const sensor_msgs::LaserScan::ConstPtr &msg) {

rays = msg->ranges;
}

// We define the callback function of the service
bool my_callback(server_turtle_wall::FindWall::Request &req, server_turtle_wall::FindWall::Response &res) {

ROS_INFO(“Finding Wall…”);
ros::Rate rate(1);

while ((min_ct > 185) || (min_ct < 175)) {
min_ray = *min_element(rays.begin(), rays.end());
min_ct = min_element(rays.begin(), rays.end()) - rays.begin();

if (min_ct > 185) {
  vel.angular.z = 0.1;

} else if (min_ct < 185) {
  vel.angular.z = -0.1;

} else {
  vel.angular.z = 0;
}

pub.publish(vel);
rate.sleep();

ROS_INFO("Running:");
ROS_INFO("The minimum ray occurs at %i and is a distance of %f.", min_ct, min_ray);

}

min_ray = *min_element(rays.begin(), rays.end());
min_ct = min_element(rays.begin(), rays.end()) - rays.begin();

ROS_INFO(“The minimum ray occurs at %i and is a distance of %f.”, min_ct, min_ray);
res.wallfound = true;
return true;
}

int main(int argc, char **argv) {
ros::init(argc, argv, “find_wall_server”);
ros::NodeHandle nh;

pub = nh.advertise<geometry_msgs::Twist>(“cmd_vel”, 1000);
ros::ServiceServer my_service = nh.advertiseService("/find_wall", my_callback);
ros::Subscriber sub = nh.subscribe("/scan", 1000, laserCallback);

ros::spin();
return 0;
}

Hi, I think the easiest thing here is to implement classes. I realize now that the instruction to start PART II of the rosject is at the end of the “Understanding ROS Services - Server”, and that is my error, so I apologize for that. The next unit teaches how to implement classes, which will make that exercise easier to solve, so I will change the instruction to that unit. Thank you for letting us know of this and let me know if there are more problems after you try to use classes.

1 Like

Thanks for the reply. I’m not sure how using classes fixes my issue. I implemented a class that contained the service, subscriber (laser scan), and publisher (velocity command) and it didn’t seem to help. I’m still not able to access the updated laser scan data while the service is running. When the service is called, the laser scan message has some value, but while the service is running, the subscriber callback is never called, and the value doesn’t update.

Using harishbalasub’s hint, I implemented 2 threads, and that seemed to work. However, that doesn’t require the use of classes and it isn’t covered in the course, as far as I’m aware. I feel like I’m missing something basic about how to use subscriber data within a service callback. A separate question on multi-threading.

Below is the code:

-------------- IMPLEMENTATION WITH CLASSES THAT ISN’T WORKING --------------

#include “ros/ros.h”
#include <geometry_msgs/Twist.h>
#include
#include <sensor_msgs/LaserScan.h>
#include <server_turtle_wall/FindWall.h>
using namespace std;

class TurtleFindWall {
public:

ros::NodeHandle nh_;
// Find wall service
ros::ServiceServer my_service;
// Velocity publisher
ros::Publisher vel_pub;
// Velocity message
geometry_msgs::Twist vel_msg;
// Laser scan subscriber
ros::Subscriber lsr_sub;

int min_ct_;
float min_ray_;
vector rays_;

TurtleFindWall() {
my_service = nh_.advertiseService("/find_wall", &TurtleFindWall::my_callback, this);
ROS_INFO(“The Service /find_wall is READY”);
vel_pub = nh_.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
lsr_sub = nh_.subscribe("/scan", 1000, &TurtleFindWall::laserCallback, this);
min_ct_ = 10000;
min_ray_ = 10000;
}

// Find wall service
bool my_callback(server_turtle_wall::FindWall::Request &req, server_turtle_wall::FindWall::Response &res) {
ROS_INFO(“Finding Wall…”);
ROS_INFO(“The minimum ray occurs at %i and is a distance of %f.”, min_ct_, min_ray_);
ros::Rate rate(1);
while ((min_ct_ > 185) || (min_ct_ < 175)) {
min_ray_ = *min_element(rays_.begin(), rays_.end());
min_ct_ = min_element(rays_.begin(), rays_.end()) - rays_.begin();
if (min_ct_ > 185) {
vel_msg.angular.z = 0.1;
} else if (min_ct_ < 185) {
vel_msg.angular.z = -0.1;
} else {
vel_msg.angular.z = 0;
}
vel_pub.publish(vel_msg);
rate.sleep();
ROS_INFO(“Angle: %i Distance: %f.”, min_ct_, min_ray_);
}
vel_msg.angular.z = 0;
vel_pub.publish(vel_msg);
ROS_INFO(“The minimum ray occurs at %i and is a distance of %f.”, min_ct_, min_ray_);
res.wallfound = true;
return true;
}

// Laser scan subscriber
void laserCallback(const sensor_msgs::LaserScan::ConstPtr &msg) {
rays_ = msg->ranges;
}

};

int main(int argc, char **argv) {
ros::init(argc, argv, “find_wall_server”);
TurtleFindWall turtlefindwall;
ros::spin();
return 0;
}

-------------- IMPLEMENTATION WITH MULTI-THREADING THAT IS WORKING --------------

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include
#include <sensor_msgs/LaserScan.h>
#include <server_turtle_wall/FindWall.h>
using namespace std;

ros::Publisher pub;
geometry_msgs::Twist vel;
vector rays;

int count = 0;
int min_ct = 10000;
float min_ray = 10000;
bool is_min = 0;

// Laser scan subscriber
void laserCallback(const sensor_msgs::LaserScan::ConstPtr &msg) {
rays = msg->ranges;
}

// Find wall service
bool my_callback(server_turtle_wall::FindWall::Request &req, server_turtle_wall::FindWall::Response &res) {

ROS_INFO(“Finding Wall…”);
ros::Rate rate(1);

while ((min_ct > 185) || (min_ct < 175)) {
min_ray = *min_element(rays.begin(), rays.end());
min_ct = min_element(rays.begin(), rays.end()) - rays.begin();
if (min_ct > 185) {
vel.angular.z = 0.1;
} else if (min_ct < 185) {
vel.angular.z = -0.1;
} else {
vel.angular.z = 0;
}
pub.publish(vel);
rate.sleep();
ROS_INFO(“Running:”);
ROS_INFO(“The minimum ray occurs at %i and is a distance of %f.”, min_ct, min_ray);
}

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

ROS_INFO(“The minimum ray occurs at %i and is a distance of %f.”, min_ct, min_ray);
res.wallfound = true;
return true;
}

int main(int argc, char **argv) {

ros::init(argc, argv, “find_wall_server”);
ros::NodeHandle nh;

pub = nh.advertise<geometry_msgs::Twist>(“cmd_vel”, 1000);
ros::ServiceServer my_service = nh.advertiseService("/find_wall", my_callback);
ros::Subscriber sub = nh.subscribe("/scan", 1000, laserCallback);

ros::MultiThreadedSpinner spinner(2);
spinner.spin();

return 0;

}

Please can you update this in the course. I have been stuck for hours trying to return from inner function to outer. Using classes completely circumvents this problem.
Thank you.

Hi @sjeppesen, we will work on adding a section that explains how you can implement multithreaded executors in order to deal with a subscriber within a service server. You are right, using classes does not solve the issue.

In the meantime, please look into this example on how to do just that:

For a python version, you can take a look at this post:

Can anyone please share the python version of the same for ROS1 ? I have been stuck on this section since many weeks.

Hi @arjun2312 , where is the problem you are facing in this section? I can try to help you if I have more information, like whether the server can be launched. That way we can identify where the issue is. Screenshots are always helpful