Error with last metric while working on Actions_quiz

Course Support ROS2 in 5 Days C++
1

Hi,
I am working on the actions_quiz package. While using Gradebot I am still getting wrong the last point.

image
image827×633 61.5 KB

I don’t know where the problem in my code is.
image
image1207×457 158 KB

My code for the execute funcion is the following:

void execute(const std::shared_ptr<GoalHandleDistance> goal_handle)
    {
        RCLCPP_INFO(this->get_logger(), "Executing goal");
        const auto goal = goal_handle->get_goal();
        auto feedback = std::make_shared<DistanceAct::Feedback>();
        auto & current_distance_feedback = feedback->current_dist;
        // message = "Starting movement...";
        auto result = std::make_shared<DistanceAct::Result>();
        auto distance_msg = std_msgs::msg::Float64();
        rclcpp::Rate loop_rate(1);
        float distance_traveled = 0.0;

        for (int i = 0; (i < goal->seconds) && rclcpp::ok(); ++i) {
            // Check if there is a cancel request
            if (goal_handle->is_canceling()) {
                result->status = true;
                result->total_dist = distance_traveled;
                goal_handle->canceled(result);
                RCLCPP_INFO(this->get_logger(), "Goal canceled");
                return;
            }
            // Move robot forward and send feedback
            distance_traveled = get_distance_traveled();
            current_distance_feedback = distance_traveled;
            distance_msg.data = distance_traveled;

            total_distance_publisher->publish(distance_msg);
            goal_handle->publish_feedback(feedback);
            RCLCPP_INFO(this->get_logger(), "Publish feedback");

            loop_rate.sleep();
        }

        // Check if goal is done
        if (rclcpp::ok()) {
            distance_traveled = get_distance_traveled();
            distance_msg.data = distance_traveled;
            result->status = true;
            result->total_dist = distance_traveled;
            total_distance_publisher->publish(distance_msg);
            goal_handle->succeed(result);
            RCLCPP_INFO(this->get_logger(), "Goal succeeded");
        }

The get_distance_traveled function is the following:

float get_distance_traveled(){
        float distance_traveled;
        distance_traveled = sqrt(pow(this->current_pos_x - this->start_pos_x, 2) + pow(this->current_pos_y - this->start_pos_y, 2));
        return distance_traveled;
    }

Thanks in advance for your help.
Ronaldo

2

Hi @w.ronaldo.cd ,

Your code seems to be working correctly. The only problem that I see is that the last value received from feedback is not the same as total distance travelled. Maybe that is what is causing the error.

Your feedback’s last value reports 6.5xxx while your total distance reports 7.0xxx.

Try to get both of them as the same value. That could fix your problem. Otherwise, I do not see any issues.

Regards,
Girish

3

Hi @girishkumar.kannan

I got my problem solved. First, I applied what you suggested and second I changed the position of the sleep function inside the loop. I put it before the get_distance_traveled() function.

void execute(const std::shared_ptr<GoalHandleDistance> goal_handle)
    {
        RCLCPP_INFO(this->get_logger(), "Executing goal");
        const auto goal = goal_handle->get_goal();
        auto feedback = std::make_shared<DistanceAct::Feedback>();
        auto & current_distance_feedback = feedback->current_dist;
        // message = "Starting movement...";
        auto result = std::make_shared<DistanceAct::Result>();
        auto distance_msg = std_msgs::msg::Float64();
        rclcpp::Rate loop_rate(1);
        float distance_traveled = 0.0;

        for (int i = 0; (i < goal->seconds) && rclcpp::ok(); ++i) {
            // Check if there is a cancel request
            if (goal_handle->is_canceling()) {
                result->status = true;
                result->total_dist = distance_traveled;
                goal_handle->canceled(result);
                RCLCPP_INFO(this->get_logger(), "Goal canceled");
                return;
            }
            // Move robot forward and send feedback
            loop_rate.sleep();
            distance_traveled = get_distance_traveled();
            current_distance_feedback = distance_traveled;
            distance_msg.data = distance_traveled;

            total_distance_publisher->publish(distance_msg);
            goal_handle->publish_feedback(feedback);
            RCLCPP_INFO(this->get_logger(), "Publish feedback");

            
        }

        // Check if goal is done
        if (rclcpp::ok()) {
            result->status = true;
            result->total_dist = distance_traveled;
            // total_distance_publisher->publish(distance_msg);
            goal_handle->succeed(result);
            RCLCPP_INFO(this->get_logger(), "Goal succeeded");
        }
    }

I just have an observation. In the jupyter notebook’s example of the output, it shows that after the loop finished, the program calculates once more the distance traveled. Last feedback and result are different (as I did previously):

image
image551×1090 75.7 KB

Thanks

4

Hi @w.ronaldo.cd ,

Nice! Glad to know that you have solved!

Good observation. But I actually don’t go strictly by the example images of outputs. When I did this chapter, I designed it in a way that feedback’s last value is same as distance travelled.
Gradebot gave me a 10 on the very first attempt. So I got happy and proceeded.

This is how I understood the output image:
The goal is 20 seconds - so there should be 20 feedback messages - result should have (true, total distance) and status must be succeeded.
It is implied that the total distance must be the same as last feedback value - because it is the logic.
I did not analyse it much deeper.

Regards,
Girish