I am going through part 2.2 of the “ROS2 Basics Python Real Robot Project” and I need to add a synchronous request from the node I created to control the robot movement so the robots get positioned correctly by the find_wall service before starting to follow the wall. I did it as shown in the code below and it appears to be working all right, but I am unsure if it is correct. Can someone please check and correct me if necessary?
from find_wall.srv import FindWall
import rclpy
# import the ROS2 python libraries
from rclpy.node import Node
# import the Twist module from geometry_msgs interface
from geometry_msgs.msg import Twist
# import the LaserScan module from sensor_msgs interface
from sensor_msgs.msg import LaserScan
from rclpy.qos import ReliabilityPolicy, QoSProfile
class Wall_following_node(Node):
def __init__(self):
# Here you have the class constructor
# call the class constructor
super().__init__('wall_following_node')
# create the publisher object
self.publisher_ = self.create_publisher(Twist, 'cmd_vel', 10)
# create the subscriber object
self.subscriber = self.create_subscription(LaserScan, '/scan', self.laser_callback, QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE))
# create the Service Client object
# defines the name and type of the Service Server you will work with.
self.client = self.create_client(FindWall, 'find_wall')
# checks once per second if a Service matching the type and name of the Client is available.
while not self.client.wait_for_service(timeout_sec=1.0):
# if it is not available, a message is displayed
self.get_logger().info('service not available, waiting again...')
# define the timer period for 0.5 seconds
self.timer_period = 0.25
# define the variables to save the received info
self.laser_forward = 0
self.laser_right = 0
self.lasers = []
self.last_error = 0
self.x = 0.0
self.y = 0.0
self.counter = 0
# create a Twist message
self.cmd = Twist()
self.timer = self.create_timer(self.timer_period, self.motion)
# create an FindWall request
self.req = FindWall.Request()
def send_request(self):
# send the request
self.future = self.client.call_async(self.req)
def laser_callback(self,msg):
#sim
self.laser_forward = msg.ranges[0]
self.laser_right = msg.ranges[540]
#real
#self.laser_forward = msg.ranges[360]
#self.laser_right = msg.ranges[180]
def motion(self):
#Bang-Bang Control
# if self.laser_forward > 0.5:
# self.cmd.linear.x = 0.1
# if self.laser_right > 0.3:
# self.cmd.angular.z = -0.15
# elif self.laser_right < 0.20:
# self.cmd.angular.z = 0.15
# else:
# self.cmd.angular.z = 0.0
# else:
# self.cmd.linear.x = 0.1
# self.cmd.angular.z = 1.0
#PD control
setpoint = 0.25
error = setpoint - self.laser_right
self.cmd.linear.x = 0.1
if self.laser_forward > 0.5:
self.cmd.angular.z = 3*error + 20*(error-self.last_error)
else:
self.cmd.angular.z = 1.0
self.last_error = error
# Publishing the cmd_vel values to a Topic
self.publisher_.publish(self.cmd)
self.counter = self.counter + 1
def main(args=None):
# initialize the ROS communication
rclpy.init(args=args)
# declare the node constructor
wall_following_node = Wall_following_node()
#Run Syncronous request
response = wall_following_node.send_request()
print(response)
# pause the program execution, waits for a request to kill the node (ctrl+c)
rclpy.spin(wall_following_node)
# Explicity destroy the node
wall_following_node.destroy_node()
# shutdown the ROS communication
rclpy.shutdown()
if __name__ == '__main__':
main()```
No, you are not making a synchronous request here. You are making an asynchronous request. Please review the notebook again for the ways to make sync and async requests.
Just in case you’re confusing them:
Synchronous requests → you wait for the service to complete before going to the next line of the program. Use self.client.call
Asynchronous requests → you send the request and don’t wait for an answer. You check periodically if the request is done. Use self.client.call_async
I did the code again, this time using self.client.call but the code only works as expected when I use two threads. See the code below:
from find_wall.srv import FindWall
import rclpy
# import the ROS2 python libraries
from rclpy.node import Node
# import the Twist module from geometry_msgs interface
from geometry_msgs.msg import Twist
# import the LaserScan module from sensor_msgs interface
from sensor_msgs.msg import LaserScan
from rclpy.qos import ReliabilityPolicy, QoSProfile
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.executors import MultiThreadedExecutor
class Wall_following_node(Node):
def __init__(self):
# Here you have the class constructor
# call the class constructor
super().__init__('wall_following_node')
self.group1 = ReentrantCallbackGroup()
self.wall_finder_client = self.create_client(FindWall, 'find_wall', callback_group=self.group1)
# checks once per second if a Service matching the type and name of the Client is available.
while not self.wall_finder_client.wait_for_service(timeout_sec=1.0):
# if it is not available, a message is displayed
self.get_logger().info('service not available, waiting again...')
# create the publisher object
self.publisher_ = self.create_publisher(Twist, 'cmd_vel', 10)
# create the subscriber object
self.subscriber = self.create_subscription(LaserScan, '/scan', self.laser_callback, QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE), callback_group=self.group1)
self.timer_period = 0.25
self.laser_forward = 0
self.laser_right = 0
self.lasers = []
self.last_error = 0
self.x = 0.0
self.y = 0.0
self.counter = 0
self.cmd = Twist()
self.timer = self.create_timer(self.timer_period, self.motion)
self.req = FindWall.Request()
self.state = "FIND_WALL"
self.find_wall_response = False
def send_request(self):
# send the request
self.find_wall_response = self.wall_finder_client.call(self.req)
def laser_callback(self,msg):
#sim
self.laser_forward = msg.ranges[0]
self.laser_right = msg.ranges[540]
#real
#self.laser_forward = msg.ranges[360]
#self.laser_right = msg.ranges[180]
def motion(self):
if self.state == "FIND_WALL":
self.send_request()
self.state = "FOLLOW_WALL"
elif self.state == "FOLLOW_WALL":
#Bang-Bang Control
# if self.laser_forward > 0.5:
# self.cmd.linear.x = 0.1
# if self.laser_right > 0.3:
# self.cmd.angular.z = -0.15
# elif self.laser_right < 0.20:
# self.cmd.angular.z = 0.15
# else:
# self.cmd.angular.z = 0.0
# else:
# self.cmd.linear.x = 0.1
# self.cmd.angular.z = 1.0
#PD control
setpoint = 0.25
error = setpoint - self.laser_right
self.cmd.linear.x = 0.1
if self.laser_forward > 0.5:
self.cmd.angular.z = 3*error + 20*(error-self.last_error)
else:
self.cmd.angular.z = 1.0
self.last_error = error
# Publishing the cmd_vel values to a Topic
self.publisher_.publish(self.cmd)
self.counter = self.counter + 1
def main(args=None):
rclpy.init(args=args)
wall_follower_node = Wall_following_node()
executor = MultiThreadedExecutor(num_threads=2)
# Add the node to the executor
executor.add_node(wall_follower_node)
try:
# Spin the executor
executor.spin()
finally:
# Shutdown the executor
executor.shutdown()
wall_follower_node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
The code above works as expected, but I don’t understand why I must use two threads. The way I understand synchronous requests, when the program gets to the line self.send_request() the thread stops and only continues when the request is done. But When I use only one thread the thread stops and never continues, even after the request is done. The program gets blocked at the end of the request and never executes line self.state = "FOLLOW_WALL". Why?
Hi, I’ve had this same issue. I resorted to using asynchronous calls. It seems really strange that threads “die” (for lack of a better word) after being used in a synchronous call. Some clarification on this would be really helpful
Threads are tricky, and the sync call should run in its own thread in order not to block the main thread.
You just described the way ROS 1 services work. You don’t even need to call them in a separate thread, but ROS 2 sync service calls seem to be much more complicated. Async calls are much easier to work with as @AshK mentioned.
You must have been asked to do this as a sync call in order to learn a bit about how that works, and it’s good you now know how to make it work. Good job!