Save spot is not saving the turtlebot position

[ghassan11]

Hi everyone,

i am facing a problem in the Real robot project of the course (ROS2 Navigation in 5 days)
in chapter 2 localization part 5 we should do the following :

Create a service that saves these spots into a file.

In the previous section, you created a file with the coordinates of the 3 spots. But, the whole process was too cumbersome, don’t you think? Why don’t we create a ROS Service that does the work for us? Sounds like a good idea, right? Then, let’s do it!

So, now you’ll create a program that will do the following:

• It will launch a node named spot_recorder.
• This node will contain a service server named /save_spot that will take a string as input
• When this service is called, it will store the current coordinates of the robot (position and orientation values) with a label that will be the string provided on the service.
• When the end string is provided in the service call, the node will write all of the values stored into a file named spots.txt.
• Finally, the service will return a message indicating if the file has been saved correctly.

My problem is that when i call the service to save the point it respond with false :

this is my launch file:

this is the service when it is called :

this is my code :

from custom_interfaces.srv import MyServiceMessage
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseWithCovarianceStamped

class Service(Node):
    def __init__(self):
        super().__init__('spot_recorder')
        self.service = self.create_service(MyServiceMessage, 'save_spot', self.custom_service_callback)
        self.subscription = self.create_subscription(
            PoseWithCovarianceStamped,
            'amcl_pose',
            self.listener_callback,
            10)
        self.robot_pose = PoseWithCovarianceStamped()
        self.pose_received = False
        self.text_to_save = str()

    def listener_callback(self, msg):
        self.robot_pose = msg.pose.pose
        self.pose_received = True
        self.get_logger().info(f'I heard: x={self.robot_pose.position.x}, y={self.robot_pose.position.y}')

    def custom_service_callback(self, request, response):
        if not self.pose_received:
            self.get_logger().info('I don\'t have a pose to save')
            response.navigation_successfull = False
            response.message = 'No pose to save'
            return response

        if request.label == 'end':
            file_path = '/home/user/ros2_ws/src/spot_recorder_service/config/spots.txt'
            try:
                with open(file_path, 'w') as file:
                    file.write(self.text_to_save)
                response.navigation_successfull = True
                response.message = 'File saved successfully'
            except Exception as e:
                response.navigation_successfull = False
                response.message = f'Failed to save file: {e}'
        else:
            self.text_to_save += f'Label: {request.label}\n'
            self.text_to_save += 'Position:\n'
            self.text_to_save += f'- x: {self.robot_pose.position.x}\n'
            self.text_to_save += f'- y: {self.robot_pose.position.y}\n'
            self.text_to_save += f'- z: {self.robot_pose.position.z}\n'
            self.text_to_save += 'Orientation:\n'
            self.text_to_save += f'- x: {self.robot_pose.orientation.x}\n'
            self.text_to_save += f'- y: {self.robot_pose.orientation.y}\n'
            self.text_to_save += f'- z: {self.robot_pose.orientation.z}\n'
            self.text_to_save += f'- w: {self.robot_pose.orientation.w}\n\n'
            response.navigation_successfull = True
            response.message = 'Spot added'
            self.get_logger().info('Spot added')

        return response

def main(args=None):
    rclpy.init(args=args)
    service = Service()
    rclpy.spin(service)
    service.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

I hope anyone can help me in my issue or tell me why it responds with False

Thank you

Ghassan

[girishkumar.kannan]

Hi @ghassan11 ,

Your listener_callback is getting blocked in your program because you have not used MultiThreadedExecutor.

Since you have defined your service (along with its callback) as the first item in your initialization function, the laser callback is not being called, since the service callback has become the registered default callback for this node.

If you switch the laser subscriber above the service declaration, then your service callback will get blocked and your laser callback will be running.

Only one callback will be assigned to a node by default, in your case the first callback is referenced to service callback, so the laser callback gets blocked.

Your solution is to just implement MultiThreadedExecutor with at least 2 threads and set ReentrantCallbackGroup for both the callbacks - service and laserscan.

That should fix your issue.

Regards,
Girish