From e05a75b0c6d37d595eb9dd91fafa28b5386b63be Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Thu, 10 Aug 2023 01:14:17 +0000 Subject: [PATCH] remove unsupported models --- .travis.yml | 71 ------ README.md | 15 +- realsense2_camera/CMakeLists.txt | 18 +- .../include/base_realsense_node.h | 4 - realsense2_camera/include/constants.h | 26 +-- realsense2_camera/include/image_publisher.h | 4 - realsense2_camera/include/ros_utils.h | 4 - realsense2_camera/launch/rs_launch.py | 46 ++-- realsense2_camera/package.xml | 2 +- realsense2_camera/scripts/rs2_listener.py | 14 +- realsense2_camera/scripts/rs2_test.py | 38 ++-- realsense2_camera/src/base_realsense_node.cpp | 22 +- realsense2_camera/src/dynamic_params.cpp | 4 - realsense2_camera/src/parameters.cpp | 4 - .../src/realsense_node_factory.cpp | 5 - realsense2_camera/src/ros_utils.cpp | 8 +- realsense2_camera/src/rs_node_setup.cpp | 7 - realsense2_camera/src/tfs.cpp | 93 -------- .../test/utils/pytest_rs_utils.py | 2 +- realsense2_description/urdf/_l515.urdf.xacro | 213 ------------------ realsense2_description/urdf/_r430.urdf.xacro | 14 -- .../urdf/test_l515_camera.urdf.xacro | 12 - 22 files changed, 46 insertions(+), 580 deletions(-) delete mode 100644 .travis.yml delete mode 100644 realsense2_description/urdf/_l515.urdf.xacro delete mode 100644 realsense2_description/urdf/test_l515_camera.urdf.xacro diff --git a/.travis.yml b/.travis.yml deleted file mode 100644 index fca14f802a..0000000000 --- a/.travis.yml +++ /dev/null @@ -1,71 +0,0 @@ -sudo: required -matrix: - include: - - dist: bionic - - dist: focal - - dist: jammy - -env: - # - git clone -v --progress https://github.com/doronhi/realsense.git # This is Done automatically by TravisCI -before_install: - - if [[ $(lsb_release -sc) == "bionic" ]]; then _python=python; _ros_dist=dashing; - elif [[ $(lsb_release -sc) == "focal" ]]; then _python=python3; _ros_dist=foxy; fi - elif [[ $(lsb_release -sc) == "jammy" ]]; then _python=python3; _ros_dist=iron; fi - - echo _python:$_python - - echo _ros_dist:$_ros_dist - - - sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-key C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C8B3A55A6F3EFCDE - - sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" - - sudo apt-get update -qq - - sudo apt-get install librealsense2-dkms --allow-unauthenticated -y - - sudo apt-get install librealsense2-dev --allow-unauthenticated -y - -install: - # install ROS: - - sudo apt update && sudo apt install curl gnupg2 lsb-release -y - - curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add - - - sudo sh -c 'echo "deb [arch=$(dpkg --print-architecture)] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" > /etc/apt/sources.list.d/ros2-latest.list' - - sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 - - sudo apt update -qq - - sudo apt install ros-$_ros_dist-ros-base -y - - sudo apt install python3-colcon-common-extensions -y - - sudo apt-get install python3-rosdep -y - - #Environment setup - - echo "source /opt/ros/$_ros_dist/setup.bash" >> ~/.bashrc - - source ~/.bashrc - - # install realsense2-camera - - mkdir -p ~/ros2_ws/src/realsense-ros - - mv * ~/ros2_ws/src/realsense-ros/ # This leaves behind .git, .gitignore and .travis.yml but no matter. - - cd ~/ros2_ws - - sudo rosdep init - - rosdep update - - rosdep install -i --from-path src --rosdistro $_ros_dist -y - - sudo apt purge ros-$_ros_dist-librealsense2 -y - - colcon build - - - . install/local_setup.bash - -script: - # download data: - - bag_filename="https://librealsense.intel.com/rs-tests/TestData/outdoors_1color.bag"; - - wget $bag_filename -P "records/" - - bag_filename="https://librealsense.intel.com/rs-tests/D435i_Depth_and_IMU_Stands_still.bag"; - - wget $bag_filename -P "records/" - - # install packages for tests: - - sudo apt-get install python3-pip -y - - pip3 install numpy --upgrade - - pip3 install numpy-quaternion tqdm - # Run test: - - python3 src/realsense-ros/realsense2_camera/scripts/rs2_test.py --all - -before_cache: - - rm -f $HOME/.gradle/caches/modules-2/modules-2.lock - - rm -fr $HOME/.gradle/caches/*/plugin-resolution/ -cache: - directories: - - $HOME/.gradle/caches/ - - $HOME/.gradle/wrapper/ - - $HOME/.android/build-cache diff --git a/README.md b/README.md index ec670e6614..262600151f 100644 --- a/README.md +++ b/README.md @@ -142,8 +142,8 @@ - Install dependencies ```bash sudo apt-get install python3-rosdep -y - sudo rosdep init # "sudo rosdep init --include-eol-distros" for Eloquent and earlier - rosdep update # "sudo rosdep update --include-eol-distros" for Eloquent and earlier + sudo rosdep init # "sudo rosdep init --include-eol-distros" for Foxy and earlier + rosdep update # "sudo rosdep update --include-eol-distros" for Foxy and earlier rosdep install -i --from-path src --rosdistro $ROS_DISTRO --skip-keys=librealsense2 -y ``` @@ -200,13 +200,13 @@ - They have, at least, the **profile** parameter. - The profile parameter is a string of the following format: \X\X\ (The dividing character can be X, x or ",". Spaces are ignored.) - For example: ```depth_module.profile:=640x480x30 rgb_camera.profile:=1280x720x30``` - - Since infra, infra1, infra2, fisheye, fisheye1, fisheye2 and depth are all streams of the depth_module, their width, height and fps are defined by the same param **depth_module.profile** + - Since infra, infra1, infra2 and depth are all streams of the depth_module, their width, height and fps are defined by the same param **depth_module.profile** - If the specified combination of parameters is not available by the device, the default or previously set configuration will be used. - Run ```ros2 param describe ``` to get the list of supported profiles. - Note: Should re-enable the stream for the change to take effect. - ****_format** - This parameter is a string used to select the stream format. - - can be any of *infra, infra1, infra2, color, depth, fisheye, fisheye1, fisheye2*. + - can be any of *infra, infra1, infra2, color, depth*. - For example: ```depth_module.depth_format:=Z16 depth_module.infra1_format:=y8 rgb_camera.color_format:=RGB8``` - This parameter supports both lower case and upper case letters. - If the specified parameter is not available by the stream, the default or previously set configuration will be used. @@ -217,14 +217,14 @@ - Run ```rs-enumerate-devices``` command to know the list of profiles supported by the connected sensors. - **enable_****: - Choose whether to enable a specified stream or not. Default is true for images and false for orientation streams. - - can be any of *infra, infra1, infra2, color, depth, fisheye, fisheye1, fisheye2, gyro, accel, pose*. + - can be any of *infra, infra1, infra2, color, depth, gyro, accel*. - For example: ```enable_infra1:=true enable_color:=false``` - **enable_sync**: - gathers closest frames of different sensors, infra red, color and depth, to be sent with the same timetag. - This happens automatically when such filters as pointcloud are enabled. - ****_qos**: - Sets the QoS by which the topic is published. - - can be any of *infra, infra1, infra2, color, depth, fisheye, fisheye1, fisheye2, gyro, accel, pose*. + - can be any of *infra, infra1, infra2, color, depth, gyro, accel*. - Available values are the following strings: `SYSTEM_DEFAULT`, `DEFAULT`, `PARAMETER_EVENTS`, `SERVICES_DEFAULT`, `PARAMETERS`, `SENSOR_DATA`. - For example: ```depth_qos:=SENSOR_DATA``` - Reference: [ROS2 QoS profiles formal documentation](https://docs.ros.org/en/rolling/Concepts/About-Quality-of-Service-Settings.html#qos-profiles) @@ -270,7 +270,6 @@ - For example: `initial_reset:=true` - ****_frame_id**, ****_optical_frame_id**, **aligned_depth_to_**_frame_id**: Specify the different frame_id for the different frames. Especially important when using multiple cameras. - **base_frame_id**: defines the frame_id all static transformations refers to. -- **odom_frame_id**: defines the origin coordinate system in ROS convention (X-Forward, Y-Left, Z-Up). pose topic defines the pose relative to that system. - **unite_imu_method**: - D400 cameras have built in IMU components which produce 2 unrelated streams, each with it's own frequency: @@ -299,8 +298,6 @@ - 0 or negative values mean no diagnostics topic is published. Defaults to 0.
The `/diagnostics` topic includes information regarding the device temperatures and actual frequency of the enabled streams. -- **publish_odom_tf**: If True (default) publish TF from odom_frame to pose_frame. -

diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 4eeda43bdf..96c51f249d 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -144,23 +144,7 @@ set(SOURCES if(NOT DEFINED ENV{ROS_DISTRO}) message(FATAL_ERROR "ROS_DISTRO is not defined." ) endif() -if("$ENV{ROS_DISTRO}" STREQUAL "dashing") - message(STATUS "Build for ROS2 Dashing") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DDASHING") - set(SOURCES "${SOURCES}" src/ros_param_backend_dashing.cpp) -elseif("$ENV{ROS_DISTRO}" STREQUAL "eloquent") - message(STATUS "Build for ROS2 eloquent") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DELOQUENT") - set(SOURCES "${SOURCES}" src/ros_param_backend_foxy.cpp) -elseif("$ENV{ROS_DISTRO}" STREQUAL "foxy") - message(STATUS "Build for ROS2 Foxy") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DFOXY") - set(SOURCES "${SOURCES}" src/ros_param_backend_foxy.cpp) -elseif("$ENV{ROS_DISTRO}" STREQUAL "galactic") - message(STATUS "Build for ROS2 Galactic") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DGALACTIC") - set(SOURCES "${SOURCES}" src/ros_param_backend_foxy.cpp) -elseif("$ENV{ROS_DISTRO}" STREQUAL "humble") +if("$ENV{ROS_DISTRO}" STREQUAL "humble") message(STATUS "Build for ROS2 Humble") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DHUMBLE") set(SOURCES "${SOURCES}" src/ros_param_backend_foxy.cpp) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index b414473e31..3e0d727701 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -40,7 +40,6 @@ #include #include #include -#include #include #include @@ -249,7 +248,6 @@ namespace realsense2_camera void FillImuData_LinearInterpolation(const CimuData imu_data, std::deque& imu_msgs); void imu_callback(rs2::frame frame); void imu_callback_sync(rs2::frame frame, imu_sync_method sync_method=imu_sync_method::COPY); - void pose_callback(rs2::frame frame); void multiple_message_callback(rs2::frame frame, imu_sync_method sync_method); void frame_callback(rs2::frame frame); @@ -295,7 +293,6 @@ namespace realsense2_camera std::map> _image_publishers; std::map::SharedPtr> _imu_publishers; - std::shared_ptr> _odom_publisher; std::shared_ptr _synced_imu_publisher; std::map::SharedPtr> _info_publishers; std::map::SharedPtr> _metadata_publishers; @@ -316,7 +313,6 @@ namespace realsense2_camera bool _is_color_enabled; bool _is_depth_enabled; bool _pointcloud; - bool _publish_odom_tf; imu_sync_method _imu_sync_method; stream_index_pair _pointcloud_texture; PipelineSyncer _syncer; diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index cd53468e0f..a9321d9190 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -31,18 +31,6 @@ #define ROS_WARN(...) RCLCPP_WARN(_logger, __VA_ARGS__) #define ROS_ERROR(...) RCLCPP_ERROR(_logger, __VA_ARGS__) -#ifdef DASHING -// Based on: https://docs.ros2.org/dashing/api/rclcpp/logging_8hpp.html -#define MSG(msg) (static_cast(std::ostringstream() << msg)).str() -#define ROS_DEBUG_STREAM(msg) RCLCPP_DEBUG(_logger, MSG(msg)) -#define ROS_INFO_STREAM(msg) RCLCPP_INFO(_logger, MSG(msg)) -#define ROS_WARN_STREAM(msg) RCLCPP_WARN(_logger, MSG(msg)) -#define ROS_ERROR_STREAM(msg) RCLCPP_ERROR(_logger, MSG(msg)) -#define ROS_FATAL_STREAM(msg) RCLCPP_FATAL(_logger, MSG(msg)) -#define ROS_DEBUG_STREAM_ONCE(msg) RCLCPP_DEBUG_ONCE(_logger, MSG(msg)) -#define ROS_INFO_STREAM_ONCE(msg) RCLCPP_INFO_ONCE(_logger, MSG(msg)) -#define ROS_WARN_STREAM_COND(cond, msg) RCLCPP_WARN_EXPRESSION(_logger, cond, MSG(msg)) -#else // Based on: https://docs.ros2.org/foxy/api/rclcpp/logging_8hpp.html #define ROS_DEBUG_STREAM(msg) RCLCPP_DEBUG_STREAM(_logger, msg) #define ROS_INFO_STREAM(msg) RCLCPP_INFO_STREAM(_logger, msg) @@ -52,15 +40,12 @@ #define ROS_DEBUG_STREAM_ONCE(msg) RCLCPP_DEBUG_STREAM_ONCE(_logger, msg) #define ROS_INFO_STREAM_ONCE(msg) RCLCPP_INFO_STREAM_ONCE(_logger, msg) #define ROS_WARN_STREAM_COND(cond, msg) RCLCPP_WARN_STREAM_EXPRESSION(_logger, cond, msg) -#endif #define ROS_WARN_ONCE(msg) RCLCPP_WARN_ONCE(_logger, msg) #define ROS_WARN_COND(cond, ...) RCLCPP_WARN_EXPRESSION(_logger, cond, __VA_ARGS__) namespace realsense2_camera { - const uint16_t SR300_PID = 0x0aa5; // SR300 - const uint16_t SR300v2_PID = 0x0B48; // SR305 const uint16_t RS400_PID = 0x0ad1; // PSR const uint16_t RS410_PID = 0x0ad2; // ASR const uint16_t RS415_PID = 0x0ad3; // ASRC @@ -80,11 +65,7 @@ namespace realsense2_camera const uint16_t RS430i_PID = 0x0b4b; // D430i const uint16_t RS405_PID = 0x0B5B; // DS5U const uint16_t RS455_PID = 0x0B5C; // D455 - const uint16_t RS457_PID = 0xABCD; // D457 - const uint16_t RS_L515_PID_PRE_PRQ = 0x0B3D; // - const uint16_t RS_L515_PID = 0x0B64; // - const uint16_t RS_L535_PID = 0x0b68; - + const uint16_t RS457_PID = 0xABCD; // D457 const bool ALLOW_NO_TEXTURE_POINTS = false; const bool ORDERED_PC = false; @@ -100,15 +81,10 @@ namespace realsense2_camera const std::string HID_QOS = "SENSOR_DATA"; const bool HOLD_BACK_IMU_FOR_FRAMES = false; - const bool PUBLISH_ODOM_TF = true; const std::string DEFAULT_BASE_FRAME_ID = "link"; - const std::string DEFAULT_ODOM_FRAME_ID = "odom_frame"; const std::string DEFAULT_IMU_OPTICAL_FRAME_ID = "camera_imu_optical_frame"; - const std::string DEFAULT_UNITE_IMU_METHOD = ""; - const std::string DEFAULT_FILTERS = ""; - const float ROS_DEPTH_SCALE = 0.001; static const rmw_qos_profile_t rmw_qos_profile_latched = diff --git a/realsense2_camera/include/image_publisher.h b/realsense2_camera/include/image_publisher.h index 6bc0bab8e6..3d7d004c74 100644 --- a/realsense2_camera/include/image_publisher.h +++ b/realsense2_camera/include/image_publisher.h @@ -17,11 +17,7 @@ #include #include -#if defined( DASHING ) || defined( ELOQUENT ) -#include -#else #include -#endif namespace realsense2_camera { class image_publisher diff --git a/realsense2_camera/include/ros_utils.h b/realsense2_camera/include/ros_utils.h index 4df1def396..feccd4647d 100644 --- a/realsense2_camera/include/ros_utils.h +++ b/realsense2_camera/include/ros_utils.h @@ -31,12 +31,8 @@ namespace realsense2_camera const stream_index_pair INFRA0{RS2_STREAM_INFRARED, 0}; const stream_index_pair INFRA1{RS2_STREAM_INFRARED, 1}; const stream_index_pair INFRA2{RS2_STREAM_INFRARED, 2}; - const stream_index_pair FISHEYE{RS2_STREAM_FISHEYE, 0}; - const stream_index_pair FISHEYE1{RS2_STREAM_FISHEYE, 1}; - const stream_index_pair FISHEYE2{RS2_STREAM_FISHEYE, 2}; const stream_index_pair GYRO{RS2_STREAM_GYRO, 0}; const stream_index_pair ACCEL{RS2_STREAM_ACCEL, 0}; - const stream_index_pair POSE{RS2_STREAM_POSE, 0}; bool isValidCharInName(char c); diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index 9f56745988..9e95144169 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -51,8 +51,6 @@ {'name': 'accel_fps', 'default': '0', 'description': "''"}, {'name': 'enable_gyro', 'default': 'false', 'description': "'enable gyro stream'"}, {'name': 'enable_accel', 'default': 'false', 'description': "'enable accel stream'"}, - {'name': 'enable_pose', 'default': 'true', 'description': "'enable pose stream'"}, - {'name': 'pose_fps', 'default': '200', 'description': "''"}, {'name': 'pointcloud.enable', 'default': 'false', 'description': ''}, {'name': 'pointcloud.stream_filter', 'default': '2', 'description': 'texture stream for pointcloud'}, {'name': 'pointcloud.stream_index_filter','default': '0', 'description': 'texture stream index for pointcloud'}, @@ -96,36 +94,20 @@ def launch_setup(context, params, param_name_suffix=''): _config_file = LaunchConfiguration('config_file' + param_name_suffix).perform(context) params_from_file = {} if _config_file == "''" else yaml_to_dict(_config_file) # Realsense - if (os.getenv('ROS_DISTRO') == "dashing") or (os.getenv('ROS_DISTRO') == "eloquent"): - return [ - launch_ros.actions.Node( - package='realsense2_camera', - node_namespace=LaunchConfiguration('camera_namespace' + param_name_suffix), - node_name=LaunchConfiguration('camera_name' + param_name_suffix), - node_executable='realsense2_camera_node', - prefix=['stdbuf -o L'], - parameters=[params - , params_from_file - ], - output='screen', - arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level' + param_name_suffix)], - ) - ] - else: - return [ - launch_ros.actions.Node( - package='realsense2_camera', - namespace=LaunchConfiguration('camera_namespace' + param_name_suffix), - name=LaunchConfiguration('camera_name' + param_name_suffix), - executable='realsense2_camera_node', - parameters=[params - , params_from_file - ], - output='screen', - arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level' + param_name_suffix)], - emulate_tty=True, - ) - ] + return [ + launch_ros.actions.Node( + package='realsense2_camera', + namespace=LaunchConfiguration('camera_namespace' + param_name_suffix), + name=LaunchConfiguration('camera_name' + param_name_suffix), + executable='realsense2_camera_node', + parameters=[params + , params_from_file + ], + output='screen', + arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level' + param_name_suffix)], + emulate_tty=True, + ) + ] def generate_launch_description(): return LaunchDescription(declare_configurable_parameters(configurable_parameters) + [ diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index faedde2dd2..ab4d1d763b 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -3,7 +3,7 @@ realsense2_camera 4.54.1 - RealSense camera package allowing access to Intel SR300 and D400 3D cameras + RealSense camera package allowing access to Intel D400 3D cameras LibRealSense ROS Team Apache License 2.0 diff --git a/realsense2_camera/scripts/rs2_listener.py b/realsense2_camera/scripts/rs2_listener.py index 1fcbc97834..b56185cd35 100644 --- a/realsense2_camera/scripts/rs2_listener.py +++ b/realsense2_camera/scripts/rs2_listener.py @@ -24,13 +24,8 @@ import struct import quaternion import os -if (os.getenv('ROS_DISTRO') != "dashing"): - import tf2_ros -if (os.getenv('ROS_DISTRO') == "humble"): - from sensor_msgs.msg import PointCloud2 as msg_PointCloud2 - from sensor_msgs_py import point_cloud2 as pc2 -# from sensor_msgs.msg import PointCloud2 as msg_PointCloud2 -# import sensor_msgs.point_cloud2 as pc2 +from sensor_msgs.msg import PointCloud2 as msg_PointCloud2 +from sensor_msgs_py import point_cloud2 as pc2 from sensor_msgs.msg import Imu as msg_Imu try: @@ -220,9 +215,8 @@ def wait_for_messages(self, themes): node.get_logger().info('Subscribing %s on topic: %s' % (theme_name, theme['topic'])) self.func_data[theme_name]['sub'] = node.create_subscription(theme['msg_type'], theme['topic'], theme['callback'](theme_name), qos.qos_profile_sensor_data) - if (os.getenv('ROS_DISTRO') != "dashing"): - self.tfBuffer = tf2_ros.Buffer() - self.tf_listener = tf2_ros.TransformListener(self.tfBuffer, node) + self.tfBuffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tfBuffer, node) self.prev_time = time.time() break_timeout = False diff --git a/realsense2_camera/scripts/rs2_test.py b/realsense2_camera/scripts/rs2_test.py index 697f72ad5b..c22f35efc2 100644 --- a/realsense2_camera/scripts/rs2_test.py +++ b/realsense2_camera/scripts/rs2_test.py @@ -21,8 +21,7 @@ from rclpy.node import Node from importRosbag.importRosbag import importRosbag import numpy as np -if (os.getenv('ROS_DISTRO') != "dashing"): - import tf2_ros +import tf2_ros import itertools import subprocess import time @@ -277,20 +276,16 @@ def print_results(results): def get_tfs(coupled_frame_ids): res = dict() - if (os.getenv('ROS_DISTRO') == "dashing"): - for couple in coupled_frame_ids: + tfBuffer = tf2_ros.Buffer() + node = Node('tf_listener') + listener = tf2_ros.TransformListener(tfBuffer, node) + rclpy.spin_once(node) + for couple in coupled_frame_ids: + from_id, to_id = couple + if (tfBuffer.can_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=3e6))): + res[couple] = tfBuffer.lookup_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=1e6)).transform + else: res[couple] = None - else: - tfBuffer = tf2_ros.Buffer() - node = Node('tf_listener') - listener = tf2_ros.TransformListener(tfBuffer, node) - rclpy.spin_once(node) - for couple in coupled_frame_ids: - from_id, to_id = couple - if (tfBuffer.can_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=3e6))): - res[couple] = tfBuffer.lookup_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=1e6)).transform - else: - res[couple] = None return res def kill_realsense2_camera_node(): @@ -372,17 +367,12 @@ def main(): #{'name': 'points_cloud_1', 'type': 'pointscloud_avg', 'params': {'rosbag_filename': outdoors_filename, 'pointcloud.enable': 'true'}}, {'name': 'depth_w_cloud_1', 'type': 'depth_avg', 'params': {'rosbag_filename': outdoors_filename, 'pointcloud.enable': 'true'}}, {'name': 'align_depth_color_1', 'type': 'align_depth_color', 'params': {'rosbag_filename': outdoors_filename, 'align_depth.enable':'true'}}, - {'name': 'align_depth_ir1_1', 'type': 'align_depth_ir1', 'params': {'rosbag_filename': outdoors_filename, 'align_depth.enable': 'true', - 'enable_infra1':'true', 'enable_infra2':'true'}}, + {'name': 'align_depth_ir1_1', 'type': 'align_depth_ir1', 'params': {'rosbag_filename': outdoors_filename, 'align_depth.enable': 'true', 'enable_infra1':'true', 'enable_infra2':'true'}}, {'name': 'depth_avg_decimation_1', 'type': 'depth_avg_decimation', 'params': {'rosbag_filename': outdoors_filename, 'decimation_filter.enable':'true'}}, {'name': 'align_depth_ir1_decimation_1', 'type': 'align_depth_ir1_decimation', 'params': {'rosbag_filename': outdoors_filename, 'align_depth.enable':'true', 'decimation_filter.enable':'true'}}, - ] - if (os.getenv('ROS_DISTRO') != "dashing"): - all_tests.extend([ - {'name': 'static_tf_1', 'type': 'static_tf', 'params': {'rosbag_filename': outdoors_filename, - 'enable_infra1':'true', 'enable_infra2':'true'}}, - {'name': 'accel_up_1', 'type': 'accel_up', 'params': {'rosbag_filename': './records/D435i_Depth_and_IMU_Stands_still.bag', 'enable_accel': 'true', 'accel_fps': '0.0'}}, - ]) + {'name': 'static_tf_1', 'type': 'static_tf', 'params': {'rosbag_filename': outdoors_filename, 'enable_infra1':'true', 'enable_infra2':'true'}}, + {'name': 'accel_up_1', 'type': 'accel_up', 'params': {'rosbag_filename': './records/D435i_Depth_and_IMU_Stands_still.bag', 'enable_accel': 'true', 'accel_fps': '0.0'}}, + ] # Normalize parameters: for test in all_tests: diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 729ff66fba..11e8b2b6d8 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -22,10 +22,8 @@ // Header files for disabling intra-process comms for static broadcaster. #include -// This header file is not available in ROS 2 Dashing. -#ifndef DASHING + #include -#endif using namespace realsense2_camera; @@ -113,7 +111,6 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node, _is_color_enabled(false), _is_depth_enabled(false), _pointcloud(false), - _publish_odom_tf(false), _imu_sync_method(imu_sync_method::NONE), _is_profile_changed(false), _is_align_depth_changed(false) @@ -182,7 +179,6 @@ void BaseRealSenseNode::initializeFormatsMaps() rs_format_to_cv_format[RS2_FORMAT_RGBA8] = CV_8UC4; rs_format_to_cv_format[RS2_FORMAT_BGRA8] = CV_8UC4; rs_format_to_cv_format[RS2_FORMAT_YUYV] = CV_16UC3; - rs_format_to_cv_format[RS2_FORMAT_M420] = CV_16UC3; rs_format_to_cv_format[RS2_FORMAT_RAW8] = CV_8UC1; rs_format_to_cv_format[RS2_FORMAT_RAW10] = CV_16UC1; rs_format_to_cv_format[RS2_FORMAT_RAW16] = CV_16UC1; @@ -198,7 +194,6 @@ void BaseRealSenseNode::initializeFormatsMaps() rs_format_to_ros_format[RS2_FORMAT_RGBA8] = sensor_msgs::image_encodings::RGBA8; rs_format_to_ros_format[RS2_FORMAT_BGRA8] = sensor_msgs::image_encodings::BGRA8; rs_format_to_ros_format[RS2_FORMAT_YUYV] = sensor_msgs::image_encodings::YUV422; - //rs_format_to_ros_format[RS2_FORMAT_M420] = not supported in ROS2 image msg yet rs_format_to_ros_format[RS2_FORMAT_RAW8] = sensor_msgs::image_encodings::TYPE_8UC1; rs_format_to_ros_format[RS2_FORMAT_RAW10] = sensor_msgs::image_encodings::TYPE_16UC1; rs_format_to_ros_format[RS2_FORMAT_RAW16] = sensor_msgs::image_encodings::TYPE_16UC1; @@ -630,9 +625,6 @@ void BaseRealSenseNode::multiple_message_callback(rs2::frame frame, imu_sync_met if (sync_method > imu_sync_method::NONE) imu_callback_sync(frame, sync_method); else imu_callback(frame); break; - case RS2_STREAM_POSE: - pose_callback(frame); - break; default: frame_callback(frame); } @@ -672,17 +664,7 @@ rclcpp::Time BaseRealSenseNode::frameSystemTimeSec(rs2::frame frame) { double elapsed_camera_ns = millisecondsToNanoseconds(timestamp_ms - _camera_time_base); - /* - Fixing deprecated-declarations compilation warning. - Duration(rcl_duration_value_t) is deprecated in favor of - static Duration::from_nanoseconds(rcl_duration_value_t) - starting from GALACTIC. - */ -#if defined(FOXY) || defined(ELOQUENT) || defined(DASHING) - auto duration = rclcpp::Duration(elapsed_camera_ns); -#else auto duration = rclcpp::Duration::from_nanoseconds(elapsed_camera_ns); -#endif return rclcpp::Time(_ros_time_base + duration); } @@ -788,7 +770,7 @@ void BaseRealSenseNode::updateExtrinsicsCalibData(const rs2::video_stream_profil void BaseRealSenseNode::SetBaseStream() { - const std::vector base_stream_priority = {DEPTH, POSE}; + const std::vector base_stream_priority = {DEPTH}; std::set checked_sips; std::map available_profiles; for(auto&& sensor : _available_ros_sensors) diff --git a/realsense2_camera/src/dynamic_params.cpp b/realsense2_camera/src/dynamic_params.cpp index 618da0766a..4fb87d67f7 100644 --- a/realsense2_camera/src/dynamic_params.cpp +++ b/realsense2_camera/src/dynamic_params.cpp @@ -113,11 +113,7 @@ namespace realsense2_camera try { ROS_DEBUG_STREAM("setParam::Setting parameter: " << param_name); -#if defined(DASHING) || defined(ELOQUENT) || defined(FOXY) - //do nothing for old versions -#else descriptor.dynamic_typing=true; // Without this, undeclare_parameter() throws in Galactic onward. -#endif if (!_node.get_parameter(param_name, result_value)) { result_value = _node.declare_parameter(param_name, initial_value, descriptor); diff --git a/realsense2_camera/src/parameters.cpp b/realsense2_camera/src/parameters.cpp index 16965f27cf..e98a48d65b 100644 --- a/realsense2_camera/src/parameters.cpp +++ b/realsense2_camera/src/parameters.cpp @@ -77,10 +77,6 @@ void BaseRealSenseNode::getParameters() _hold_back_imu_for_frames = _parameters->setParam(param_name, HOLD_BACK_IMU_FOR_FRAMES); _parameters_names.push_back(param_name); - param_name = std::string("publish_odom_tf"); - _publish_odom_tf = _parameters->setParam(param_name, PUBLISH_ODOM_TF); - _parameters_names.push_back(param_name); - param_name = std::string("base_frame_id"); _base_frame_id = _parameters->setParam(param_name, DEFAULT_BASE_FRAME_ID); _base_frame_id = (static_cast(std::ostringstream() << _camera_name << "_" << _base_frame_id)).str(); diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 410aacc78f..d57ff44e54 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -358,8 +358,6 @@ void RealSenseNodeFactory::startDevice() { switch(pid) { - case SR300_PID: - case SR300v2_PID: case RS400_PID: case RS405_PID: case RS410_PID: @@ -377,9 +375,6 @@ void RealSenseNodeFactory::startDevice() case RS457_PID: case RS465_PID: case RS_USB2_PID: - case RS_L515_PID_PRE_PRQ: - case RS_L515_PID: - case RS_L535_PID: _realSenseNode = std::unique_ptr(new BaseRealSenseNode(*this, _device, _parameters, this->get_node_options().use_intra_process_comms())); break; default: diff --git a/realsense2_camera/src/ros_utils.cpp b/realsense2_camera/src/ros_utils.cpp index b4661126cf..0ee2172904 100644 --- a/realsense2_camera/src/ros_utils.cpp +++ b/realsense2_camera/src/ros_utils.cpp @@ -111,10 +111,8 @@ static const rmw_qos_profile_t rmw_qos_profile_latched = const rmw_qos_profile_t qos_string_to_qos(std::string str) { -#if !defined(DASHING) && !defined(ELOQUENT) if (str == "UNKNOWN") return rmw_qos_profile_unknown; -#endif if (str == "SYSTEM_DEFAULT") return rmw_qos_profile_system_default; if (str == "DEFAULT") @@ -133,10 +131,8 @@ const rmw_qos_profile_t qos_string_to_qos(std::string str) const std::string list_available_qos_strings() { std::stringstream res; -#ifndef DASHING - res << "UNKNOWN" << "\n"; -#endif - res << "SYSTEM_DEFAULT" << "\n" + res << "UNKNOWN" << "\n" + << "SYSTEM_DEFAULT" << "\n" << "DEFAULT" << "\n" << "PARAMETER_EVENTS" << "\n" << "SERVICES_DEFAULT" << "\n" diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index fcdb673e34..69ca91cc34 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -281,13 +281,6 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi IMUInfo info_msg = getImuInfo(profile); _imu_info_publishers[sip]->publish(info_msg); } - else if (profile.is()) - { - std::stringstream data_topic_name, info_topic_name; - data_topic_name << stream_name << "/sample"; - _odom_publisher = _node.create_publisher(data_topic_name.str(), - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos), qos)); - } std::string topic_metadata(stream_name + "/metadata"); _metadata_publishers[sip] = _node.create_publisher(topic_metadata, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(info_qos), info_qos)); diff --git a/realsense2_camera/src/tfs.cpp b/realsense2_camera/src/tfs.cpp index 3e82666c9f..8e6201761b 100644 --- a/realsense2_camera/src/tfs.cpp +++ b/realsense2_camera/src/tfs.cpp @@ -31,15 +31,9 @@ void BaseRealSenseNode::restartStaticTransformBroadcaster() rclcpp::PublisherOptionsWithAllocator> options; options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; - #ifndef DASHING _static_tf_broadcaster = std::make_shared(_node, tf2_ros::StaticBroadcasterQoS(), std::move(options)); - #else - _static_tf_broadcaster = std::make_shared(_node, - rclcpp::QoS(100), - std::move(options)); - #endif } void BaseRealSenseNode::append_static_tf_msg(const rclcpp::Time& t, @@ -310,90 +304,3 @@ void BaseRealSenseNode::startDynamicTf() } } -void BaseRealSenseNode::pose_callback(rs2::frame frame) -{ - double frame_time = frame.get_timestamp(); - bool placeholder_false(false); - if (_is_initialized_time_base.compare_exchange_strong(placeholder_false, true) ) - { - _is_initialized_time_base = setBaseTime(frame_time, frame.get_frame_timestamp_domain()); - } - - ROS_DEBUG("Frame arrived: stream: %s ; index: %d ; Timestamp Domain: %s", - rs2_stream_to_string(frame.get_profile().stream_type()), - frame.get_profile().stream_index(), - rs2_timestamp_domain_to_string(frame.get_frame_timestamp_domain())); - rs2_pose pose = frame.as().get_pose_data(); - rclcpp::Time t(frameSystemTimeSec(frame)); - - geometry_msgs::msg::PoseStamped pose_msg; - pose_msg.pose.position.x = -pose.translation.z; - pose_msg.pose.position.y = -pose.translation.x; - pose_msg.pose.position.z = pose.translation.y; - pose_msg.pose.orientation.x = -pose.rotation.z; - pose_msg.pose.orientation.y = -pose.rotation.x; - pose_msg.pose.orientation.z = pose.rotation.y; - pose_msg.pose.orientation.w = pose.rotation.w; - - static tf2_ros::TransformBroadcaster br(_node); - geometry_msgs::msg::TransformStamped msg; - msg.header.stamp = t; - msg.header.frame_id = DEFAULT_ODOM_FRAME_ID; - msg.child_frame_id = FRAME_ID(POSE); - msg.transform.translation.x = pose_msg.pose.position.x; - msg.transform.translation.y = pose_msg.pose.position.y; - msg.transform.translation.z = pose_msg.pose.position.z; - msg.transform.rotation.x = pose_msg.pose.orientation.x; - msg.transform.rotation.y = pose_msg.pose.orientation.y; - msg.transform.rotation.z = pose_msg.pose.orientation.z; - msg.transform.rotation.w = pose_msg.pose.orientation.w; - - if (_publish_odom_tf) br.sendTransform(msg); - - if (0 != _odom_publisher->get_subscription_count()) - { - double cov_pose(_linear_accel_cov * pow(10, 3-(int)pose.tracker_confidence)); - double cov_twist(_angular_velocity_cov * pow(10, 1-(int)pose.tracker_confidence)); - - geometry_msgs::msg::Vector3Stamped v_msg; - tf2::Vector3 tfv(-pose.velocity.z, -pose.velocity.x, pose.velocity.y); - tf2::Quaternion q(-msg.transform.rotation.x, - -msg.transform.rotation.y, - -msg.transform.rotation.z, - msg.transform.rotation.w); - tfv=tf2::quatRotate(q,tfv); - v_msg.vector.x = tfv.x(); - v_msg.vector.y = tfv.y(); - v_msg.vector.z = tfv.z(); - - tfv = tf2::Vector3(-pose.angular_velocity.z, -pose.angular_velocity.x, pose.angular_velocity.y); - tfv=tf2::quatRotate(q,tfv); - geometry_msgs::msg::Vector3Stamped om_msg; - om_msg.vector.x = tfv.x(); - om_msg.vector.y = tfv.y(); - om_msg.vector.z = tfv.z(); - - nav_msgs::msg::Odometry odom_msg; - - odom_msg.header.frame_id = DEFAULT_ODOM_FRAME_ID; - odom_msg.child_frame_id = FRAME_ID(POSE); - odom_msg.header.stamp = t; - odom_msg.pose.pose = pose_msg.pose; - odom_msg.pose.covariance = {cov_pose, 0, 0, 0, 0, 0, - 0, cov_pose, 0, 0, 0, 0, - 0, 0, cov_pose, 0, 0, 0, - 0, 0, 0, cov_twist, 0, 0, - 0, 0, 0, 0, cov_twist, 0, - 0, 0, 0, 0, 0, cov_twist}; - odom_msg.twist.twist.linear = v_msg.vector; - odom_msg.twist.twist.angular = om_msg.vector; - odom_msg.twist.covariance ={cov_pose, 0, 0, 0, 0, 0, - 0, cov_pose, 0, 0, 0, 0, - 0, 0, cov_pose, 0, 0, 0, - 0, 0, 0, cov_twist, 0, 0, - 0, 0, 0, 0, cov_twist, 0, - 0, 0, 0, 0, 0, cov_twist}; - _odom_publisher->publish(odom_msg); - ROS_DEBUG("Publish %s stream", rs2_stream_to_string(frame.get_profile().stream_type())); - } -} diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index 0fd4563381..4f7d0657f7 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -576,7 +576,7 @@ def create_subscription(self, msg_type, topic , data_type, store_raw_data): super().create_subscription(msg_type, topic , self.rsCallback(topic,msg_type, store_raw_data), data_type) self.data[topic] = deque() self.frame_counter[topic] = 0 - if (os.getenv('ROS_DISTRO') != "dashing") and (self.tfBuffer == None): + if (self.tfBuffer == None): self.tfBuffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tfBuffer, super()) diff --git a/realsense2_description/urdf/_l515.urdf.xacro b/realsense2_description/urdf/_l515.urdf.xacro deleted file mode 100644 index c761ddf734..0000000000 --- a/realsense2_description/urdf/_l515.urdf.xacro +++ /dev/null @@ -1,213 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/realsense2_description/urdf/_r430.urdf.xacro b/realsense2_description/urdf/_r430.urdf.xacro index d4ea736bc8..28f65f76bf 100644 --- a/realsense2_description/urdf/_r430.urdf.xacro +++ b/realsense2_description/urdf/_r430.urdf.xacro @@ -125,20 +125,6 @@ aluminum peripherial evaluation case. - - - - - - - - - - - - - - diff --git a/realsense2_description/urdf/test_l515_camera.urdf.xacro b/realsense2_description/urdf/test_l515_camera.urdf.xacro deleted file mode 100644 index 6b1c3354a0..0000000000 --- a/realsense2_description/urdf/test_l515_camera.urdf.xacro +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - -