Hi!
In part 2 of Rosject for ROS Basics in 5 Days (Python) course we are asked to create a service server so that our robot can approach a wall. I tried to do that, here is my code:
#! /usr/bin/env python
import rospy
from final_project.srv import FindWall, FindWallResponse
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
def my_callback(request):
print("Initiating service call")
print(lidar_data)
#Identify shortest laser ray
ranges = lidar_data.ranges
smallest_reading = min(ranges)
middle_reading = ranges[360]
#Rotate the robot until the front of it is facing the wall
while(middle_reading != smallest_reading):
vel.angular.z = 0.2
pub.publish(vel)
vel.angular.z = 0
vel.linear.x = 0.2
#Move the robot forward until the front ray is smaller than 30cm
while(middle_reading > 0.3):
pub.publish(vel)
#Rotate the robot again until ray number 270 of the laser ranges is pointing to the wall
right = ranges[180]
vel.angular.z = 0.2
vel.linear.x = 0
while(middle_reading != right):
pub.publish(vel)
print("Ending service call")
#Return the service message with True
response = FindWallResponse()
response.wallfound = True
return response
def get_lidar_data(msg):
lidar_data = msg
rospy.init_node('service_server')
#Start subscriber so I can read the LiDAR data
sub = rospy.Subscriber('/scan', LaserScan, get_lidar_data)
my_service = rospy.Service('/find_wall', FindWall, my_callback)
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
vel = Twist()
lidar_data = LaserScan()
print("Service server properly initiated")
rospy.spin()
But when I try to run my node, I get an error that lidar_data is empty.
This raises many questions:
- Since the service is synchronous and stops execution of the program, is it possible to update laser readings from the subscriber while the service is running? If so, how can that be done?
- I can’t seem to be reading any data from my subscriber before my service is called. Why is this happening?
- Are there any general recommendations on my code so I can make it cleaner and better?
Thanks in advance!
Hi @jmledran ,
Welcome to the Community!
I quickly glanced through your code and found out that you are missing the global keyword.
You get this error because the scope of lidar_data inside get_lidar_data(...) is only within that function.
So to update the global variable lidar_data you must reference the variable inside the callback function like so:
def get_lidar_data(msg):
global lidar_data # to modify global variable lidar_data
lidar_data = msg
Similarly, inside your my_callback(...) function, you must reference global keyword before you make use of the variable:
def my_callback(request):
print("Initiating service call")
global lidar_data
print(lidar_data)
# other statements below
The best way to make the program better is to use object orientation. This helps you avoid referencing global keyword in many places. Give appropriate names to functions, for example, you can change get_lidar_data to scan_callback and my_callback to service_callback.
This automatically happens when you have proper functions written for scan and service callbacks. When your program initialization is complete and when program comes to rospy.spin() line, your callbacks get renewed / called again.
It does happen. You are not able to see them because you have your print(lidar_data) inside your service callback and not inside get_lidar_data(). Add the print line in the scan subscriber callback and you will have scan messages flooding your terminal/screen.
The bast way to make use of sensor messages (scan messages) is to use appropriate variables in your subscriber callback that also updates your class variables (global variables if you are not using classes).
Try the above modifications and let me know if your program worked!
Regards,
Girish
Hi!
Thanks a lot for the fast reply.
I took into consideration all of the advices you gave me and changed to python classes.
It made everything simpler and more structured.
However, I face a new issue. My robot isn’t moving when I call my service. It’s just idle. It’s like it never enters my class.
To give more detail:
- I run my service server node which inside the service callback instantiates my class and then call a function using my object.
- The server is correctly loaded
- I call my service using rosservice call and when I do, my callback is called (I know because my print in my service callback is executed), but my program stops advancing. When it reaches the class instantiation and function call, it doesn’t continue execution.
I add my service code below:
#! /usr/bin/env python
import rospy
from final_project.srv import FindWall, FindWallResponse
from main_node import MainNode
def my_callback(request):
print("Initiating service call")
main_node_exe = MainNode()
main_node_exe.find_nearest_wall_and_follow()
print("Ending service call")
#Return the service message with True
response = FindWallResponse()
response.wallfound = True
return response
rospy.init_node('service_server')
my_service = rospy.Service('/find_wall', FindWall, my_callback)
print("Service server properly initiated")
rospy.spin()
Is there something I’m missing?
Why my main_node_exe.find_nearest_wall_and_follow() isn’t executed?
Is the problem in my server or in my python class?
I don’t get any error message in the terminal. So there’s no feedback. All I know is that when I call my service nothing happens.
Thanks in advance.
Hi @jmledran ,
Yes, your robot will not move! Here are the reasons why:
- You are instantiating
MainNode() inside a callback function. - Callback functions, as the name states, are functions that get called over and over again in your program. So, if you initialize MainNode inside a callback, it would just remain as is and will not do anything, in other words, it just keeps reinitializing. You should instantiate MainNode outside your my_callback function and reference the variable with global keyword inside the callback.
- When I mentioned you to use classes, I wanted you to make the entire code into one class with member functions and variables - you just took a part of code and made it into a
module and not a class. Modules are classes or programs present in an individual python file. from main_node import MainNode line tells you that MainNode is a class of main_node module.
At this point I have a few questions for you.
- Did you check your
MainNode program if it is working or not? I am assuming you have the /cmd_vel publisher and the /scan subscriber in that program.
- Why is your service server code not using classes?
My understanding is that you need to have all the program components in one program file under one class and as one node. Try making a class named ServiceServer and initialize it with velocity publisher, scan subscriber and callback, and service and its callback. Then add the required member functions.
Regards,
Girish
Hi Girish!
First of all, thanks a lot for your help.
I’ve finally found what was wrong, I used a rospy.spin() inside my class, and that was holding my whole execution. When I removed it, my program started to move.
I have one last question. What’s the best approach to make a robot do any action (rotate, for example), till a condition is met?
I’m trying to rotate my robot till it reaches a fixed value, but I think maybe giving it a range is better, because waiting for the robot to reach an exact value might take forever. Or is it a better approach?
Here is my code, so you can understand what I’m referring to.
def rotate_front_to_wall(self):
#Rotate the robot until the front of it is facing the wall
print("Rotating till facing the closest wall")
self.vel.angular.z = 0.2
self.vel.linear.x = 0.0
while(self.middle_reading != self.smallest_reading):
self.publish_once_vel()
self.rate.sleep()
print("Closest wall found")
Thanks a lot!
Hi @jmledran ,
There was no way I could have known that, because you did not post that code here. Glad you found it yourself and got it working!
Simple, use a while loop with the correct condition. I have finished this course and got the certificate - I used a while loop to get this working.
From my experience, I can tell you that your robot will not stop rotating. Here is the simple reason why.
The moment you publish a cmd_vel message with linear.x = 0.0 and angular.z = 0.2, the robot will keep doing that endlessly. So if you put that inside your while loop, until you stop the robot (by sending linear.x = 0 and angular.z = 0), your robot is going to keep spinning, even after it overshoots the point where it has to stop spinning.
The best way to get rid of this is to add stop_robot() method (just set all values of cmd_vel message to 0.0).
So you will turn your while loop like this:
while (frontal_scan_range != smallest_range):
# make the robot spin
self.publish_cmd_vel_message(linear_x=0.0, angular_z=0.2)
# wait for the robot to spin a little
self.rate.sleep()
# stop the robot
self.publish_cmd_vel_message(linear_x=0.0, angular_z=0.0)
# get the frontal range reading again
self.get_frontal_scan_range()
So this way, you are:
- making the robot spin for a short while
- stop the robot
- read the sensor values again to update the while loop condition
Another very important thing to note:
You MUST use value round off.
You will NEVER be able to match a value like 3.123456789123456.
But you CAN match a value like 3.123.
Too much presicion = Too much (unnecessary) computation -----> Waste of time and gives you no results.
(Remember that laser scan range is in meters, which means that at 3rd decimal place of precision you are at millimetres. Anything more than a millimetre precision is completely useless, meaning, 4 decimal places and above are just waste of time and computation [because you are working with robots and not atoms or molecules to require that precision]).
Let me know if you need more help. Hope I clarified your doubts.
Regards,
Girish
1 Like
Thanks a lot Girish!
I’ve finally mange to make my project work. I’ve completed part 2
1 Like