Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Incorrect visualization of pointcloud published on a moving frame #1666

Closed
f-fl0 opened this issue Sep 15, 2021 · 11 comments
Closed

Incorrect visualization of pointcloud published on a moving frame #1666

f-fl0 opened this issue Sep 15, 2021 · 11 comments

Comments

@f-fl0
Copy link

f-fl0 commented Sep 15, 2021

The transformation from a moving frame to a fixed frame is incorrect when visualizing PointCloud2 messages with the latest version of rviz (1.14.9). Using decay time with a large duration e.g. 10 seconds exhibits the problem pretty clearly (see the videos below). It seems to be related with the changes introduced in #1655. Note that LaserScan visualization appears to be correct.

I prepared a Python script to reproduce the situation and a configuration for rviz. I also recorded two videos showing the same scene in rviz using version 1.14.9 and a modified version with a suggested fix. In this scene, a robot with a 2D lidar mounted on a rotated platform, is scanning a room in a shape of a cylinder. The /scan topic is displayed in red and the /cloud topic in blue.

  • Using 1.14.9
1.14.9.mp4

The data in /scan is accumulated and displayed as expected where as the /cloud data does not.

1.14.9-modified.mp4

Both data from /scan and /cloud are accumulated and displayed as expected.

Although this modified version fixes the reported issue, it seems to break the feature introduced in the mentioned PR. The pointcloud is no longer smoothly displayed when running https://github.com/ros-visualization/rviz/blob/1.14.9/src/test/point_cloud_test.py.

  • Python script:
#!/usr/bin/env python

import geometry_msgs.msg
import laser_geometry.laser_geometry as lg
import numpy as np
import rospy
import tf2_ros
import tf_conversions
from sensor_msgs.msg import LaserScan, PointCloud2


def main():
    static_broadcaster = tf2_ros.StaticTransformBroadcaster()
    broadcaster = tf2_ros.TransformBroadcaster()

    map_link = geometry_msgs.msg.TransformStamped()
    map_link.header.stamp = rospy.Time.now()
    map_link.header.frame_id = "map"
    map_link.transform.rotation.w = 1

    base_link = geometry_msgs.msg.TransformStamped()
    base_link.header.stamp = rospy.Time.now()
    base_link.header.frame_id = "map"
    base_link.child_frame_id = "base_link"
    base_link.transform.translation.x = 2.5
    base_link.transform.translation.x = -0.2
    base_link.transform.rotation.w = 1

    lidar_link = geometry_msgs.msg.TransformStamped()
    lidar_link.header.frame_id = "sensors_link"
    lidar_link.child_frame_id = "lidar_link"
    lidar_link.transform.translation.x = 0.5
    q = tf_conversions.transformations.quaternion_from_euler(1.57, 0, 0)
    lidar_link.transform.rotation.x = q[0]
    lidar_link.transform.rotation.y = q[1]
    lidar_link.transform.rotation.z = q[2]
    lidar_link.transform.rotation.w = q[3]
    lidar_link.header.stamp = rospy.Time.now()
    static_broadcaster.sendTransform([map_link, base_link, lidar_link])

    sensors_link = geometry_msgs.msg.TransformStamped()
    sensors_link.header.stamp = rospy.Time.now()
    sensors_link.header.frame_id = "base_link"
    sensors_link.child_frame_id = "sensors_link"
    sensors_link.transform.translation.z = 1.3
    sensors_link.transform.rotation.w = 1
    broadcaster.sendTransform(sensors_link)

    radius = 5
    scan = LaserScan()
    scan.header.frame_id = "lidar_link"
    scan.angle_max = np.radians(10.0)
    scan.angle_min = np.radians(-10.0)
    scan.angle_increment = np.radians(0.25)
    scan.range_min = 0
    scan.range_max = radius * 2
    scan.ranges = radius / np.cos(
        np.linspace(scan.angle_min, scan.angle_max, int((scan.angle_max - scan.angle_min) / scan.angle_increment))
    )
    scan.intensities = [0] * len(scan.ranges)

    pub_scan = rospy.Publisher("scan", LaserScan, queue_size=1)
    lp = lg.LaserProjection()
    pub_cloud = rospy.Publisher("cloud", PointCloud2, queue_size=1)

    rate = rospy.Rate(120)
    count = 0
    angle = 0
    while not rospy.is_shutdown():
        q = tf_conversions.transformations.quaternion_from_euler(0, 0, angle)
        sensors_link.transform.rotation.x = q[0]
        sensors_link.transform.rotation.y = q[1]
        sensors_link.transform.rotation.z = q[2]
        sensors_link.transform.rotation.w = q[3]
        sensors_link.header.stamp = rospy.Time.now()
        broadcaster.sendTransform(sensors_link)
        angle += np.radians(5.0) + np.random.uniform(0.001, 0.1)
        if count % 3 == 0:
            scan.header.stamp = rospy.Time.now()
            pub_scan.publish(scan)
            pc2_msg = lp.projectLaser(scan)
            pub_cloud.publish(pc2_msg)
        count += 1
        rate.sleep()


