I am working on the Rosject and am stuck on task 2.2, which asks us to call the WallFinder service from the wall_follower script using an asynchronous service client. I have used multi-threading and a re-entrant callback group, but my code is not working. Everything builds correctly and the package launches, but the script jusr prints “service not available” into the shell.
My code is below:
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 wall_interfaces_custom.srv import FindWall
from rclpy.qos import ReliabilityPolicy, QoSProfile
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.executors import MultiThreadedExecutor
import time
class Wall_Following_Node(Node):
def __init__(self):
# Here you have the class constructor
# call the class constructor
super().__init__('wall_following')
self.group1 = ReentrantCallbackGroup()
self.group2 = ReentrantCallbackGroup()
self.group3 = ReentrantCallbackGroup()
# create the Service Client object
# defines the name and type of the Service Server you will work with.
self.find_wall_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.find_wall_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...')
self.req = FindWall.Request()
self.future = FindWall.Response()
# create the publisher object
self.publisher_ = self.create_publisher(Twist, 'cmd_vel', 10)
print(1)
# create the subscriber object
self.subscriber = self.create_subscription(LaserScan, '/scan', self.laser_callback, QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE), callback_group=self.group2)
print(2)
#self.subscriber = self.create_subscription(LaserScan, '/scan', self.laser_callback, 10)
# define the timer period for 0.5 seconds
self.timer_period = 0.5
# define the variable to save the received info
self.laser_right = 0
# define the variable to save the received info
self.laser_forward = 0
# create a Twist message
self.cmd = Twist()
print(3)
self.timer = self.create_timer(self.timer_period, self.motion,callback_group=self.group3)
print(4)
# create Boolean for synchronous request
self.request_sent = False
def send_request(self):
# display message
self.get_logger().info('Sending request...')
# send the request
self.future = self.client.call_sync(self.req)
return self.client.call_sync(self.req)
def laser_callback(self,msg):
# Save the frontal laser scan info at 0°
self.laser_forward = msg.ranges[359]
self.laser_right = msg.ranges[179]
def motion(self):
if self.request_sent is False:
self.request_sent = True
self.send_request()
if self.future.result() is False:
self.get_logger().info('Service response not received yet')
time.sleep(1)
else:
print(5)
# Test movement + connection
"""self.cmd.linear.x = 0.1
self.cmd.angular.z = 0.5
self.publisher_.publish(self.cmd)"""
print(6)
# print the data
self.get_logger().info('I receive laser right: "%s"' % str(self.laser_right))
self.get_logger().info('I receive laser forward: "%s"' % str(self.laser_forward))
# Logic of move
linear_velocity = 0.1
angular_velocity_high = 0.5
angular_velocity_low = 0.1
if self.laser_forward < 0.5:
self.cmd.linear.x = linear_velocity
self.cmd.angular.z = angular_velocity_high
else:
if self.laser_right > 0.3:
self.cmd.linear.x = linear_velocity
self.cmd.angular.z = angular_velocity_low
self.get_logger().info("first")
elif self.laser_right > 0.2 and self.laser_right < 0.3:
self.cmd.linear.x = linear_velocity
self.cmd.angular.z = 0.0
self.get_logger().info("second")
else:
self.cmd.linear.x = linear_velocity
self.cmd.angular.z = -1*angular_velocity_low
self.get_logger().info("third")
# Publishing the cmd_vel values to a Topic
self.publisher_.publish(self.cmd)
def main(args=None):
# initialize the ROS communication
rclpy.init(args=args)
try:
# declare the node constructor
wall_following = Wall_Following_Node()
executor = MultiThreadedExecutor(num_threads=4)
executor.add_node(wall_following)
try:
executor.spin()
response = wall_following.send_request()
finally:
executor.shutdown()
wall_following.destroy_node()
finally:
rclpy.shutdown()
# pause the program execution, waits for a request to kill the node (ctrl+c)
#rclpy.spin(wall_following)
# Explicity destroy the node
#wall_following.destroy_node()
# shutdown the ROS communication
#rclpy.shutdown()
if name == ‘main’:
main()
