Issue with ROS2 Python Basics Real Project Part 2. After calling main.launch.py, only first "Wall finding" part of my program runs

Course Support ROS2 In 5 Days Python
1

The Wall Following part of my program doesnt seem to initiate. I cant seem to understand why the further part of my main block isnt being reached even after destroy node. I have added wall_following code below

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

class ClientSync(Node):
    def __init__(self):
        super().__init__('find_wall_client')
        self.client = self.create_client(FindWall,'find_wall')
        while not self.client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('service not available')
        self.req = FindWall.Request()
    
    def send_request(self):
        self.future = self.client.call_async(self.req)


class WallFollow(Node):
    def __init__(self):
        super().__init__('follow_wall')
        self.group1= MutuallyExclusiveCallbackGroup()
        self.publisher_ = self.create_publisher(Twist, 'cmd_vel', 10)
        self.subscriber= self.create_subscription(LaserScan, '/scan', self.laser_callback, QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE), callback_group=self.group1) 

        self.timer_period= 0.5
        self.laser_right= 0
        self.cmd = Twist()
        

    def laser_callback(self, msg):
        self.laser_right = msg.ranges[270]
        self.laser_front = msg.ranges[359]
        

    def motion(self):
        self.get_logger().info('I receive: "%s"'%str(self.laser_right))
        self.get_logger().info('I front "%s"'%str(self.laser_front))

        if self.laser_front < 0.4:
            self.cmd.linear.x=0.05
            self.cmd.angular.z= -0.8
            self.get_logger().info('Making Turn')

        else:

            if self.laser_right > 0.3 :
                self.cmd.linear.x=0.03
                self.cmd.angular.z=  0.09
                self.get_logger().info('Moving Towards Wall')

            elif self.laser_right < 0.2:
                self.cmd.linear.x=0.03
                self.cmd.angular.z= -0.09
                self.get_logger().info('Moving Away Wall')

            else:
                self.cmd.linear.x=0.03
                self.cmd.angular.z = 0.00
                    
                self.get_logger().info('Straight')
            
        
        self.publisher_.publish(self.cmd)

def main(args=None):
    rclpy.init(args=args)
    client = ClientSync()
    client.send_request()

    while rclpy.ok():
        rclpy.spin_once(client)
        if client.future.done():
            try:
                response=client.future.result()
            except Exception as e:
                client.get_logger().info('Service call failed %r' %(e,))
            else:
                client.get_logger().info("Robot is ready to follow the wall")
            break
    client.destroy_node()

    executor = MultiThreadedExecutor(num_threads=4)
    wall_follow= WallFollow()
    executor.add_node(wall_follow)
    try:
        executor.spin()
    finally:
        executor.shutdown()
        wall_follow.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

This is the wall finder program

from geometry_msgs.msg import Twist
from new_interface.srv import FindWall
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
from rclpy.qos import ReliabilityPolicy, QoSProfile
from rclpy.callback_groups import ReentrantCallbackGroup, MutuallyExclusiveCallbackGroup
from rclpy.executors import MultiThreadedExecutor
import time
import math