if __name__ == "__main__":
    rospy.init_node("dummy_rotating_lidar")
    try:
        main()
    except rospy.ROSInterruptException:
        pass
  • rviz configuration (renamed to config.txt to upload it here)
    config.txt

Environment

  • OS Version: Ubuntu 20.04
  • ROS Distro: Noetic
  • RViz version 1.14.9 (noetic).
    Compiled against Qt version 5.12.8.
    Compiled against OGRE version 1.9.0 (Ghadamon).
@Martin-Oehler
Copy link

The mentioned commit transforms old data with new transforms, which should obviously be wrong. I don't quite understand the intention behind this commit. This is a major regression and should be reverted as soon as possible.

@StefanFabian
Copy link
Contributor

Third this. Here's also a better illustration of the erroneous behavior:

Currently:

rviz_bug.mp4

Expected: (with the fix proposed by @f-fl0)

rviz_expected.mp4

@f-fl0
Copy link
Author

f-fl0 commented Sep 15, 2021

@Martin-Oehler

I can open a PR with the fix I suggested or one that reverts the changes from https://github.com/ros-visualization/rviz/pull/1655/files. Let me know which one you believe is best.

@rhaschke
Copy link
Contributor

@UniBwTAS, could you please comment?

@ClementLeBihan
Copy link

Same problem with rviz 1.13.19 on 18.04. Tested with 1.13.20 and it's fixed !
How much time will it take to be available with apt ?

Best,
Clément

@rhaschke
Copy link
Contributor

rhaschke commented Oct 4, 2021

We just have seen a release sync a week ago: https://discourse.ros.org/c/release
Meanwhile you might want to use the package from ros-testing?

@ClementLeBihan
Copy link

Yes ros-melodic-rviz version 1.13.19-1bionic.20210921.222942 has been pushed 27/09/2021, and I was wondering how long will it take for the next release ?
As we are using dockerfile in prod with apt install ros-melodic-rviz inside, we are looking forward to a new release :)

I don't know anything about ros-testing, what's this ?

Best,
Clément

@rhaschke
Copy link
Contributor

rhaschke commented Oct 4, 2021

See here: http://wiki.ros.org/TestingRepository

@ros-discourse
Copy link

This issue has been mentioned on ROS Discourse. There might be relevant details there:

https://discourse.ros.org/t/preparing-for-noetic-sync-2021-09-23/22378/4

@danielrotaermel
Copy link

Had the same issue with rviz also running inside a docker container.
Here is a little script to get the latest development version from the testing repository.

#!/bin/bash
sudo sed -i 's/\/ros\//\/ros-testing\//g' /etc/apt/sources.list.d/ros-latest.list
sudo apt-get update
sudo apt-get install ros-melodic-rviz

@rhaschke
Copy link
Contributor

Fix released.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

7 participants