Ask Your Question

Octomap_server + RealSense D435 + RealSense T265

asked 2019-10-21 03:21:15 -0600

eMrazSVK gravatar image

Hello everyone,

I would like to create simple octomap with use of both RealSense D435 and T265 cameras. I am using my own RealSense wrapper (do not ask why please, it is needed in this case). I can publish PointCloud from D435 camera (depth stream) and pose (odometry) from T265 as a topic.

What I am currently struggling with is publishing tf. I know, that octomap_server can incrementally build occupancy grid based on odometry received from tf. I just cannot figure out, how to setup frame_id's and so.

This is how I broadcast transform:

tf_br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "tf_t265", "map"));

The transform object is filled with the exact same data as I am using in the topic for publishing T265 pose (which is being used and it works).

This is what the part of my launchfile for octomap_server looks like:

<node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
        <param name="resolution" value="0.05" />

        <!-- fixed map frame (set to 'map' if SLAM or localization running!) -->
        <param name="frame_id" type="string" value="tf_t265" />

        <!-- maximum range to integrate (speedup!) -->
        <param name="sensor_model/max_range" value="5.0" />

        <!-- data source to integrate (PointCloud2) -->
        <remap from="cloud_in" to="/d435/depth" />      

I suspect, that frame_id param is the key here, but I tried to set it to map and it does not work too. I know, that maybe I am publishing the transform incorrectly, but this is why I am asking for help. There is nothing else in my code related with tf broadcasting.

If any additional info is needed, please let me know. Thank you for any help!

Have a nice day.

edit retag flag offensive close merge delete


I know you said don't ask but I'm genuinely curious ... (and I assume it has more to do with the t265 than the d435) ... why did you write your own RealSense wrapper :D ?

Jari gravatar image Jari  ( 2019-10-21 11:56:30 -0600 )edit

Because I need only basic functionality from librealsense inside ROS. Furthermore I am developing application on Nvidia Jetson and D435+T265 simultaneously don't even work with RealSense wrapper. I am also maintaining a big ros package, where I have my own realsense node. If I was using realsense-ros as a module it would be difficult to maintain, because it is under heavy development and so it is updated too often. When I update core librealsense, my ROS wrapper still works. The realsense ROS wrapper is not hard to write though. + I needed to implement some features which weren't implemented (I dont know if it's still the case) in original RealSense ROS wrapper such as T265 complete camera reset (start/stop pipeline) or odometry Set (setting odometry to specific pose with service using transformation). It is much more flexible now for me as I have stable setup consisting ...(more)

eMrazSVK gravatar image eMrazSVK  ( 2019-10-22 02:51:19 -0600 )edit

That makes a lot of sense. Is any is this open source perchance? I'd like to look through it if that's the case.

Jari gravatar image Jari  ( 2019-10-22 08:59:15 -0600 )edit

No, it's not available anywhere for now.

eMrazSVK gravatar image eMrazSVK  ( 2019-10-22 09:02:34 -0600 )edit

Hi @eMrazSVK I am trying to perform the same as you in octomap with T265 and D435i. Currently(contrary to you I am using the realsenselib) The pose and depth point cloud are working as expected( light modifications to the rs_d400_and_t265.launch). When I run the octomap-server, after a few secs, I get MessageFilter [target=/t265/odom/sample ]: Dropped 100.00% of messages so far. Any idea what that might be? Any help would be much appreciated.

Dookei gravatar image Dookei  ( 2020-04-08 03:14:55 -0600 )edit

2 Answers

Sort by ยป oldest newest most voted

answered 2019-10-22 03:12:29 -0600

eMrazSVK gravatar image

I'm answering my own question. The solution was to add transform between pointcloud_frame_id and map frame (this fact is clearly stated in the octomap_server docs so it seems I cannot read). It is still strangely rotated, but that would be again some wrong transform.

edit flag offensive delete link more

answered 2019-10-21 11:55:07 -0600

Jari gravatar image

As mentioned here frame_id is your global frame (in this case map) and your sensor frame is another parameter called base_frame_id. Try setting that to tf_t265.

edit flag offensive delete link more


Thanks, maybe that's the case too, but what was missing was this: PointCloud has also its own frame and transform from pointcloud_frame -> map was not known to tf so I added that. Thanks for help anyways.

eMrazSVK gravatar image eMrazSVK  ( 2019-10-22 02:54:27 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower


Asked: 2019-10-21 03:21:15 -0600

Seen: 919 times

Last updated: Oct 22 '19