Issue with Quiz Submission for Action Server Using Odometry to Calculate Distance Traveled

[7uthaifa]

I’ve been working on a quiz that involves creating an Action Server to compute the total distance traveled by a robot based on odometry data. I believe I have implemented the correct solution, and my code works well when tested manually. However, despite these successful tests, my quiz submission continues to receive the following feedback:

“The total distance traveled is not computed correctly. Are you doing it manually? Are you using odometry to compute the distance traveled? Move the robot with the keyboard teleop and check that the distance traveled is computed correctly.”

Problem Details:

• I am using the odometry data from the /odom topic to calculate the distance traveled using the Euclidean formula: distance=(x2−x1)2+(y2−y1)2\text{distance} = \sqrt{(x_2 - x_1)^2 + (y_2 - y_1)^2}distance=(x2​−x1​)2+(y2​−y1​)2​
• The action server correctly publishes the feedback on the /total_distance topic, and I can see the distance increasing as the robot moves when I use the keyboard teleop.
• The Action Server also returns the correct total distance when the goal is completed.

What I’ve Tried:

1. Verified that the odometry messages are being processed correctly, and the robot’s position (x, y) is being updated properly.
2. Tested the code extensively with keyboard teleop, and the distance calculation appears accurate.
3. Monitored the feedback and result values, and they seem to reflect the correct total distance.

Issue:

Despite the code working correctly in manual tests, the quiz system still marks my solution as incorrect with the same error message about computing the distance manually.

Code:

Here is the current version of my Action Server implementation:

python

Copy code

import rclpy
from rclpy.action import ActionServer
from rclpy.node import Node
from actions_quiz_msg.action import Distance
from nav_msgs.msg import Odometry
from std_msgs.msg import Float64
import math

class DistanceActionServer(Node):

    def __init__(self):
        super().__init__('distance_action_server')
        self._action_server = ActionServer(
            self,
            Distance,
            '/distance_as',
            self.execute_callback
        )
        self.current_distance = 0.0
        self.previous_x = None
        self.previous_y = None

        # Subscribe to odometry data for calculating the distance
        self.subscription = self.create_subscription(
            Odometry,
            '/odom',
            self.odom_callback,
            10
        )
        
        # Publisher to publish current total distance
        self.distance_publisher = self.create_publisher(
            Float64,
            '/total_distance',
            10
        )
        
        self.get_logger().info('Distance Action Server has been started.')

    def odom_callback(self, msg):
        # Extract the current position from Odometry message
        x = msg.pose.pose.position.x
        y = msg.pose.pose.position.y
        
        # If this is the first odom message, store the position
        if self.previous_x is None:
            self.previous_x = x
            self.previous_y = y
            self.get_logger().info('First odometry message received. Initializing position.')
            return  # Exit early since there's no previous position to calculate distance from

        # Calculate distance using Euclidean formula
        dx = x - self.previous_x
        dy = y - self.previous_y
        distance_delta = math.sqrt(dx**2 + dy**2)
        self.current_distance += distance_delta

        # Update previous position
        self.previous_x = x
        self.previous_y = y

        # Publish the current distance to the /total_distance topic
        distance_msg = Float64()
        distance_msg.data = self.current_distance
        self.distance_publisher.publish(distance_msg)

        # Log the current total distance
        self.get_logger().info(f'Current total distance: {self.current_distance} meters.')

    def execute_callback(self, goal_handle):
        self.get_logger().info(f'Received goal: Monitor distance for {goal_handle.request.seconds} seconds.')

        feedback_msg = Distance.Feedback()
        total_duration = goal_handle.request.seconds
        start_time = self.get_clock().now()

        while (self.get_clock().now() - start_time).nanoseconds < total_duration * 1e9:
            # Send feedback at 1-second intervals
            rclpy.spin_once(self, timeout_sec=1)
            feedback_msg.current_dist = self.current_distance
            goal_handle.publish_feedback(feedback_msg)

            self.get_logger().info(f'Sending feedback: Current distance is {feedback_msg.current_dist} meters.')

        goal_handle.succeed()

        # Log that the action is complete
        self.get_logger().info(f'Goal completed. Total distance traveled: {self.current_distance} meters.')

        # Return the result after the specified time
        result = Distance.Result()
        result.status = True
        result.total_dist = self.current_distance
        return result


def main(args=None):
    rclpy.init(args=args)
    action_server = DistanceActionServer()
    rclpy.spin(action_server)
    action_server.destroy()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

Questions:

• Has anyone encountered a similar issue with the quiz system?
• Are there specific edge cases or tests that I might be missing?
• Could this be related to the environment setup, or something else I should try to investigate?

Any feedback or suggestions would be greatly appreciated.

Thank you for your help!