Launching service server and service client at the same time

Course Support ROS2 In 5 Days Python
1

Hi!
I have a situation similar to this issue.

I think I have implemented the suggestions here but am missing something, since my robot does not do what it’s supposed to yet. I can start my wallfollower and it successfully calls the service, and the robot aligns itself (with the left side instead of the right, but it does finish the movement). However, once the robot is aligned it starts moving forward uncontrolled, and in the log I am getting messages from both the follower and the finder service, so I think I am somehow not stopping the call to the service. I can’t seem to figure out why, so help would be greatly appreciated, thanks! Here is my terminal output and code:

findwall_service-1] [INFO] [1719760460.437753388] [findwall_service]: step 3 - right: "0.2651532292366028"
[findwall_service-1] [INFO] [1719760460.493412154] [findwall_service]: getting laser scan
[follower-2] [INFO] [1719760460.514687905] [follower]: Service response not received yet
[findwall_service-1] [INFO] [1719760460.939144703] [findwall_service]: step 3 - right: "0.22146907448768616"
[findwall_service-1] [INFO] [1719760460.939799199] [findwall_service]: stopping
[findwall_service-1] [INFO] [1719760460.940233245] [findwall_service]: response.wallfound: "True"
[follower-2] [INFO] [1719760460.948193756] [follower]: front: "1.1211130619049072"
[follower-2] [INFO] [1719760460.948695904] [follower]: right: "0.8934889435768127"
[findwall_service-1] [INFO] [1719760460.995074109] [findwall_service]: getting laser scan
[follower-2] [INFO] [1719760461.010973597] [follower]: front: "1.1211130619049072"
[follower-2] [INFO] [1719760461.011688821] [follower]: right: "0.8934889435768127"
[findwall_service-1] [INFO] [1719760461.442780875] [findwall_service]: getting laser scan
[findwall_service-1] [INFO] [1719760461.501622220] [findwall_service]: getting laser scan
[follower-2] [INFO] [1719760461.511351069] [follower]: front: "1.1538267135620117"
[follower-2] [INFO] [1719760461.511957754] [follower]: right: "0.8253895044326782"
[findwall_service-1] [INFO] [1719760461.944470021] [findwall_service]: getting laser scan
[findwall_service-1] [INFO] [1719760462.009681391] [findwall_service]: getting laser scan
[follower-2] [INFO] [1719760462.011536775] [follower]: front: "1.1280021667480469"
[follower-2] [INFO] [1719760462.012085198] [follower]: right: "0.5611406564712524"
[findwall_service-1] [INFO] [1719760462.448191340] [findwall_service]: getting laser scan
[follower-2] [INFO] [1719760462.513550997] [follower]: front: "1.116640567779541"
[findwall_service-1] [INFO] [1719760462.513754961] [findwall_service]: getting laser scan
[follower-2] [INFO] [1719760462.514165805] [follower]: right: "0.43443185091018677"
[findwall_service-1] [INFO] [1719760462.952012308] [findwall_service]: getting laser scan
[follower-2] [INFO] [1719760463.014229655] [follower]: front: "1.1221697330474854"
[follower-2] [INFO] [1719760463.014856244] [follower]: right: "0.33829182386398315"
[findwall_service-1] [INFO] [1719760463.015039358] [findwall_service]: getting laser scan
[findwall_service-1] [INFO] [1719760463.455380979] [findwall_service]: getting laser scan
[follower-2] [INFO] [1719760463.514109152] [follower]: front: "1.1334280967712402"
[follower-2] [INFO] [1719760463.514818347] [follower]: right: "0.2733255922794342"
[findwall_service-1] [INFO] [1719760463.519017944] [findwall_service]: getting laser scan
[findwall_service-1] [INFO] [1719760463.956829902] [findwall_service]: getting laser scan
[follower-2] [INFO] [1719760464.012613694] [follower]: front: "1.1659984588623047"

