Hello community,
I did not find the category for the ROS2 Basics Python, so I just post the question here.
I am currently working on the rosject of ROS2 Basics Python, and I encounter some problems in Topic 2 Services.
The requirement is to build a service that finds the nearest wall. So first I use a subscriber to get laser scan values. Then when the service is called, I publish a message to rotate the robot to the wall side. However, it seems like the while loop that I use to check if the robot faces the wall blocks the subscriber thread. Therefore the laser value is not updated and the robot will keep rotating.
I know maybe I should use something like unblocking wait in the while loop. But I am not sure if it is the right way to do it.
Below is the code I have at the moment. And I am wondering do you provide the solution for this rosject?
Please help. Thanks!
# import the Empty module from std_servs service interface
from std_srvs.srv import Empty
# import the Twist module from geometry_msgs messages interface
from geometry_msgs.msg import Twist
# import the ROS2 python client libraries
import rclpy
from rclpy.node import Node
from custom_interfaces.srv import Wall
from sensor_msgs.msg import LaserScan
from rclpy.qos import ReliabilityPolicy, QoSProfile
from time import sleep
class Service(Node):
def __init__(self):
# Here we have the class constructor
# call the class constructor to initialize the node as service_moving
super().__init__('find_wall_service')
# create the service server object
# defines the type, name and callback function
self.srv = self.create_service(Wall, 'findWall', self.Empty_callback)
# create the publisher object
# in this case the publisher will publish on /cmd_vel topic with a queue size of 10 messages.
# use the Twist module
self.publisher_ = self.create_publisher(Twist, 'cmd_vel', 10)
self.subscriber = self.create_subscription(LaserScan, '/scan', self.scan_update, QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT))
# prevent unused variable warning
self.subscriber
self.laser_front = 0
self.laser_right = 0
self.nearest_idx = 0
def scan_update(self, msg):
self.laser_right = msg.ranges[90]
self.laser_front = msg.ranges[180]
self.nearest_idx = msg.ranges.index(min(msg.ranges))
# The logger stops printing the message after the service is called, so I assumed this part is not running
self.get_logger().info(f'smallest index = {self.nearest_idx}')
def Empty_callback(self, request, response):
msg = Twist()
if self.nearest_idx < 170:
msg.angular.z = -0.3
self.get_logger().info('Turn right to find wall')
elif self.nearest_idx > 190:
msg.angular.z = 0.3
self.get_logger().info('Turn left to find wall')
self.publisher_.publish(msg)
while True:
if self.nearest_idx > 170 and self.nearest_idx < 190:
msg.angular.z = 0.0
self.publisher_.publish(msg)
self.get_logger().info('Wall found')
break
# it seems like the sleep is blocking the thread
sleep(0.1)
return response
def main(args=None):
# initialize the ROS communication
rclpy.init(args=args)
# declare the node constructor
moving_service = Service()
# pause the program execution, waits for a request to kill the node (ctrl+c)
rclpy.spin(moving_service)
# shutdown the ROS communication
rclpy.shutdown()
if __name__ == '__main__':
main()
