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

[winckrc]

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.

[winckrc]

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;

}

---

[roalgoal]

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

[winckrc]

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

[harishbalasub]

See if this thread helps

Subcriber cb not called when running Service server

Hi, Thanks for the reply. What i figured was that when the Service callback is executed on a loop, the Subscription callbacks are never called/executed. So I need to run the different callbacks in different threads. We don’t have such an example in the class notes. Is there any example which shows how I can run the 3 callbacks - 2 Subscription callbacks for Odometry and LaserScan, 1 Service callback - in concurrent threads? There is a simple example here, but doesnt seem to work in my ros2 …

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

[winckrc]

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.

[winckrc]

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.

[bayodesegun]

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

[winckrc]

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

[roalgoal]

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.

[winckrc]

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;

}

[sjeppesen]

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.

[roalgoal]

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:

ros2/examples/blob/master/rclcpp/executors/multithreaded_executor/multithreaded_executor.cpp

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

ROS2 Basic Python Service problem with subscriber

Hi, welcome to the community! Can you describe what is not working about your program? I see that the main() function looks correct, but I can’t see the multi threading needed for each callback function: from rclpy.callback_groups import MutuallyExclusiveCallbackGroup class FindwallService(Node): def __init__(self: # Multithreading self.group1 = MutuallyExclusiveCallbackGroup() self.group2 = MutuallyExclusiveCallbackGroup() # defines the type, name and callback…

[arjun2312]

@winckrc

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

}

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

[roalgoal]

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

[AlfMachine]

I’m having the same problem but i don’t know how to solve it.
Services are syncronous so if they set up the program will wait untill it ends.
I’ve tried to make it with a class and i still have the same problem, when the service is called the subscriber stops, no mater if it’s a class or in the same file, it stops. So do we have to work with multithreads? i’ve been reading about tf too but i don’t know what way do i have to take or what to do yet.
The following photos are a call to the service in which i create a object of the subscriber class. The second image is the result, it’s printing a message Moving and the number is the number of the closest laser facing a wall, the robot is rotating so it’s supose to be update but it’s not. My idea was to keep moving the robot with a while until the closest point was 360 but with no update i can’t do it in this way.

Note: i’m not using a service client i’m calling the service directly with rosservice call /find_wall
i’m thinking that i may need 2 nodes, one the service node and the other the scan laser reader

[rlekkerkerker]

Hi @AlfMachine,

Have you found the solution for this problem yet?
I’m encountering exactly the same problem. I’m following the ROS2 Basics in 5 days Humble (Python) course and just finished Unit 4 Understanding ROS2 Services

Ater finishing Unit 4 the course tells us to perform assignment 2.1 Create Service Server in the Rosject ROS2 Basics Python Real Robot Project and this is where I get stuck with the same problem you describe. @girishkumar.kannan explained me that multithreaded executors could be a solution, but I do not understand that these principles are not explained yet in the course so I get the feeling this is not the way to go for this assignment. Can somebody please point me in the right direction?

Thanks in advance!

[bayodesegun]

@rlekkerkerker

ROS2 Basics in 5 days Humble (Python)

@rlekkerkerker Your post does not belong here. Please post it in the category for ROS2 Basics in 5 days Humble (Python)