and this is the wall follower code:

import time
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from rclpy.qos import ReliabilityPolicy, QoSProfile
from custom_interfaces.srv import FindWall
from threading import Thread
from rclpy.callback_groups import ReentrantCallbackGroup, MutuallyExclusiveCallbackGroup
from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor


class WallFollower(Node):

    def __init__(self):
        super().__init__('follower')
        self.group1 = ReentrantCallbackGroup()
        self.group2 = ReentrantCallbackGroup()
        self.group3 = ReentrantCallbackGroup()

        self.client = self.create_client(FindWall, 'findwall', callback_group=self.group1)
        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...')
        self.req = FindWall.Request()
        self.future = FindWall.Response()

        self.subscriber = self.create_subscription(
            LaserScan,
            '/scan',
            self.listener_callback,
            QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE),
            callback_group=self.group2)

        self.publisher = self.create_publisher(Twist, 'cmd_vel', 10)

        
        self.timer_period = 0.5
        self.cmd = Twist()
        self.timer = self.create_timer(self.timer_period, self.motion, callback_group=self.group3)
        self.laser_forward = 0
        self.laser_right = 0
        self.req_sent = False

    def listener_callback(self, msg):
        self.laser_forward = msg.ranges[390] #71
        self.laser_right = msg.ranges[290] #17

    def send_request(self):
        self.get_logger().info('SENDING REQUEST...')
        self.future = self.client.call(self.req) #self.client.call_async(self.req)


    def motion(self):
        if self.req_sent == False:
            self.req_sent = True
            self.send_request()

        if not self.future.wallfound:
            self.get_logger().info('Service response not received yet')
            time.sleep(1)

        else:

            self.get_logger().info('front: "%s"' % str(self.laser_forward))
            self.get_logger().info('right: "%s"' % str(self.laser_right))

            if self.laser_forward < 0.8:
                self.cmd.linear.x = 0.0
                self.cmd.angular.z = 0.3 #-
            
            elif self.laser_right > 0.3:
                self.cmd.linear.x = 0.1
                self.cmd.angular.z = -0.2 #+

            elif self.laser_right < 0.2:
                self.cmd.linear.x = 0.1
                self.cmd.angular.z = 0.2 #-
            
            else:
                self.cmd.linear.x = 0.1
                self.cmd.angular.z = 0.0

            self.publisher.publish(self.cmd)
        
            


def main(args=None):
    rclpy.init(args=args)
    try:
        follower = WallFollower()
        executor = MultiThreadedExecutor(num_threads = 4)
        executor.add_node(follower)
        try:
            executor.spin()
        finally: 
            executor.shutdown()
            follower.destroy_node()
    finally:
        rclpy.shutdown()
        

if __name__ == '__main__':
    main()

and the wall finding service:

from geometry_msgs.msg import Twist

#import std_msgs
from custom_interfaces.srv import FindWall
from sensor_msgs.msg import LaserScan
from rclpy.qos import ReliabilityPolicy, QoSProfile
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.executors import MultiThreadedExecutor



import rclpy
from rclpy.node import Node
import time


