0
$\begingroup$

I'm using ROS2 Humble on Ubuntu 22.04. In my project I'm trying to visualize in rviz2 a pointcloud generated by a depth camera, mounted on a drone simulated in gazebo garden. Thanks to this topic I've been able to visualize the pointcloud in rviz2, setting the mobile frame of the pointcloud as fixed frame, but, since I have a non trivial TF hierarchy, the resulting orientation of the points is wrong.TF graph by rqt

The reason why the TF hierarchy has this shape is that the drone pose and attitude is updated republishing the autopilot (PX4) odometry, which consist in the transformation between a NED inertial frame and a FRD mobile frame. Since I'm interested in using an ENU fixed frame and FLU mobile frames, I published other two static TF to properly convert the notation. Finally, I added the last static transformation from the drone base_link to the depth_camera_frame, since the camera is pointing downwards.

In my perspective rviz2 should be able to convert the points coming from the depth_camera_frame according to the TF hierarchy and display them in the map fixed frame. Unfortunately this is not happening. Does anyone have any clue about what I'm missing?

Thank you so much

EDIT:

As Lucas suggested, I set as fixed frame all the available TFs. Doing so I realized that the TF which "brokens" the chain from the camera frame to the inertial frame is the time varying one. In other words, the frames base_link and base_link_frd, which are connected to depth_camera_frame by means of static TF only, are showing in some ways the points, while map_ned and map are just returning the message in rviz "showing [0] points from [0] messages". I would say that problem like this could arise from the incorrect temporization of the pointcloud packets and the TF updates, but this is just an impression. Anyway I upload at this link a bag file in mcap format and below the source code of the odometry republisher code.

Again, thank you so much for the help!

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
from geometry_msgs.msg import Quaternion
from sensor_msgs.msg import JointState
from tf2_ros import TransformBroadcaster, TransformStamped
from px4_msgs.msg import VehicleOdometry
import _thread

class OdometryStatePublisher(Node):
    """Node that converts the topic published by PX4 into topics listened by rviz """
    
    def __init__(self):
        # Configure node
        super().__init__('odometry_state_publisher')  # set node name
        self.nodeName = self.get_name()
        qos_subscriber = QoSProfile(
            reliability=QoSReliabilityPolicy.BEST_EFFORT,
            history=QoSHistoryPolicy.KEEP_LAST,
            depth=10)
        qos_publisher = QoSProfile(
            reliability=QoSReliabilityPolicy.RELIABLE,
            history=QoSHistoryPolicy.KEEP_LAST,
            depth=10)
        # Configure subscription to PX4 odometry
        self.odometryPosition=[0, 0, 0]    # set initial odometry
        self.odometryQuaternion=[1, 0, 0, 0] # q0,qx,qy,qz
        self.px4OdometrySubscription = self.create_subscription(msg_type=VehicleOdometry,
                                                                topic='/fmu/out/vehicle_odometry',
                                                                callback=self.odometry_updater,
                                                                qos_profile=qos_subscriber)
        # Configure transfer function publisher
        self.tfBroadcaster = TransformBroadcaster(self, qos=qos_publisher)
        # Configure publishing thread
        self.endOfProgram=False
        self.publishing_rate = self.create_rate(10) #Hz
        # Start publishing thread
        try:
            _thread.start_new_thread(self.topicPublisher, ())
        except:
            print("Error in thread creation!!!")
        # Log that the node is started
        self.get_logger().info("{0} started".format(self.nodeName))


        
    # Callback executed by the odometry subscriber    
    def odometry_updater(self, msg):
        self.odometryPosition=msg.position
        self.odometryQuaternion=msg.q
    

        
    # Function executed in the publishing thread 
    def topicPublisher(self):
        odometryTF = TransformStamped()
        odometryTF.header.frame_id = 'map_ned'
        odometryTF.child_frame_id = 'base_link_frd'
        # Loop
        while(not(self.endOfProgram)):
            # Create current TF and publish it
            now = self.get_clock().now()
            odometryTF.header.stamp = now.to_msg()
            odometryTF.transform.translation.x = float(self.odometryPosition[0])
            odometryTF.transform.translation.y = float(self.odometryPosition[1])
            odometryTF.transform.translation.z = float(self.odometryPosition[2])
            odometryTF.transform.rotation = Quaternion(x=float(self.odometryQuaternion[1]),
                                                       y=float(self.odometryQuaternion[2]),
                                                       z=float(self.odometryQuaternion[3]),
                                                       w=float(self.odometryQuaternion[0]))
            self.tfBroadcaster.sendTransform(odometryTF)
            # Print logs
            #self.get_logger().info("p: " + str(self.odometryPosition) + "; q: " + str(self.odometryQuaternion))
            # Sleep until next loop
            self.publishing_rate.sleep()
        

    # Main thread    
    def main(self):
        try:
            rclpy.spin(self)
        except KeyboardInterrupt:  # this happens after Ctrl+C
            pass
        self.endOfProgram=True
        print("\nShutdown now!")
        
        
        
############################################################################
# Workaround to avoid configuring entry points in a package without setup.py        
def odometryStatePublisher_main(args=None):
    rclpy.init(args=args)  # initialize ROS communication
    node = OdometryStatePublisher()
    node.main()

if __name__ == '__main__':
    odometryStatePublisher_main()

SOLUTION: Thanks to Lucas Walter and Mike973 I realized that I forgot to properly configure the nodes in the launch file, setting the parameter use_sim_time to True.

Thank you so much guys for the help!

$\endgroup$
4
  • $\begingroup$ What happens if you start with the rviz fixed frame on depth_camera_frame, then base_link, then base_link_frd, and so on- at what point does the point cloud disappear? Also can you share a short mcap bag of the point cloud and tf topics? $\endgroup$ Commented Dec 14, 2023 at 14:47
  • 1
    $\begingroup$ Thank you so much @LucasWalter for the suggestion, I've updated the question with the bag file and my rviz configuration. $\endgroup$ Commented Dec 16, 2023 at 10:33
  • $\begingroup$ Have you correctly set use_sim_time parameter for all nodes, including rviz2 ? See answers.ros.org/question/415092/… $\endgroup$
    – Mike973
    Commented Dec 16, 2023 at 14:27
  • $\begingroup$ Thank you @Mike973 that was the issue! $\endgroup$ Commented Dec 16, 2023 at 20:28

1 Answer 1

0
$\begingroup$

Thanks to Lucas Walter and Mike973 I realized that I forgot to properly configure the nodes in the launch file, setting the parameter use_sim_time to True.

Thank you so much guys for the help!

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.