Odom topic in python

[r0n1tr]

Hi sir,

We are using a python script to overlay odometry feed from a cloud generated environment (.ply) that is converted into a 2d occupancy grid for autonomous navigation, it seems to be that there's a significant difference in latency between the odom feed in rtab_map and then using it in python. Additionally, when we do "reset odometry" in rtabmap, the odometry shown in rtabmap corrects itself once localised, but the odometry topic in python is not corrected, and becomes very incorrect. I was wondering if there was a specific reason or a severe oversight on our part. Thank you for your time.

Here is our code for extra context:

import os
import threading
import rospy
import open3d as o3d
import numpy as np
import matplotlib.pyplot as plt
from sensor_msgs.msg import PointCloud2
from nav_msgs.msg import Odometry
import sensor_msgs.point_cloud2 as pc2

class OccupancyMapNode:
    def _init_(self, resolution=0.01):
        self.resolution = resolution
        self.occupancy_grid = None
        self.odometry_x = 0.0
        self.odometry_y = 0.0
        self.odometry_yaw = 0.0
        self.robot_height = None
        self.lock = threading.Lock()
        self.height_threshold_lower = 0.1  # Exclude points below this height
        
        # Set the environment variables
        local_ip = "172.17.0.2"
        remote_pi_ip = "192.168.137.26"
        os.environ['ROS_IP'] = local_ip
        os.environ['ROS_MASTER_URI'] = f"http://{remote_pi_ip}:11311"
        
        rospy.init_node('occupancy_map_node', anonymous=True)
        
        # Subscribe to the point cloud and odometry topics
        point_cloud_topic = "/rtabmap/cloud_obstacles"
        odometry_topic = "/rtabmap/odom"
        self.point_cloud_subscriber = rospy.Subscriber(point_cloud_topic, PointCloud2, self.point_cloud_callback)
        self.odometry_subscriber = rospy.Subscriber(odometry_topic, Odometry, self.odometry_callback)

        # Start ROS thread
        self.ros_thread = threading.Thread(target=self.ros_spin)
        self.ros_thread.start()

        plt.ion()
        self.fig, self.ax = plt.subplots()

    def ros_spin(self):
        rospy.spin()

    def point_cloud_callback(self, msg):
        points = []
        for p in pc2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True):
            points.append([p[0], p[1], p[2]])
        points = np.array(points)
        
        if len(points) == 0:
            print("No points found in the point cloud :(")
            return

        with self.lock:
            self.occupancy_grid = points

    def odometry_callback(self, msg):
        position = msg.pose.pose.position
        orientation = msg.pose.pose.orientation
        with self.lock:
            self.odometry_x = position.x
            self.odometry_y = position.y
            quaternion = (orientation.x, orientation.y, orientation.z, orientation.w)
            euler = self.quaternion_to_euler(quaternion)
            self.odometry_yaw = euler[2]  # Yaw is the third element

    def quaternion_to_euler(self, quaternion):
        # Convert quaternion to euler angles (roll, pitch, yaw)
        x, y, z, w = quaternion
        t0 = +2.0 * (w * x + y * z)
        t1 = +1.0 - 2.0 * (x * x + y * y)
        roll = np.arctan2(t0, t1)
        
        t2 = +2.0 * (w * y - z * x)
        t2 = +1.0 if t2 > +1.0 else t2
        t2 = -1.0 if t2 < -1.0 else t2
        pitch = np.arcsin(t2)
        
        t3 = +2.0 * (w * z + x * y)
        t4 = +1.0 - 2.0 * (y * y + z * z)
        yaw = np.arctan2(t3, t4)
        
        return roll, pitch, yaw

    def display_occupancy_map(self):
        with self.lock:
            if self.occupancy_grid is None:
                return
            
            # Dynamically determine the ceiling height threshold based on the 90th percentile
            height_threshold_upper = np.percentile(self.occupancy_grid[:, 2], 50)
            
            # Filter points based on height thresholds
            filtered_points = self.occupancy_grid[(self.occupancy_grid[:, 2] > self.height_threshold_lower) &
                                                  (self.occupancy_grid[:, 2] < height_threshold_upper)]
            
            self.ax.clear()
            self.ax.scatter(filtered_points[:, 0], filtered_points[:, 1], s=1)  # Display occupancy grid
            
            # Plot odometry position as a red dot
            self.ax.scatter(self.odometry_x, self.odometry_y, color='red')  
            
            self.ax.set_xlabel('X')
            self.ax.set_ylabel('Y')
            self.ax.set_title('Occupancy Map with Odometry Path')
            
            plt.draw()
            plt.gcf().canvas.flush_events()

node = OccupancyMapNode(resolution=0.01)

try:
    while not rospy.is_shutdown():
        node.display_occupancy_map()
        plt.pause(0.1)
except (KeyboardInterrupt, SystemExit):
    print("Shutting down")
    rospy.signal_shutdown('Exiting...')

[matlabbe]

For the odometry latency, do you mean there is more latency on python code or rtabmap's odometry?

For the odometry correctly, either you use TF to get map->odom transform to correct the received odometry, or subscribe to /rtabmap/mapGraph and read mapToOdom transform to correct the odometry afterwards.