class Service(Node):

    def __init__(self):

        super().__init__('findwall_service')
        self.reentrant_group_1 = ReentrantCallbackGroup()
        self.srv = self.create_service(FindWall, 'findwall', self.findwall_callback, callback_group=self.reentrant_group_1)
       
        self.publisher_ = self.create_publisher(Twist, 'cmd_vel', 10)

        self.subscriber = self.create_subscription(
            LaserScan,
            '/scan',
            self.listener_callback,
            QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE),
            callback_group=self.reentrant_group_1)

        self.cmd = Twist()
        self.laser_forward = 0
        self.laser_right = 0
        self.allranges = []

    def listener_callback(self, msg):
        time.sleep(0.5)
        self.get_logger().info('getting laser scan')

        self.laser_forward = msg.ranges[1] #71
        self.laser_right = msg.ranges[270] #17
        self.allranges = msg.ranges

    def findwall_callback(self, request, response):

        margin = 0.05 # the front might not be the minimum because there are a lot of scanners and we have the sleep,so we are good with almost minimum
        # turn until the front ray is the shortest
        while self.laser_forward - min(self.allranges) > margin:
            time.sleep(0.5)

            self.cmd.angular.z = 0.3
            self.publisher_.publish(self.cmd)
            self.get_logger().info('min: "%s"' % str(min(self.allranges)))

            self.get_logger().info('step 1 - front: "%s"' % str(self.laser_forward))



        self.cmd.angular.z = 0.0
        self.publisher_.publish(self.cmd)

        while self.laser_forward > 0.3:
            #time.sleep(0.5)
            self.cmd.linear.x = 0.05
            self.publisher_.publish(self.cmd)
            self.get_logger().info('step 2 - front: "%s"' % str(self.laser_forward))


        self.cmd.linear.x = 0.0
        self.publisher_.publish(self.cmd)

        while self.laser_right - min(self.allranges) > margin:
            time.sleep(0.5)

            self.cmd.angular.z = 0.3
            self.publisher_.publish(self.cmd)
            self.get_logger().info('step 3 - right: "%s"' % str(self.laser_right))


        self.cmd.angular.z = 0.0
        self.publisher_.publish(self.cmd)
        self.get_logger().info('stopping')


        response.wallfound = True
        self.get_logger().info('response.wallfound: "%s"' % str(response.wallfound))

        return response



def main(args=None):
    # initialize the ROS communication
    rclpy.init(args=args)
    # declare the node constructor  
    findwall_service = Service()
    executor = MultiThreadedExecutor(num_threads=2)
    executor.add_node(findwall_service)
    # pause the program execution, waits for a request to kill the node (ctrl+c)
    try:
        executor.spin()
    finally:
        findwall_service.destroy_node
    # shutdown the ROS communication
        rclpy.shutdown()


if __name__ == '__main__':
    main()

thank you very much for any help!

2

Hi @Janka ,

Welcome to this Community!

In your wall_follower code, I see that you are never stopping the robot. There is either linear or angular velocity that is always non-zero in your code. That is bad. At some point of time, you must have both linear and angular velocities set to zeros, to make the robot stop.

In your wall_finding code, you have the same issue again. At some point you are setting linear to be zero and at another point of time, you are setting angular to be zero. But you are never setting both speeds to zeros. That is why your robot is not stopping.

When you are looking for the wall, you must always do an iterative scan->turn->stop loop. So basically your robot would do laser scan, do a small turn on the direction you prefer and then stop. When stopped, it will check if the desired index is facing the wall. If you don’t stop the robot with iterative loop, either linear speed or angular speed will always be in action, and the robot will overshoot it’s target angle.

This is more of a conceptual and behavioral understanding than a logic or algorithm based method. Your logic is correct, but it is not robust.

So to stop your robot when wall is found, apply the scan->turn->stop method in your logic. At some point your robot will face the correct direction index (the direction of wall). At that point of time the robot will be stopped. And when you set the wallfound variable to true, make sure that your service sets both the speeds of the robot to zeros, before sending the response back to wall follower program.

This should definitely fix your problem.

Regards,
Girish

3

Hi,
Thank your very much for your explanation! I will update my code to stop the robot, this is really helpful.

Cheers,
Janka

4

Hi @Janka ,

You’re welcome!
Do let me know if your issue is resolved once you implement that logic that I explained.

Also, mark the respective post as “Solution”, so this issue can be closed.

Regards,
Girish

5

Hi Girish,

My robot is working much better now, thank you very much for the help!

Cheers,
Janka

6

Hi @Janka ,

Glad to know that your problem is solved and that your robot is working correctly now!

Regards,
Girish

7

This topic was automatically closed 5 days after the last reply. New replies are no longer allowed.