TrackingFeedback Message

Package: nav2_msgs
Category: Other Messages

Real-time tracking error for Nav2 controller

Message Definition

Field Type Description
header std_msgs/Header Real-time tracking error for Nav2 controller
tracking_error float32 The sign of the tracking error indicates which side of the path the robot is on Positive sign indicates the robot is to the left of the path, negative to the right
current_path_index uint32 Integer numerical value
robot_pose geometry_msgs/PoseStamped Pose with header information (frame_id and timestamp)
distance_to_goal float32 Floating point numerical value
speed float32 Speed limit value in meters per second
remaining_path_length float32 Floating point numerical value

Usage Examples

Python

import rclpy
from rclpy.node import Node
from nav2_msgs.msg import TrackingFeedback

class TrackingFeedbackPublisher(Node):
    def __init__(self):
        super().__init__('trackingfeedback_publisher')
        self.publisher = self.create_publisher(TrackingFeedback, 'trackingfeedback', 10)
        
    def publish_message(self):
        msg = TrackingFeedback()
        msg.header.frame_id = 'map'
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.tracking_error = 0.0
        msg.current_path_index = 0
        # Set msg.robot_pose as needed
        msg.distance_to_goal = 0.0
        msg.speed = 0.0
        msg.remaining_path_length = 0.0
        self.publisher.publish(msg)

C++

#include "rclcpp/rclcpp.hpp"
#include "nav2_msgs/msg/tracking_feedback.hpp"

class TrackingFeedbackPublisher : public rclcpp::Node
{
public:
    TrackingFeedbackPublisher() : Node("trackingfeedback_publisher")
    {
        publisher_ = create_publisher<nav2_msgs::msg::TrackingFeedback>("trackingfeedback", 10);
    }

    void publish_message()
    {
        auto msg = nav2_msgs::msg::TrackingFeedback();
        msg.header.frame_id = "map";
        msg.header.stamp = this->now();
        msg.tracking_error = 0.0;
        msg.current_path_index = 0;
        // Set msg.robot_pose as needed
        msg.distance_to_goal = 0.0;
        msg.speed = 0.0;
        msg.remaining_path_length = 0.0;
        publisher_->publish(msg);
    }

private:
    rclcpp::Publisher<nav2_msgs::msg::TrackingFeedback>::SharedPtr publisher_;
};