class WallFinder(Node):

    def __init__(self):
        super().__init__('wall_finding_server')


        self.group1 = MutuallyExclusiveCallbackGroup()
        self.group2 = MutuallyExclusiveCallbackGroup()
        

        self.wall_find_srv = self.create_service(FindWall, 'find_wall', self.empty_callback, callback_group=self.group1)

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

        self.laser_sub= self.create_subscription(LaserScan, '/scan', self.laser_callback, QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT), callback_group=self.group2)
        
        self.timer_period= 0.5
        self.laser_front= 0
        self.laser_right= 0
        self.min_range= 100
        self.min_index= 0
        self.turn_complete= False
        i=0
        
        self.cmd= Twist()



    def laser_callback(self,msg):
        self.laser_right =  msg.ranges[270]
        #270 for sim 180 for actual robot
        self.laser_front = msg.ranges[359]
        ranges= msg.ranges
        self.min_range = min(ranges)
        self.min_index = ranges.index(self.min_range)

    def turn_to_wall(self):
        while self.turn_complete==False:
            self.get_logger().info('Turning towards wall "%s"' % str(self.min_index))

            if 1 <= self.min_index <=180:
                self.cmd.linear.x = 0.0
                self.cmd.angular.z = 0.3
                self.get_logger().info('Turning')
            elif 180 < self.min_index < 357:
                self.cmd.linear.x = 0.0
                self.cmd.angular.z = -0.3
                self.get_logger().info('Turning')
            elif 357 < self.min_index < 362:
                self.cmd.linear.x = 0.0
                self.cmd.angular.z = 0.0
                self.turn_complete = True
                self.get_logger().info('Stop')
                break

            self.mov_pub.publish(self.cmd)
            time.sleep(0.1)

    def parallel_wall(self):
        for i in range(150):
            self.get_logger().info('turning parallel to wall: "%s"' % str(self.min_index))
            if 0 < self.min_index < 90 or 272 < self.min_index < 359:
                self.cmd.linear.x = 0.0
                self.cmd.angular.z = 0.3
            if 90 < self.min_index < 268:
                self.cmd.linear.x = 0.0
                self.cmd.angular.z = -0.3
            elif 268 < self.min_index < 272:
                self.cmd.linear.x = 0.0
                self.cmd.angular.z = 0.0
                break
            
            self.mov_pub.publish(self.cmd)
            time.sleep(0.1)
    
    def stop(self):
        self.cmd.linear.x = 0.0
        self.cmd.angular.z = 0.0
        self.mov_pub.publish(self.cmd)
        time.sleep(0.1)

    def approach_wall(self):
        for i in range(150):
            self.get_logger().info('Distance between robot and wall: "%s"' %str(self.laser_front))
            if self.laser_front > 0.28 :
                self.cmd.linear.x = 0.03
                self.cmd.angular.z = 0.0
            elif 90 < self.laser_front <= 0.27:
                self.cmd.linear.x = -0.01
                self.cmd.angular.z = 0.0
            else:
                self.cmd.linear.x = 0.0
                self.cmd.angular.z = 0.0
                break
            self.mov_pub.publish(self.cmd)
            time.sleep(0.1)


    def empty_callback(self, request, response):
        self.get_logger().info('Front distance: "%s"' % str(self.laser_front))
        self.get_logger().info('Min range"%s"' % str(self.min_range))
        self.get_logger().info('Min_index "%s"' % str(self.min_index))
        self.get_logger().info('Robot is now moving')
        
        self.turn_to_wall()
        self.approach_wall()
        self.parallel_wall()
        self.stop()
        response.wallfound= True
        self.get_logger().info('Mission complete status: "%s"' %str(response.wallfound))
        return response

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


if __name__ == '__main__':
    main()

This is where the output gets stuck

Screenshot 2024-04-01 161641
Screenshot 2024-04-01 161641787×355 87.7 KB

2

Another thing I have realized is that my wall_following program isnt working at all. When I run only the wall following launch file, nothing publishes. I distinctly remember testing Part 1 and getting it to work. Suddenly now after I edited the part 1 to accommodate the service, it has completely stopped working

3

Hi @neilthomas110 ,

Welcome to this Community!

Your service call must be synchronous, so you cannot call your service with call_async function.
You need to use the call function, which is synchronous.

You need to instantiate your service client node inside the wall follower node and add the service client node to executor. So here is a short pseudocode:

class WallFollow(Node):
    def __init__(self):
        # ...
        # instantiate ClientSync node here [assume variable is srvclient]
        # ...

And then in the main function, you need to add service client node to spin:

def main(args=None):
    # ...
    executor = MultiThreadedExecutor(num_threads=4)
    wall_follow= WallFollow()
    srvclient = wall_follow.srvclient
    executor.add_node(wall_follow)
    executor.add_node(srvclient)
    # ...
    srvclient.destroy_node()
    wall_follow.destroy_node()
    # ...

This is because you need to have wall_follower timer callback and laser scan callback running in parallel, so you need to use Reentrant callback group for these two callbacks with MultiThreadedExecutor.

These should fix your problem. If you still have issues, don’t hesitate to continue this thread!

Regards,
Girish

4

Thank you very much! I was able to get it to work. I also realised I must’ve deleted the publisher_timer while editing to add the service client which caused the wall following to not initiate

5

Hi @neilthomas110 ,

Glad to know you got your issue resolved!

Regard,
Girish

6

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