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 58506045cc..75822f5048 100644 --- a/README.md +++ b/README.md @@ -143,8 +143,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 ``` @@ -203,7 +203,7 @@ User can set the camera name and camera namespace, to distinguish between camera ```ros2 launch realsense2_camera rs_launch.py camera_namespace:=robot1 camera_name:=D455_1``` - - With ros2 run (using remapping mechanisim [Reference](https://docs.ros.org/en/foxy/How-To-Guides/Node-arguments.html)): + - With ros2 run (using remapping mechanisim [Reference](https://docs.ros.org/en/humble/How-To-Guides/Node-arguments.html)): ```ros2 run realsense2_camera realsense2_camera_node --ros-args -r __node:=D455_1 -r __ns:=robot1``` @@ -269,13 +269,13 @@ User can set the camera name and camera namespace, to distinguish between camera - 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. @@ -286,14 +286,14 @@ User can set the camera name and camera namespace, to distinguish between camera - 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) @@ -354,7 +354,6 @@ User can set the camera name and camera namespace, to distinguish between camera - If set to true, the device will reset prior to usage. - For example: `initial_reset:=true` - **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. - **clip_distance**: - Remove from the depth image all values above a given value (meters). Disable by giving negative value (default) - For example: `clip_distance:=1.5` @@ -371,8 +370,6 @@ User can set the camera name and camera namespace, to distinguish between camera - 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 d6a2761a23..fd06cb4868 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -144,34 +144,18 @@ 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) + set(SOURCES "${SOURCES}" src/ros_param_backend.cpp) elseif("$ENV{ROS_DISTRO}" STREQUAL "iron") message(STATUS "Build for ROS2 Iron") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DIRON") - set(SOURCES "${SOURCES}" src/ros_param_backend_foxy.cpp) + set(SOURCES "${SOURCES}" src/ros_param_backend.cpp) elseif("$ENV{ROS_DISTRO}" STREQUAL "rolling") message(STATUS "Build for ROS2 Rolling") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DROLLING") - set(SOURCES "${SOURCES}" src/ros_param_backend_rolling.cpp) + set(SOURCES "${SOURCES}" src/ros_param_backend.cpp) else() message(FATAL_ERROR "Unsupported ROS Distribution: " "$ENV{ROS_DISTRO}") endif() diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index a1cfe45037..34c5e8ebae 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -39,8 +39,6 @@ #include #include #include -#include -#include #include #include @@ -249,7 +247,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); @@ -294,7 +291,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; @@ -317,7 +313,6 @@ namespace realsense2_camera bool _is_accel_enabled; bool _is_gyro_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..0f99fee585 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -31,19 +31,7 @@ #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 +// Based on: https://docs.ros2.org/latest/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) #define ROS_WARN_STREAM(msg) RCLCPP_WARN_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/profile_manager.h b/realsense2_camera/include/profile_manager.h index b753ff67a9..3021e95f6c 100644 --- a/realsense2_camera/include/profile_manager.h +++ b/realsense2_camera/include/profile_manager.h @@ -103,12 +103,4 @@ namespace realsense2_camera protected: std::map > _fps; }; - - class PoseProfilesManager : public MotionProfilesManager - { - public: - using MotionProfilesManager::MotionProfilesManager; - void registerProfileParameters(std::vector all_profiles, std::function update_sensor_func) override; - }; - } 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 ef0f167f7e..c6c14db6c4 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -40,8 +40,6 @@ {'name': 'enable_infra', 'default': 'false', 'description': 'enable infra0 stream'}, {'name': 'enable_infra1', 'default': 'false', 'description': 'enable infra1 stream'}, {'name': 'enable_infra2', 'default': 'false', 'description': 'enable infra2 stream'}, - {'name': 'enable_fisheye1', 'default': 'true', 'description': 'enable fisheye1 stream'}, - {'name': 'enable_fisheye2', 'default': 'true', 'description': 'enable fisheye2 stream'}, {'name': 'depth_module.profile', 'default': '0,0,0', 'description': 'depth module profile'}, {'name': 'depth_module.depth_format', 'default': 'Z16', 'description': 'depth stream format'}, {'name': 'depth_module.infra_format', 'default': 'RGB8', 'description': 'infra0 stream format'}, @@ -55,7 +53,6 @@ {'name': 'depth_module.gain.1', 'default': '16', 'description': 'Depth module first gain value. Used for hdr_merge filter'}, {'name': 'depth_module.exposure.2', 'default': '1', 'description': 'Depth module second exposure value. Used for hdr_merge filter'}, {'name': 'depth_module.gain.2', 'default': '16', 'description': 'Depth module second gain value. Used for hdr_merge filter'}, - {'name': 'enable_confidence', 'default': 'true', 'description': 'enable confidence'}, {'name': 'enable_sync', 'default': 'false', 'description': "'enable sync mode'"}, {'name': 'enable_rgbd', 'default': 'false', 'description': "'enable rgbd topic'"}, {'name': 'enable_gyro', 'default': 'false', 'description': "'enable gyro stream'"}, @@ -63,8 +60,6 @@ {'name': 'gyro_fps', 'default': '0', 'description': "''"}, {'name': 'accel_fps', 'default': '0', 'description': "''"}, {'name': 'unite_imu_method', 'default': "0", 'description': '[0-None, 1-copy, 2-linear_interpolation]'}, - {'name': 'enable_pose', 'default': 'true', 'description': "'enable pose stream'"}, - {'name': 'pose_fps', 'default': '200', 'description': "''"}, {'name': 'clip_distance', 'default': '-2.', 'description': "''"}, {'name': 'angular_velocity_cov', 'default': '0.01', 'description': "''"}, {'name': 'linear_accel_cov', 'default': '0.01', 'description': "''"}, @@ -101,37 +96,18 @@ def yaml_to_dict(path_to_yaml): 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=LaunchConfiguration('output' + param_name_suffix), - 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=LaunchConfiguration('output' + param_name_suffix), - 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=LaunchConfiguration('output' + param_name_suffix), + 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..9cc356f7bd 100644 --- a/realsense2_camera/scripts/rs2_listener.py +++ b/realsense2_camera/scripts/rs2_listener.py @@ -19,18 +19,12 @@ from rclpy import qos from sensor_msgs.msg import Image as msg_Image import numpy as np -import inspect import ctypes 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 +import tf2_ros +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 +214,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..b1351aba66 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(): @@ -338,7 +333,7 @@ def run_tests(tests): listener_res = msg_retriever.wait_for_messages(themes) if 'static_tf' in [test['type'] for test in rec_tests]: print ('Gathering static transforms') - frame_ids = ['camera_link', 'camera_depth_frame', 'camera_infra1_frame', 'camera_infra2_frame', 'camera_color_frame', 'camera_fisheye_frame', 'camera_pose'] + frame_ids = ['camera_link', 'camera_depth_frame', 'camera_infra1_frame', 'camera_infra2_frame', 'camera_color_frame'] coupled_frame_ids = [xx for xx in itertools.combinations(frame_ids, 2)] listener_res['static_tf'] = get_tfs(coupled_frame_ids) @@ -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/scripts/show_center_depth.py b/realsense2_camera/scripts/show_center_depth.py index fb957664f4..acf8ac0db2 100644 --- a/realsense2_camera/scripts/show_center_depth.py +++ b/realsense2_camera/scripts/show_center_depth.py @@ -31,8 +31,6 @@ def __init__(self, depth_image_topic, depth_info_topic): self.bridge = CvBridge() self.sub = self.create_subscription(msg_Image, depth_image_topic, self.imageDepthCallback, 1) self.sub_info = self.create_subscription(CameraInfo, depth_info_topic, self.imageDepthInfoCallback, 1) - confidence_topic = depth_image_topic.replace('depth', 'confidence') - self.sub_conf = self.create_subscription(msg_Image, confidence_topic, self.confidenceCallback, 1) self.intrinsics = None self.pix = None self.pix_grade = None @@ -62,17 +60,6 @@ def imageDepthCallback(self, data): except ValueError as e: return - def confidenceCallback(self, data): - try: - cv_image = self.bridge.imgmsg_to_cv2(data, data.encoding) - grades = np.bitwise_and(cv_image >> 4, 0x0f) - if (self.pix): - self.pix_grade = grades[self.pix[1], self.pix[0]] - except CvBridgeError as e: - print(e) - return - - def imageDepthInfoCallback(self, cameraInfo): try: @@ -106,7 +93,6 @@ def main(): print ('Application subscribes to %s and %s topics.' % (depth_image_topic, depth_info_topic)) print ('Application then calculates and print the range to the closest object.') print ('If intrinsics data is available, it also prints the 3D location of the object') - print ('If a confedence map is also available in the topic %s, it also prints the confidence grade.' % depth_image_topic.replace('depth', 'confidence')) print () listener = ImageListener(depth_image_topic, depth_info_topic) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index d9cfb06529..368d2f10c1 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -22,10 +22,7 @@ // 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; @@ -115,7 +112,6 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node, _is_accel_enabled(false), _is_gyro_enabled(false), _pointcloud(false), - _publish_odom_tf(false), _imu_sync_method(imu_sync_method::NONE), _is_profile_changed(false), _is_align_depth_changed(false) @@ -633,9 +629,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); } @@ -675,17 +668,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); } @@ -791,7 +774,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) @@ -899,7 +882,6 @@ bool BaseRealSenseNode::fillROSImageMsgAndReturnStatus( img_msg_ptr->width = width; img_msg_ptr->is_bigendian = false; img_msg_ptr->step = width * cv_matrix_image.elemSize(); - return true; } diff --git a/realsense2_camera/src/dynamic_params.cpp b/realsense2_camera/src/dynamic_params.cpp index 618da0766a..acce9038ca 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 + descriptor.dynamic_typing=true; // Without this, undeclare_parameter() throws error. 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 e007f60515..14accf94f0 100644 --- a/realsense2_camera/src/parameters.cpp +++ b/realsense2_camera/src/parameters.cpp @@ -74,10 +74,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/profile_manager.cpp b/realsense2_camera/src/profile_manager.cpp index beffd2d574..95dadbbad6 100644 --- a/realsense2_camera/src/profile_manager.cpp +++ b/realsense2_camera/src/profile_manager.cpp @@ -591,20 +591,3 @@ void MotionProfilesManager::registerFPSParams() } } -/////////////////////////////////////////////////////////////////////////////////////// - -void PoseProfilesManager::registerProfileParameters(std::vector all_profiles, std::function update_sensor_func) -{ - std::set checked_sips; - for (auto& profile : all_profiles) - { - if (!profile.is()) continue; - _all_profiles.push_back(profile); - stream_index_pair sip(profile.stream_type(), profile.stream_index()); - checked_sips.insert(sip); - } - registerSensorUpdateParam("enable_%s", checked_sips, _enabled_profiles, true, update_sensor_func); - registerSensorUpdateParam("%s_fps", checked_sips, _fps, 0, update_sensor_func); - registerSensorQOSParam("%s_qos", checked_sips, _profiles_image_qos_str, HID_QOS); - registerSensorQOSParam("%s_info_qos", checked_sips, _profiles_info_qos_str, DEFAULT_QOS); -} diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 46b1d13ce1..e53b459551 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_param_backend_foxy.cpp b/realsense2_camera/src/ros_param_backend.cpp similarity index 100% rename from realsense2_camera/src/ros_param_backend_foxy.cpp rename to realsense2_camera/src/ros_param_backend.cpp diff --git a/realsense2_camera/src/ros_param_backend_dashing.cpp b/realsense2_camera/src/ros_param_backend_dashing.cpp deleted file mode 100644 index 8ecf8e7e8f..0000000000 --- a/realsense2_camera/src/ros_param_backend_dashing.cpp +++ /dev/null @@ -1,34 +0,0 @@ -// Copyright 2023 Intel Corporation. All Rights Reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ros_param_backend.h" - -namespace realsense2_camera -{ - void ParametersBackend::add_on_set_parameters_callback(ros2_param_callback_type callback) - { - rclcpp::Node::OnParametersSetCallbackType prev_callback = _node.set_on_parameters_set_callback(callback); - if (prev_callback) - { - rclcpp::Node::OnParametersSetCallbackType prev_callback = _node.set_on_parameters_set_callback(prev_callback); - std::stringstream msg; - msg << "Cannot set another callback to current node: " << _node.get_name(); - throw std::runtime_error(msg.str()); - } - } - - ParametersBackend::~ParametersBackend() - { - } -} diff --git a/realsense2_camera/src/ros_param_backend_rolling.cpp b/realsense2_camera/src/ros_param_backend_rolling.cpp deleted file mode 100644 index e3998e9808..0000000000 --- a/realsense2_camera/src/ros_param_backend_rolling.cpp +++ /dev/null @@ -1,32 +0,0 @@ -// Copyright 2023 Intel Corporation. All Rights Reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ros_param_backend.h" - -namespace realsense2_camera -{ - void ParametersBackend::add_on_set_parameters_callback(ros2_param_callback_type callback) - { - _ros_callback = _node.add_on_set_parameters_callback(callback); - } - - ParametersBackend::~ParametersBackend() - { - if (_ros_callback) - { - _node.remove_on_set_parameters_callback((rclcpp::node_interfaces::OnSetParametersCallbackHandle*)(_ros_callback.get())); - _ros_callback.reset(); - } - } -} diff --git a/realsense2_camera/src/ros_sensor.cpp b/realsense2_camera/src/ros_sensor.cpp index d860a79dec..635424d8c5 100644 --- a/realsense2_camera/src/ros_sensor.cpp +++ b/realsense2_camera/src/ros_sensor.cpp @@ -176,12 +176,6 @@ void RosSensor::registerSensorParameters() { _profile_managers.push_back(profile_manager); } - profile_manager = std::make_shared(_params.getParameters(), _logger); - profile_manager->registerProfileParameters(all_profiles, _update_sensor_func); - if (profile_manager->isTypeExist()) - { - _profile_managers.push_back(profile_manager); - } } void RosSensor::runFirstFrameInitialization() 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 6e05d9ad85..d98157f6d9 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -136,8 +136,7 @@ void BaseRealSenseNode::setAvailableSensors() const std::string module_name(rs2_to_ros(sensor.get_info(RS2_CAMERA_INFO_NAME))); std::unique_ptr rosSensor; if (sensor.is() || - sensor.is() || - sensor.is()) + sensor.is()) { ROS_DEBUG_STREAM("Set " << module_name << " as VideoSensor."); rosSensor = std::make_unique(sensor, _parameters, frame_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger, _use_intra_process, _dev.is()); @@ -147,11 +146,6 @@ void BaseRealSenseNode::setAvailableSensors() ROS_DEBUG_STREAM("Set " << module_name << " as ImuSensor."); rosSensor = std::make_unique(sensor, _parameters, imu_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger, false, _dev.is()); } - else if (sensor.is()) - { - ROS_DEBUG_STREAM("Set " << module_name << " as PoseSensor."); - rosSensor = std::make_unique(sensor, _parameters, multiple_message_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger, false, _dev.is()); - } else { ROS_WARN_STREAM("Module Name \"" << module_name << "\" does not define a callback."); @@ -285,15 +279,8 @@ 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, + _metadata_publishers[sip] = _node.create_publisher(topic_metadata, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(info_qos), info_qos)); if (!((rs2::stream_profile)profile==(rs2::stream_profile)_base_profile)) @@ -351,7 +338,7 @@ void BaseRealSenseNode::updateSensors() std::vector wanted_profiles; bool is_profile_changed(sensor->getUpdatedProfiles(wanted_profiles)); - bool is_video_sensor = (sensor->is() || sensor->is() || sensor->is()); + bool is_video_sensor = (sensor->is() || sensor->is()); // do all updates if profile has been changed, or if the align depth filter status has been changed // and we are on a video sensor. TODO: explore better options to monitor and update changes 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/rosbag/test_rosbag_depth_tests.py b/realsense2_camera/test/rosbag/test_rosbag_depth_tests.py index abbfe28cab..f393f42709 100644 --- a/realsense2_camera/test/rosbag/test_rosbag_depth_tests.py +++ b/realsense2_camera/test/rosbag/test_rosbag_depth_tests.py @@ -146,7 +146,7 @@ def test_static_tf_1(self,delayed_launch_descr_with_parameters): self.shutdown() def process_data(self, themes): #print ('Gathering static transforms') - frame_ids = ['camera_link', 'camera_depth_frame', 'camera_infra1_frame', 'camera_infra2_frame', 'camera_color_frame', 'camera_fisheye_frame', 'camera_pose'] + frame_ids = ['camera_link', 'camera_depth_frame', 'camera_infra1_frame', 'camera_infra2_frame', 'camera_color_frame'] coupled_frame_ids = [xx for xx in itertools.combinations(frame_ids, 2)] res = {} for couple in coupled_frame_ids: diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index 63ede24d57..bdd1bdba1a 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -580,7 +580,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..883523359d 100644 --- a/realsense2_description/urdf/_r430.urdf.xacro +++ b/realsense2_description/urdf/_r430.urdf.xacro @@ -32,7 +32,6 @@ aluminum peripherial evaluation case. publishing TF values with actual calibrated camera extrinsic values --> - - - - - - - - - - - - - - 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 @@ - - - - - - - - - - - -