I am trying to do the services section of the Rosject used with ROS2 Basics in 5 days course. It seems that the variable I assigned to read and store the measurements from the /scan topic I subscribed to is not being updated during the service callback function since the code is stuck in the while loop and the self.closest_meas_ind variable is not being updated. Am I doing something wrong ?
You can find my code below:
import rclpy
from rclpy.node import Node
from rclpy.qos import ReliabilityPolicy, QoSProfile
# from std_srvs.srv import Empty
from custom_interface.srv import FindWall
from std_msgs.msg import Float32
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
class Service(Node):
def __init__(self):
super().__init__('find_wall_service')
self.srv = self.create_service(
FindWall, 'find_wall', self.service_callback)
self.publisher_ = self.create_publisher(Twist, '/cmd_vel', 10)
self.subscriber_ = self.create_subscription(
LaserScan,
'/scan',
self.read_measurement,
QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT)) # is the most used to read LaserScan data and some sensor data.
self.timer = 0.5
self.cmd = Twist()
self.curr_meas = LaserScan()
self.closest_meas_ind = int()
self.closest_meas_val = Float32()
# subscriber callback function
def read_measurement(self, msg):
self.curr_meas = msg.ranges
# service callback function
def service_callback(self, request, response):
self.get_logger().info(
f'The length of the ranges array is: {len(self.curr_meas)}')
self.get_logger().info('Finding the smallest measurment value and index')
# Finding the smallest measurement index
self.closest_meas_ind = min(
range(len(self.curr_meas)), key=self.curr_meas.__getitem__)
# Finding the smallest measurement value
self.closest_meas_val = self.curr_meas[self.closest_meas_ind]
self.get_logger().info(
f'The closes measurement is: {self.closest_meas_val}m and the index is: {self.closest_meas_ind}')
# rotating the robot until the front laser ray is the smallest
if self.closest_meas_ind < 359: # if the index is located in the right hand side
self.cmd.linear.x = 0.0 # making sure that the robot is not moving forward
self.cmd.angular.z = -0.3 # rotate clockwise to point to the closest wall
self.get_logger().info(
'closest wall is on the right - rotating clockwise to reach the closest wall')
self.publisher_.publish(self.cmd)
else:
self.cmd.linear.x = 0.0 # making sure that the robot is not moving forward
self.cmd.angular.z = 0.3 # rotate counterclockwise to point to the closest wall
self.get_logger().info(
'closest wall is on the left - rotating counterclockwise to reach the closest wall')
self.publisher_.publish(self.cmd)
while self.closest_meas_ind != 359:
self.get_logger().info(
f'The current measurement front measurement is: {self.curr_meas[0]}m')
# updating the closest measurement index
self.closest_meas_ind = min(
range(len(self.curr_meas)), key=self.curr_meas.__getitem__) self.get_logger().info('Robot currently facing closest wall - stopping robot...')
self.cmd.linear.x = 0.0
self.cmd.angular.z = 0.0
self.publisher_.publish(self.cmd)
response.wallfound = True
return response
def main(args=None): # check the main function of a service node
rclpy.init(args=args)
srv = Service()
rclpy.spin(srv)
rclpy.shutdown()
if __name__ == '__main':
main()
Firstly, I have taken the liberty to change the “Course Support Category” from ROS Basics In 5 Days (Python) to ROS2 Basics in 5 Days (Python).
I see that you have declared self.curr_meas as LaserScan() in your __init__() - but in the scan subscriber callback you are storing only the ranges and not the entire message as it is.
So, technically, it would be better if you change the line from self.curr_meas = LaserScan() to self.curr_meas = [].
In your service callback, the length of self.curr_meas will be constant, which in your case seems to be 360 values.
Going through your service callback logic, I see that you are getting the smallest range and the index value and then trying to spin the robot to face that direction. But the problem I see is that, you are spinning the robot quite fast and chances are, you will overshoot your final angle every time - your robot will continue to keep spinning in some direction for a long time.
Since you have mentioned that:
I would first like to know if your code is actually entering the service callback?
Is the robot moving in the simulation?
Could you post your terminal outputs?
Also, since your self.curr_meas will always be a list, I do not understand why you would require key=self.curr_meas.__getitem__ in your min() function.
Your self.closest_meas_val is a variable that you seem to be using within your code, so you do not have to define it as Float32().
The following will be perfectly fine (in Python):
Sorry for the mistake in the course support category. Thank you so much for all the coding tips!
I corrected them. And I should point out that there are 720 laser measurements.
And, honestly I just searched on the web for how to find the index of the minimum value in an array and I found that I can use index=min(range(len(a)), key=a.__getitem__)
I updated this part of the code too and now I am searching for the index by using self.closest_meas_ind = self.curr_meas.index(min(self.curr_meas)).
Furthermore, I updated the subscriber callback function and added a get_loggger().info() message to see if the measurements are actually being read and updated. And, and they are being updated as soon as I launch the service server and they are updated indefinetly. However, when I send a service call request message to the service server node, I stop receiving the get_logger().info() messages from the subscriber callback function and the code enters the service callback function, the robot starts rotating and the code gets stuck in the while loop. And, as you can see from the terminal output below, the self.closest_meas_ind variable is not being updated.
[wall_finding-1] [INFO] [1671571793.436780437] [find_wall_service]: The current closest measurement index is: 495
[wall_finding-1] [INFO] [1671571793.438017355] [find_wall_service]: The current closest measurement index is: 495
[wall_finding-1] [INFO] [1671571793.439493040] [find_wall_service]: The current closest measurement index is: 495
[wall_finding-1] [INFO] [1671571793.440988617] [find_wall_service]: The current closest measurement index is: 495
[wall_finding-1] [INFO] [1671571793.442249404] [find_wall_service]: The current closest measurement index is: 495
[wall_finding-1] [INFO] [1671571793.443843381] [find_wall_service]: The current closest measurement index is: 495
[wall_finding-1] [INFO] [1671571793.445250178] [find_wall_service]: The current closest measurement index is: 495
[wall_finding-1] [INFO] [1671571793.446667131] [find_wall_service]: The current closest measurement index is: 495
[wall_finding-1] [INFO] [1671571793.447996301] [find_wall_service]: The current closest measurement index is: 495
[wall_finding-1] [INFO] [1671571793.449303546] [find_wall_service]: The current closest measurement index is: 495
[wall_finding-1] [INFO] [1671571793.450619280] [find_wall_service]: The current closest measurement index is: 495
[wall_finding-1] [INFO] [1671571793.451902095] [find_wall_service]: The current closest measurement index is: 495
[wall_finding-1] [INFO] [1671571793.453262068] [find_wall_service]: The current closest measurement index is: 495
[wall_finding-1] [INFO] [1671571793.454583462] [find_wall_service]: The current closest measurement index is: 495
[wall_finding-1] [INFO] [1671571793.455953004] [find_wall_service]: The current closest measurement index is: 495
[wall_finding-1] [INFO] [1671571793.457271674] [find_wall_service]: The current closest measurement index is: 495
[wall_finding-1] [INFO] [1671571793.459079113] [find_wall_service]: The current closest measurement index is: 495
[wall_finding-1] [INFO] [1671571793.460281619] [find_wall_service]: The current closest measurement index is: 495
[wall_finding-1] [INFO] [1671571793.461737281] [find_wall_service]: The current closest measurement index is: 495
[wall_finding-1] [INFO] [1671571793.522076636] [find_wall_service]: The current closest measurement index is: 495
[wall_finding-1] [INFO] [1671571793.523250421] [find_wall_service]: The current closest measurement index is: 495
[wall_finding-1] [INFO] [1671571793.524363074] [find_wall_service]: The current closest measurement index is: 495
[wall_finding-1] [INFO] [1671571793.532656389] [find_wall_service]: The current closest measurement index is: 495
[wall_finding-1] [INFO] [1671571793.533703051] [find_wall_service]: The current closest measurement index is: 495
[wall_finding-1] [INFO] [1671571793.534744968] [find_wall_service]: The current closest measurement index is: 495
[wall_finding-1] [INFO] [1671571793.536027124] [find_wall_service]: The current closest measurement index is: 495
[wall_finding-1] [INFO] [1671571793.544150772] [find_wall_service]: The current closest measurement index is: 495
[wall_finding-1] [INFO] [1671571793.547046410] [find_wall_service]: The current closest measurement index is: 495
[wall_finding-1] [INFO] [1671571793.549163463] [find_wall_service]: The current closest measurement index is: 495
[wall_finding-1] [INFO] [1671571793.555619207] [find_wall_service]: The current closest measurement index is: 495
[wall_finding-1] [INFO] [1671571793.558297672] [find_wall_service]: The current closest measurement index is: 495
^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
You can also find the updated version of the code below:
import rclpy
from rclpy.node import Node
from rclpy.qos import ReliabilityPolicy, QoSProfile
# from std_srvs.srv import Empty
from custom_interface.srv import FindWall
from std_msgs.msg import Float32
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
class Service(Node):
def __init__(self):
super().__init__('find_wall_service')
self.srv = self.create_service(
FindWall, 'find_wall', self.service_callback)
self.publisher_ = self.create_publisher(Twist, '/cmd_vel', 10)
self.subscriber_ = self.create_subscription(
LaserScan,
'/scan',
self.read_measurement,
QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT)) # is the most used to read LaserScan data and some sensor data.
self.timer = 0.5
self.cmd = Twist()
self.curr_meas = []
self.closest_meas_ind = 0
self.closest_meas_val = 0.0
# subscriber callback function
def read_measurement(self, msg):
self.curr_meas = msg.ranges
self.get_logger().info(
f'I just read the laser measurment and font_dist={self.curr_meas[359]}m')
# service callback function
def service_callback(self, request, response):
self.get_logger().info(
f'The length of the ranges array is: {len(self.curr_meas)}')
self.get_logger().info('Finding the smallest measurment value and index')
# Finding the smallest measurement index
self.closest_meas_ind = self.curr_meas.index(min(self.curr_meas))
# Finding the smallest measurement value
self.closest_meas_val = self.curr_meas[self.closest_meas_ind]
self.get_logger().info(
f'The closes measurement is: {self.closest_meas_val}m and the index is: {self.closest_meas_ind}')
# rotating the robot until the front laser ray is the smallest
if self.closest_meas_ind < 359: # if the index is located in the right hand side
self.cmd.linear.x = 0.0 # making sure that the robot is not moving forward
self.cmd.angular.z = -0.05 # rotate clockwise to point to the closest wall
self.get_logger().info(
'closest wall is on the right - rotating clockwise to reach the closest wall')
self.publisher_.publish(self.cmd)
else:
self.cmd.linear.x = 0.0 # making sure that the robot is not moving forward
self.cmd.angular.z = 0.05 # rotate counterclockwise to point to the closest wall
self.get_logger().info(
'closest wall is on the left - rotating counterclockwise to reach the closest wall')
self.publisher_.publish(self.cmd)
while self.closest_meas_ind != 359:
# updating the closest measurement index
self.get_logger().info(
f'The current closest measurement index is: {self.closest_meas_ind}')
self.closest_meas_ind = self.curr_meas.index(min(self.curr_meas))
self.get_logger().info('Robot currently facing closest wall - stopping robot...')
self.cmd.linear.x = 0.0
self.cmd.angular.z = 0.0
self.publisher_.publish(self.cmd)
response.wallfound = True
return response
def main(args=None): # check the main function of a service node
rclpy.init(args=args)
srv = Service()
rclpy.spin(srv)
rclpy.shutdown()
if __name__ == '__main':
main()
I also updated the rotation speeds and uploaded a gif showing the rotating robot.
I can now understand why your self.closest_meas_ind is not updating.
You need to use MultiThreadedExecutor in your program and define your subscriber and the service callback as ReentrantCallbackGroup.
Please lookup this link to know more about that concept: Executors and Callback Groups in ROS2 | ROS2 Developers Open Class #149 - YouTube
The explanation starts after the 15:00 minute mark. You can skip the first 15 minutes.
Basically, your current main() function will be changed to a new set of code where you define a MultiThreadedExecutor and add the class Service() as a Node.
The reason why your variable is not updating is that the subscriber callback is working on the main program and blocks the code on the service callback - thus making the while loop inside your service callback work inefficiently.
By defining the callbacks as ReentrantCallbackGroup, you are making the callbacks to work in parallel which will not get blocked by the main program.
Watch the video first. Your program will definitely work after you implement this concept.
Let me know if you still need help [after implementing the concept].
Thank you so much for your help @girishkumar.kannan it finally works! Although I had some problems of overshooting the angle I am searching for as you mentioned earlier. So, my work around was to create acceptable ranges for the angles I’m looking for with a margin for error of ± 2 degrees. And the code now works well I think. But I would like to ask you if this is a good idea in your experience ?
You can also find my updated code below:
import rclpy
import time
from rclpy.node import Node
from rclpy.callback_groups import ReentrantCallbackGroup, MutuallyExclusiveCallbackGroup
from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor
from rclpy.qos import ReliabilityPolicy, QoSProfile
# from std_srvs.srv import Empty
from custom_interface.srv import FindWall
from std_msgs.msg import Float32
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
class Service(Node):
def __init__(self):
super().__init__('find_wall_service')
self.group = ReentrantCallbackGroup()
self.srv = self.create_service(
FindWall, 'find_wall', self.service_callback, callback_group=self.group)
self.publisher_ = self.create_publisher(Twist, '/cmd_vel', 10)
# The QoS Profile below is the most used to read LaserScan data and some sensor data
self.subscriber_ = self.create_subscription(
LaserScan,
'/scan',
self.read_measurement,
QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT),
callback_group=self.group)
self.timer = 0.5
self.cmd = Twist()
self.curr_meas = []
self.closest_meas_ind = 0
self.closest_meas_val = 0.0
self.front_laser_meas = 0.0
# front laser acceptable range with ± 2 degree of error
self.acceptable_front_range = range(355, 363)
# right laser acceptable range with ± 2 degrees of error
self.acceptable_right_range = range(175, 183)
# subscriber callback function
def read_measurement(self, msg):
self.curr_meas = msg.ranges
self.front_laser_meas = msg.ranges[359]
self.get_logger().info('read laser measurement')
def stop_robot(self):
self.get_logger().info('STOPPING ROBOT...')
self.cmd.linear.x = 0.0
self.cmd.angular.z = 0.0
self.publisher_.publish(self.cmd)
def rotate_left(self):
self.get_logger().info(
'closest wall is on the left - rotating counterclockwise to reach the closest wall')
self.cmd.linear.x = 0.0 # making sure that the robot is not moving forward
self.cmd.angular.z = 0.1 # rotate counterclockwise to point to the closest wall
self.publisher_.publish(self.cmd)
def rotate_right(self):
self.get_logger().info(
'closest wall is on the right - rotating clockwise to reach the closest wall')
self.cmd.linear.x = 0.0 # making sure that the robot is not moving forward
self.cmd.angular.z = -0.1 # rotate counterclockwise to point to the closest wall
self.publisher_.publish(self.cmd)
def move_forward(self):
self.get_logger().info('MOVING ROBOT FORWARD...')
self.cmd.linear.x = 0.05
self.cmd.angular.z = 0.0
self.publisher_.publish(self.cmd)
def update_closest_measurement_val_and_ind(self):
# updating closest measurement value and index
self.closest_meas_ind = self.curr_meas.index(min(self.curr_meas))
self.closest_meas_val = self.curr_meas[self.closest_meas_ind]
def service_callback(self, request, response):
# Finding closest wall by using the closest laser measurement
self.update_closest_measurement_val_and_ind()
self.get_logger().info(
f'The closest measurement is: {self.closest_meas_val}m and the index is: {self.closest_meas_ind}')
# rotating the robot until the front laser ray is the smallest
if self.closest_meas_ind < 359: # if the closest index is located on the right hand side
self.rotate_right()
else: # the closest index is located on the left hand side
self.rotate_left()
self.get_logger().warn(
f'Value of closest index before while loop: {self.closest_meas_ind}')
while self.closest_meas_ind not in self.acceptable_front_range:
self.get_logger().info(
'Front measurement not in acceptable range')
# updating the closest measurement index and value
self.update_closest_measurement_val_and_ind()
self.get_logger().info(
f'Update complete - minimum ind ={self.closest_meas_ind} and closest_meas_val = {self.closest_meas_val}m')
self.get_logger().info('Robot currently facing closest wall - stopping robot...')
self.stop_robot()
# Moving the robot forward until the front laser ray has a distance smaller than 30 cm
self.move_forward()
while True:
self.get_logger().info(
f'Current front laser measurment: {self.front_laser_meas}m')
if self.front_laser_meas < 0.3:
self.get_logger().info('Distance to closest wall < 30cm')
self.stop_robot()
break
# Rotating robot until the closest wall is on the right
self.get_logger().info('Robot can now rotate to make the wall be on the right')
self.rotate_left()
while True:
# updating the closest measurement index and value
self.update_closest_measurement_val_and_ind()
self.get_logger().info(
f'Closest measurement index = {self.closest_meas_ind}')
if self.closest_meas_ind in self.acceptable_right_range:
self.get_logger().info('The closest wall is currently on the right - Stopping robot...')
self.stop_robot()
break
# At this point, the robot is ready to start following the wall
response.wallfound = True
return response
def main(args=None): # check the main function of a service node
rclpy.init(args=args)
try:
srv = Service()
executor = MultiThreadedExecutor(num_threads=6)
executor.add_node(srv)
try:
executor.spin()
finally:
executor.shutdown()
srv.destroy_node()
finally:
rclpy.shutdown()
if __name__ == '__main':
main()
Error of +/- 2 degrees is totally fine. Do not worry about that.
The reason is, the robot is trying to find the shortest distance to wall.
As we all know, from mathematics, the significance of angle becomes more important if the distance increases.
So, if the distance between the wall and robot is few meters (very far from robot) then +/- 2 degrees will be a significant factor to worry about. Since you are working on finding the shortest distance from robot to a wall, the distance is lesser than 1 meter, so +/- 2 degrees will not cause any problems to you.
I hope that I explained this clearly. Let me know if you still need help!
Regards,
Girish
P.S: Instead of putting in too much effort into minimizing +/- 2 degree error, try to add your own algorithm to find a wall faster - an example would be to use statistical methods to find if the robot is actually facing a wall or an obstacle by analyzing laser scan samples for the frontal range of say +/- 45 degrees from the front laser scan ray (I think this is 360 if you have 720 laser scan readings).
