Ros2 Basics in 5 Days (Python), Unit 5 Quiz, get current location

Course Support ROS2 In 5 Days Python
1

Hello,
I have a problem in actions quiz server.
I want to calculate the distance by taking the last location and the current location.
how do I get these values? I have created a subscriber to /odom topic but I get the location values just outside the loop of the movement. How do I get these values constantly? Can I call the “get_odom” method in my loop? how?
this is my code now:

import rclpy
from rclpy.action import ActionServer
from rclpy.node import Node

from actions_quiz_msg.action import Distance

from geometry_msgs.msg import Twist
from custom_interfaces.msg import Dist

from nav_msgs.msg import Odometry
from rclpy.qos import ReliabilityPolicy, QoSProfile

import time
from math import sqrt


class MyActionServer(Node):

    def __init__(self):
        super().__init__('actions_quiz_server')
        self._action_server = ActionServer(
            self, Distance, 'distance_as', self.execute_callback)
        self.cmd = Twist()  # Creating Twist parameter
        self.dst = Dist()  # Creating Dist parameter
        self.publisherMove_ = self.create_publisher(
            Twist, 'cmd_vel', 10)  # Create publisher for movement
        self.publisherDist_ = self.create_publisher(
            Dist, 'total_distance', 10)  # Create publisher for distance
        self.subscriber_ = self.create_subscription(
            Odometry, "/odom", self.get_odom, QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT))  # Create subscriber to odometry

        # Save initial values
        self.msg = Odometry()
        self.TraveledDistance = 0

    # This method will run every time thats /odom topic get new position from the robot
    def get_odom(self, msg):
        # Save the current odometry of position and orientation
        self.current_odom_x = msg.pose.pose.position.x
        self.current_odom_y = msg.pose.pose.position.y
       
    def execute_callback(self, goal_handle):

        self.get_logger().info('Executing goal...')  # First printing - server terminal

        feedback_msg = Distance.Feedback()  # Creating feedback message

        self.last_x = self.current_odom_x
        self.last_y = self.current_odom_y

        for i in range(1, goal_handle.request.seconds):

            # Calculate current distance traveled: (Calculated as straight lines)
            dx = (self.current_odom_x-self.last_x)
            dy = (self.current_odom_y-self.last_y)
            self.TraveledDistance = self.TraveledDistance + \
                sqrt((dx) ** 2+(dy) ** 2)
            self.last_x = self.current_odom_x
            self.last_y = self.current_odom_y

            # Insert calculated distance to variables:
            self.dst.dist = self.TraveledDistance

            # Insert value into feedback message
            feedback_msg.current_dist = self.TraveledDistance
            self.get_logger().info('current_dist: ' + str(self.TraveledDistance))

            # Print feedback
            self.get_logger().info('Feedback: ' + str(feedback_msg.current_dist))

            # Publish the feedback message
            goal_handle.publish_feedback(feedback_msg)

            # publish to /total_distance topic
            self.publisherDist_.publish(self.dst)

            # Publish feedback message

            # Set desired velocity and publish data
            self.cmd.linear.x = 0.2
            self.cmd.angular.z = 0.0
            self.publisherMove_.publish(self.cmd)

            time.sleep(1)

        goal_handle.succeed()  # Set Goal status as succeed

        # Stop the Robot
        self.cmd.linear.x = 0.0
        self.cmd.angular.z = 0.0
        self.publisherMove_.publish(self.cmd)

        result = Distance.Result()  # Creating result message
        result.total_dist = self.TraveledDistance
        result.status = True
        return result


def main(args=None):
    rclpy.init(args=args)

    actions_quiz_server = MyActionServer()

    rclpy.spin(actions_quiz_server)


if __name__ == '__main__':
    main()

Thanks

2

Hello @omerdanieli ,

You can achieve this in different ways. The simplest one would probably be to add a rclpy.spin_once(self) inside your action loop. This line makes your node execute all the pending callbacks, therefore updating their values.

1 Like