Create an odometry publisher node in python ros2

Question

I would like to know how to create an odometry publisher node in python ros2 that can publish on the topic nav_msgs/Odometry and TFs (base_link with respect to the Odom frame). Could anyone tell me how to do that?

Answer 1

You could do something like the following -

import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry

class OdometryPublisher(Node):
    def __init__(self):
        super().__init__('odometry_publisher')
        self.publisher_ = self.create_publisher(Odometry, 'odometry', 10)

    def publish_odometry(self, x, y, z, quat_x, quat_y, quat_z, quat_w):
        msg = Odometry()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.pose.pose.position.x = x
        msg.pose.pose.position.y = y
        msg.pose.pose.position.z = z
        msg.pose.pose.orientation.x = quat_x
        msg.pose.pose.orientation.y = quat_y
        msg.pose.pose.orientation.z = quat_z
        msg.pose.pose.orientation.w = quat_w
        self.publisher_.publish(msg)

def main(args=None):
    rclpy.init(args=args)
    node = OdometryPublisher()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

In the above code, the OdometryPublisher class inherits from the rclpy.node.Node class and creates a publisher for the Odometry message on the odometry topic. The publish_odometry() method is used to publish an Odometry message with the given position and orientation data. The main function initializes the ROS 2 node, spins to handle incoming messages, and properly cleans up before shutting down.

You can call the publish_odometry(x, y, z, quat_x, quat_y, quat_z, quat_w) method to publish the odometry data you want.

Note: Make sure you have the nav_msgs package installed, otherwise it will cause error