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

Publish static transforms when intra porocess communication is enabled #2381

Merged

Conversation

Yadunund
Copy link
Contributor

@Yadunund Yadunund commented Jun 17, 2022

Hi @MartyG-RealSense ,

In the existing implementation of the ROS 2 driver, if users enable intra_process_communication, the driver will not publish static tf transforms due to this condition

However there is really no need to sacrifice static tf publishing at the expense on enabling intra_process_communication as rclcpp is smart enough to selectively disable the intra process publishing mechanism for publishers (or subscribers) by suitably overwriting the PublisherOptions for a given publisher.

This PR enables users to benefit from both intra_process_comminication as well as static tf transforms published by this driver. It short, it overcomes the static_tf limitation described in the README here 😄

It has been tested on foxy and galactic on Ubuntu 20.04 with a D455 camera.

Once you rebuild the package, and load the driver component into a component container with use_intra_process_comms:=True, you should still be able to ros2 topic echo /tf_static --qos-reliability reliable --qos-durability transient_local to see the static transforms.

Signed-off-by: Yadunund [email protected]

@MartyG-RealSense
Copy link
Collaborator

Thanks so much @Yadunund for your contribution to the ROS2 wrapper! :)

@Yadunund
Copy link
Contributor Author

@MartyG-RealSense I'm unfamiliar with the review process for this repo. Do I need to assign a reviewer?

@MartyG-RealSense
Copy link
Collaborator

Hi @Yadunund No, you don't need to assign a reviewer as the RealSense development team does that. Thanks very much for your patience!

@Yadunund
Copy link
Contributor Author

Gotcha, thank for the heads up 😄

@christian-rauch
Copy link
Contributor

This PR fixes #2596 for me. @Yadunund Can you rebase this PR?

@christian-rauch
Copy link
Contributor

Could you split the functional and the white space changes? This would make it easier to backport/cherry-pick the actual fix into other branches.

@Nir-Az Nir-Az self-requested a review January 13, 2023 17:24
@Nir-Az
Copy link
Collaborator

Nir-Az commented Jan 13, 2023

Thats a great solution!
We will review it.
Can you please undo unnessesary whitespaces and rebase for no conflicts?
Thanks!

@Yadunund
Copy link
Contributor Author

Sure! Will get to doing so in a bit.

@Yadunund Yadunund force-pushed the publish_tf_with_intra_process branch from b81ce1a to 6fd6b5f Compare January 15, 2023 15:05
@Yadunund
Copy link
Contributor Author

@christian-rauch @Nir-Az hope the diff is suitable now!

@Yadunund
Copy link
Contributor Author

Sorry for an extra commit after CI started. I tried to save one extra copy from happening 😅

@Nir-Az
Copy link
Collaborator

Nir-Az commented Jan 15, 2023

Sorry for an extra commit after CI started. I tried to save one extra copy from happening 😅

NP, did you manage to verify that the intra process functionality still works for images using our latency tool?

@christian-rauch
Copy link
Contributor

I think "Rebase" is not a good commit message. Can you restore the original message or write a new one that reflects the actual change, e.g. "publish static transformations for intra-process communication" or something along that line?

@christian-rauch
Copy link
Contributor

I would also suggest squashing those two commits together as they are changing the same line twice.

@Nir-Az
Copy link
Collaborator

Nir-Az commented Jan 15, 2023

According to the CI results The API was changed since dashing, we'll have to see what do we do about it.

@Yadunund
Copy link
Contributor Author

Apart from foxy and humble, other ROS 2 distros are End of Life. Do we still want CI to pass for those distros?

