ROS2 Basic Python Service problem with subscriber

Course Support ROS Basics In 5 Days (C++)
1

Hello:

I´m trying to make the ROS2 Basic en python cusrses part of services in the simulation and I´m having a problem.

I create a service server and a subscriber for the /scan topic in the same node, and I´ve try to use Multithread executor to get measures of laser scan while I´m making corrections of the speed in the server but is not working and I cannot find what is happening. Could somebody give a look to the code and tell me were can I have the problem?

Here is the code:

# import the Empty module from std_servs service interface
#from std_srvs.srv import Empty
from custom_srv_interfaces.srv import FindWall
# 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 rclpy.qos import ReliabilityPolicy, QoSProfile
from sensor_msgs.msg import LaserScan
from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor




class FindwallService(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')
        # create the service server object
        # defines the type, name and callback function
        self.srv = self.create_service(FindWall, 'find_wall', 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.listener_callback, QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT))
        
        self.subscriber

        self.laser_front = 0
        self.laser_right = 0

            
        

    def Empty_callback(self, request, response):
        # The callback function recives the self class parameter, 
        # received along with two parameters called request and response
        # - receive the data by request
        # - return a result as response


        near_wall = False

        vel = Twist()

        while near_wall == False:
            self.get_logger().info("Front distance %s" %str(self.laser_front))
            self.get_logger().info("Right distance %s" %str(self.laser_right))

            if self.laser_front > 0.5:
                self.get_logger().info("Vel = 0.2")
                vel.linear.x = 0.2
                publisher.publish(vel)

            elif self.laser_front < 0.5 and self.laser_front > 0.1:
                self.get_logger().info("Vel = 0.1")
                vel.linear.x = 0.1
                publisher.publish(vel)

            else:
                vel.linear.x = 0
                publisher.publish(vel)
                near_wall = True
                self.get_logger().info("Near the wall")
                response.wallfound = True
                break

            
            self.get_logger().info("Publishig speed")
        
        # return the response parameter
        return response


    def listener_callback (self, laser):

        self.get_logger().info("Reading")


        self.laser_front = laser.ranges[0]
        self.laser_right = laser.ranges[120]

        #self.get_logger().info("Front distance %s" %str(self.laser_front))
        #self.get_logger().info("Right distance %s" %str(self.laser_right))

        



def main(args=None):
    # initialize the ROS communication
    rclpy.init(args=args)

    try:
    # declare the node constructor
        findwall_service = FindwallService()

        executor = MultiThreadedExecutor(num_threads=2)
        executor.add_node(findwall_service)

        try:
            # pause the program execution, waits for a request to kill the node (ctrl+c)
            executor.spin()
        finally:
            executor.shutdown()
            findwall_service.destroy_node()

    finally:
        # shutdown the ROS communication
        rclpy.shutdown()

if __name__ == '__main__':
    main()

Thanks for the help

2

Hi, welcome to the community!

Can you describe what is not working about your program?
I see that the main() function looks correct, but I can’t see the multi threading needed for each callback function:

from rclpy.callback_groups import MutuallyExclusiveCallbackGroup

class FindwallService(Node):

    def __init__(self:

        # Multithreading 
        self.group1 = MutuallyExclusiveCallbackGroup()
        self.group2 = MutuallyExclusiveCallbackGroup()
# defines the type, name and callback function
        self.srv = self.create_service(FindWall, '/find_wall', self.Empty_callback, callback_group=self.group1)
3

Hello roalgoal and thanks for your answer. That whas exactly what I had missed. The multi-threading is not explained in the course so I didn´t know that you need to separate in callback groups. You solve my problem, thanks a lot!!!

1 Like