If so I think I can get dashing CI to pass with some #ifdefine flags in the code. But I'm not sure why galactic CI is failing as seen below.

  Found the following packages:
    - realsense2_camera
    - realsense2_camera_msgs
    - realsense2_description
  Package maintainer emails: [email protected]
  Traceback (most recent call last):
    File "/tmp/ros_buildfarm/ros_buildfarm/scripts/devel/create_devel_task_generator.py", line 287, in resolve_names
      resolved_names = resolve_for_os(
    File "/usr/lib/python3/dist-packages/rosdep2/catkin_support.py", line 87, in resolve_for_os
      d = view.lookup(rosdep_key)
    File "/usr/lib/python3/dist-packages/rosdep2/lookup.py", line 206, in lookup
      return self.rosdep_defs[rosdep_name]
  KeyError: 'ros_workspace'
  
  During handling of the above exception, another exception occurred:
  
  Traceback (most recent call last):
    File "/tmp/ros_buildfarm/scripts/devel/create_devel_task_generator.py", line 21, in <module>
      run_module('ros_buildfarm.scripts.devel.create_devel_task_generator', run_name='__main__')
    File "/usr/lib/python3.8/runpy.py", line 210, in run_module
      return _run_code(code, {}, init_globals, run_name, mod_spec)
    File "/usr/lib/python3.8/runpy.py", line 87, in _run_code
      exec(code, run_globals)
    File "/tmp/ros_buildfarm/ros_buildfarm/scripts/devel/create_devel_task_generator.py", line 304, in <module>
      sys.exit(main())
    File "/tmp/ros_buildfarm/ros_buildfarm/scripts/devel/create_devel_task_generator.py", line 126, in main
      debian_pkg_names += resolve_names(['ros_workspace'], **context)
    File "/tmp/ros_buildfarm/ros_buildfarm/scripts/devel/create_devel_task_generator.py", line 290, in resolve_names
      raise RuntimeError(
  RuntimeError: Could not resolve the rosdep key 'ros_workspace'

@Nir-Az
Copy link
Collaborator

Nir-Az commented Jan 16, 2023

Apart from foxy and humble, other ROS 2 distros are End of Life. Do we still want CI to pass for those distros?

If so I think I can get dashing CI to pass with some #ifdefine flags in the code. But I'm not sure why galactic CI is failing as seen below.

  Found the following packages:
    - realsense2_camera
    - realsense2_camera_msgs
    - realsense2_description
  Package maintainer emails: [email protected]
  Traceback (most recent call last):
    File "/tmp/ros_buildfarm/ros_buildfarm/scripts/devel/create_devel_task_generator.py", line 287, in resolve_names
      resolved_names = resolve_for_os(
    File "/usr/lib/python3/dist-packages/rosdep2/catkin_support.py", line 87, in resolve_for_os
      d = view.lookup(rosdep_key)
    File "/usr/lib/python3/dist-packages/rosdep2/lookup.py", line 206, in lookup
      return self.rosdep_defs[rosdep_name]
  KeyError: 'ros_workspace'
  
  During handling of the above exception, another exception occurred:
  
  Traceback (most recent call last):
    File "/tmp/ros_buildfarm/scripts/devel/create_devel_task_generator.py", line 21, in <module>
      run_module('ros_buildfarm.scripts.devel.create_devel_task_generator', run_name='__main__')
    File "/usr/lib/python3.8/runpy.py", line 210, in run_module
      return _run_code(code, {}, init_globals, run_name, mod_spec)
    File "/usr/lib/python3.8/runpy.py", line 87, in _run_code
      exec(code, run_globals)
    File "/tmp/ros_buildfarm/ros_buildfarm/scripts/devel/create_devel_task_generator.py", line 304, in <module>
      sys.exit(main())
    File "/tmp/ros_buildfarm/ros_buildfarm/scripts/devel/create_devel_task_generator.py", line 126, in main
      debian_pkg_names += resolve_names(['ros_workspace'], **context)
    File "/tmp/ros_buildfarm/ros_buildfarm/scripts/devel/create_devel_task_generator.py", line 290, in resolve_names
      raise RuntimeError(
  RuntimeError: Could not resolve the rosdep key 'ros_workspace'

Thanks,

galactic is EOL too, I will verify if we can skip dashing and update soon..

@Nir-Az
Copy link
Collaborator

Nir-Az commented Jan 16, 2023

Apart from foxy and humble, other ROS 2 distros are End of Life. Do we still want CI to pass for those distros?

If so I think I can get dashing CI to pass with some #ifdefine flags in the code. But I'm not sure why galactic CI is failing as seen below.

  Found the following packages:
    - realsense2_camera
    - realsense2_camera_msgs
    - realsense2_description
  Package maintainer emails: [email protected]
  Traceback (most recent call last):
    File "/tmp/ros_buildfarm/ros_buildfarm/scripts/devel/create_devel_task_generator.py", line 287, in resolve_names
      resolved_names = resolve_for_os(
    File "/usr/lib/python3/dist-packages/rosdep2/catkin_support.py", line 87, in resolve_for_os
      d = view.lookup(rosdep_key)
    File "/usr/lib/python3/dist-packages/rosdep2/lookup.py", line 206, in lookup
      return self.rosdep_defs[rosdep_name]
  KeyError: 'ros_workspace'
  
  During handling of the above exception, another exception occurred:
  
  Traceback (most recent call last):
    File "/tmp/ros_buildfarm/scripts/devel/create_devel_task_generator.py", line 21, in <module>
      run_module('ros_buildfarm.scripts.devel.create_devel_task_generator', run_name='__main__')
    File "/usr/lib/python3.8/runpy.py", line 210, in run_module
      return _run_code(code, {}, init_globals, run_name, mod_spec)
    File "/usr/lib/python3.8/runpy.py", line 87, in _run_code
      exec(code, run_globals)
    File "/tmp/ros_buildfarm/ros_buildfarm/scripts/devel/create_devel_task_generator.py", line 304, in <module>
      sys.exit(main())
    File "/tmp/ros_buildfarm/ros_buildfarm/scripts/devel/create_devel_task_generator.py", line 126, in main
      debian_pkg_names += resolve_names(['ros_workspace'], **context)
    File "/tmp/ros_buildfarm/ros_buildfarm/scripts/devel/create_devel_task_generator.py", line 290, in resolve_names
      raise RuntimeError(
  RuntimeError: Could not resolve the rosdep key 'ros_workspace'

Since librealsense SDK supports Ubuntu 18, we will to allow dashing users to use it.
Can you help with adding the necessary #ifdefs to make it pass dashing build as well?

Appreciate it.

@Yadunund Yadunund force-pushed the publish_tf_with_intra_process branch from 4d3236b to 22c91d5 Compare January 17, 2023 08:27
@Yadunund Yadunund force-pushed the publish_tf_with_intra_process branch from 22c91d5 to e1a65bb Compare January 17, 2023 08:45
@Yadunund
Copy link
Contributor Author

I've updated the PR to try to accommodate dashing too. I don't have access to an 18.04 + dashing setup so I have not tried it out. I tried to match the default arg for QoS as seen in the dashing API

I've also squashed the commits to just one as requested.

Lastly verified intra-process by running the latency tool

[INFO] [launch]: All log files can be found below /home/yadu/.ros/log/2023-01-17-16-47-52-616906-yadu-155383
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [component_container-1]: process started with pid [155395]
[component_container-1] [INFO] [1673945273.197537121] [my_container]: Load Library: /home/yadu/ws_realsense/install/realsense2_camera/lib/librealsense2_camera.so
[component_container-1] [INFO] [1673945273.237799236] [my_container]: Found class: rclcpp_components::NodeFactoryTemplate<realsense2_camera::RealSenseNodeFactory>
[component_container-1] [INFO] [1673945273.237851133] [my_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<realsense2_camera::RealSenseNodeFactory>
[component_container-1] [INFO] [1673945273.243964132] [camera]: RealSense ROS v4.51.1
[component_container-1] [INFO] [1673945273.243981174] [camera]: Built with LibRealSense v2.51.1
[component_container-1] [INFO] [1673945273.243988968] [camera]: Running with LibRealSense v2.51.1
[INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/camera' in container '/my_container'
[component_container-1] [INFO] [1673945273.245593949] [my_container]: Found class: rclcpp_components::NodeFactoryTemplate<realsense2_camera::RealSenseNodeFactory>
[component_container-1] [INFO] [1673945273.245625587] [my_container]: Found class: rclcpp_components::NodeFactoryTemplate<rs2_ros::tools::frame_latency::FrameLatencyNode>
[component_container-1] [INFO] [1673945273.245640625] [my_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<rs2_ros::tools::frame_latency::FrameLatencyNode>
[component_container-1] [INFO] [1673945273.247090928] [frame_latency]: frame_latency node is UP!
[component_container-1] [INFO] [1673945273.247110113] [frame_latency]: Intra-Process is ON
[INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/frame_latency' in container '/my_container'
[component_container-1] [INFO] [1673945273.304440198] [camera]: Device with serial number 049122252220 was found.
[component_container-1] 
[component_container-1] [INFO] [1673945273.304481415] [camera]: Device with physical ID /sys/devices/pci0000:00/0000:00:08.1/0000:0c:00.3/usb6/6-4/6-4:1.0/video4linux/video2 was found.
[component_container-1] [INFO] [1673945273.304490071] [camera]: Device with name Intel RealSense D455 was found.
[component_container-1] [INFO] [1673945273.304667091] [camera]: Device with port number 6-4 was found.
[component_container-1] [INFO] [1673945273.304676979] [camera]: Device USB type: 3.2
[component_container-1] [INFO] [1673945273.304998508] [camera]: Intra-Process communication enabled
[component_container-1] [INFO] [1673945273.305336568] [camera]: getParameters...
[component_container-1] [INFO] [1673945273.305586955] [camera]: JSON file is not provided
[component_container-1] [INFO] [1673945273.305600630] [camera]: Device Name: Intel RealSense D455
[component_container-1] [INFO] [1673945273.305608816] [camera]: Device Serial No: 049122252220
[component_container-1] [INFO] [1673945273.305617402] [camera]: Device physical port: /sys/devices/pci0000:00/0000:00:08.1/0000:0c:00.3/usb6/6-4/6-4:1.0/video4linux/video2
[component_container-1] [INFO] [1673945273.305625847] [camera]: Device FW version: 05.13.00.50
[component_container-1] [INFO] [1673945273.305633932] [camera]: Device Product ID: 0x0B5C
[component_container-1] [INFO] [1673945273.305641466] [camera]: Sync Mode: Off
[component_container-1] [WARN] [1673945273.526683761] [camera]: Could not set param: rgb_camera.power_line_frequency with 3 Range: [0, 2]: parameter 'rgb_camera.power_line_frequency' could not be set: Parameter {rgb_camera.power_line_frequency} doesn't comply with integer range.
[component_container-1]  17/01 16:47:53,527 WARNING [139645325579840] (backend-v4l2.cpp:1444) Pixel format 36315752-1a66-a242-9065-d01814a likely requires patch for fourcc code RW16!
[component_container-1] [INFO] [1673945273.534205285] [camera]: Set ROS param rgb_camera.profile to default: 1280x720x30
[component_container-1] [INFO] [1673945273.543010209] [camera]: Stopping Sensor: RGB Camera
[component_container-1] [INFO] [1673945273.544342211] [camera]: Starting Sensor: RGB Camera
[component_container-1] [INFO] [1673945273.547867730] [camera]: Open profile: stream_type: Color(0), Format: RGB8, Width: 1280, Height: 720, FPS: 30
[component_container-1] [INFO] [1673945273.548168089] [camera]: RealSense Node Is Up!
[component_container-1] [WARN] [1673945274.032625098] [camera]: 
[component_container-1] [INFO] [1673945274.033651189] [frame_latency]: Got msg with address 0x7f01b048cd00 with latency of 0.0209948 [sec]
[component_container-1] [INFO] [1673945274.066154439] [frame_latency]: Got msg with address 0x7f01b064c7b0 with latency of 0.0190945 [sec]
[component_container-1] [INFO] [1673945274.098482833] [frame_latency]: Got msg with address 0x7f01b064c7b0 with latency of 0.0169352 [sec]
[component_container-1] [INFO] [1673945274.131825286] [frame_latency]: Got msg with address 0x7f01b064c7b0 with latency of 0.0157032 [sec]
[component_container-1] [INFO] [1673945274.164935377] [frame_latency]: Got msg with address 0x7f01b064c7b0 with latency of 0.0139848 [sec]
[component_container-1] [INFO] [1673945274.198673918] [frame_latency]: Got msg with address 0x7f01b064c7b0 with latency of 0.0127829 [sec]
[component_container-1] [INFO] [1673945274.231848208] [frame_latency]: Got msg with address 0x7f01b064c7b0 with latency of 0.0109026 [sec]
[component_container-1] [INFO] [1673945274.264975650] [frame_latency]: Got msg with address 0x7f01b064c7b0 with latency of 0.00885753 [sec]
[component_container-1] [INFO] [1673945274.298571706] [frame_latency]: Got msg with address 0x7f01b064c7b0 with latency of 0.00716895 [sec]
[component_container-1] [INFO] [1673945274.331903591] [frame_latency]: Got msg with address 0x7f01b064c7b0 with latency of 0.00523606 [sec]
[component_container-1] [INFO] [1673945274.365061380] [frame_latency]: Got msg with address 0x7f01b064c7b0 with latency of 0.00303051 [sec]
[component_container-1] [INFO] [1673945274.398429631] [frame_latency]: Got msg with address 0x7f01b064c7b0 with latency of 0.000933915 [sec]

@Nir-Az
Copy link
Collaborator

Nir-Az commented Jan 17, 2023

Great! thanks a lot!

Copy link
Collaborator

@Nir-Az Nir-Az left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Approved, very appreciated

@Nir-Az Nir-Az merged commit 85e3d71 into IntelRealSense:ros2-development Jan 17, 2023
@Yadunund Yadunund deleted the publish_tf_with_intra_process branch January 17, 2023 10:50
@Yadunund
Copy link
Contributor Author

Thanks for the review!

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

Successfully merging this pull request may close these issues.

4 participants