From c785bf52caaab2babc444018ab3b0e567b5d77db Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Tue, 8 Aug 2023 00:19:17 +0000 Subject: [PATCH 1/3] support color and depth/ir formats --- .../include/base_realsense_node.h | 18 +-- realsense2_camera/src/base_realsense_node.cpp | 106 +++++++++++++----- 2 files changed, 92 insertions(+), 32 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index d6f0a1b97c..b414473e31 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -205,15 +205,15 @@ namespace realsense2_camera void publishDynamicTransforms(); void publishPointCloud(rs2::points f, const rclcpp::Time& t, const rs2::frameset& frameset); Extrinsics rsExtrinsicsToMsg(const rs2_extrinsics& extrinsics) const; - IMUInfo getImuInfo(const rs2::stream_profile& profile); - + void initializeFormatsMaps(); + void fillMessageImage( const cv::Mat& cv_matrix_image, const stream_index_pair& stream, unsigned int width, unsigned int height, - unsigned int bpp, + const rs2_format& stream_format, const rclcpp::Time& t, sensor_msgs::msg::Image* img_msg_ptr); @@ -222,7 +222,6 @@ namespace realsense2_camera std::map& images, unsigned int width, unsigned int height, - unsigned int bpp, const stream_index_pair& stream); void publishFrame( @@ -234,7 +233,12 @@ namespace realsense2_camera const std::map>& image_publishers, const bool is_publishMetadata = true); - void publishRGBD(const cv::Mat& rgb_cv_matrix, const cv::Mat& depth_cv_matrix, const rclcpp::Time& t); + void publishRGBD( + const cv::Mat& rgb_cv_matrix, + const rs2_format& color_format, + const cv::Mat& depth_cv_matrix, + const rs2_format& depth_format, + const rclcpp::Time& t); void publishMetadata(rs2::frame f, const rclcpp::Time& header_time, const std::string& frame_id); @@ -293,14 +297,14 @@ namespace realsense2_camera std::map::SharedPtr> _imu_publishers; std::shared_ptr> _odom_publisher; std::shared_ptr _synced_imu_publisher; - std::map _image_formats; std::map::SharedPtr> _info_publishers; std::map::SharedPtr> _metadata_publishers; std::map::SharedPtr> _imu_info_publishers; std::map::SharedPtr> _extrinsics_publishers; rclcpp::Publisher::SharedPtr _rgbd_publisher; std::map _images; - std::map _encoding; + std::map rs_format_to_ros_format; + std::map rs_format_to_cv_format; std::map _camera_info; std::atomic_bool _is_initialized_time_base; diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index ae6b50d33c..729ff66fba 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -123,13 +123,7 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node, ROS_INFO("Intra-Process communication enabled"); } - _image_formats[1] = CV_8UC1; // CVBridge type - _image_formats[2] = CV_16UC1; // CVBridge type - _image_formats[3] = CV_8UC3; // CVBridge type - _encoding[1] = sensor_msgs::image_encodings::MONO8; // ROS message type - _encoding[2] = sensor_msgs::image_encodings::TYPE_16UC1; // ROS message type - _encoding[3] = sensor_msgs::image_encodings::RGB8; // ROS message type - + initializeFormatsMaps(); _monitor_options = {RS2_OPTION_ASIC_TEMPERATURE, RS2_OPTION_PROJECTOR_TEMPERATURE}; } @@ -171,6 +165,45 @@ void BaseRealSenseNode::publishTopics() ROS_INFO_STREAM("RealSense Node Is Up!"); } +void BaseRealSenseNode::initializeFormatsMaps() +{ + // from rs2_format to OpenCV format + // https://docs.opencv.org/3.4/d1/d1b/group__core__hal__interface.html + // https://docs.opencv.org/2.4/modules/core/doc/basic_structures.html + // CV_{U|S|F}C() + // where U is unsigned integer type, S is signed integer type, and F is float type. + // For example, CV_8UC1 means a 8-bit single-channel array, + // CV_32FC2 means a 2-channel (complex) floating-point array, and so on. + rs_format_to_cv_format[RS2_FORMAT_Y8] = CV_8UC1; + rs_format_to_cv_format[RS2_FORMAT_Y16] = CV_16UC1; + rs_format_to_cv_format[RS2_FORMAT_Z16] = CV_16UC1; + rs_format_to_cv_format[RS2_FORMAT_RGB8] = CV_8UC3; + rs_format_to_cv_format[RS2_FORMAT_BGR8] = CV_8UC3; + 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; + + // from rs2_format to ROS2 image msg encoding (format) + // http://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/Image.html + // http://docs.ros.org/en/jade/api/sensor_msgs/html/image__encodings_8h_source.html + rs_format_to_ros_format[RS2_FORMAT_Y8] = sensor_msgs::image_encodings::MONO8; + rs_format_to_ros_format[RS2_FORMAT_Y16] = sensor_msgs::image_encodings::MONO16; + rs_format_to_ros_format[RS2_FORMAT_Z16] = sensor_msgs::image_encodings::TYPE_16UC1; + rs_format_to_ros_format[RS2_FORMAT_RGB8] = sensor_msgs::image_encodings::RGB8; + rs_format_to_ros_format[RS2_FORMAT_BGR8] = sensor_msgs::image_encodings::BGR8; + 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; +} + void BaseRealSenseNode::setupFilters() { _filters.push_back(std::make_shared(std::make_shared(), _parameters, _logger)); @@ -220,7 +253,13 @@ cv::Mat& BaseRealSenseNode::fix_depth_scale(const cv::Mat& from_image, cv::Mat& to_image.create(from_image.rows, from_image.cols, from_image.type()); } - CV_Assert(from_image.depth() == _image_formats[2]); + // Samer to fix + // ROS_INFO_STREAM(from_image.depth()); + // ROS_INFO_STREAM(from_image.channels()); + // ROS_INFO_STREAM(CV_MAKETYPE(from_image.depth(),from_image.channels())); + // ROS_INFO_STREAM(rs_format_to_cv_format[RS2_FORMAT_Z16]); + // CV_Assert(CV_MAKETYPE(from_image.depth(),from_image.channels()) == rs_format_to_cv_format[RS2_FORMAT_Z16]); + // CV_Assert(3 == 1); int nRows = from_image.rows; int nCols = from_image.cols; @@ -555,7 +594,9 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame) // On this line we already know original_depth_frame is valid. if(_enable_rgbd && original_color_frame) { - publishRGBD(_images[COLOR], _depth_aligned_image[COLOR], t); + auto color_format = original_color_frame.get_profile().format(); + auto depth_format = original_depth_frame.get_profile().format(); + publishRGBD(_images[COLOR], color_format, _depth_aligned_image[COLOR], depth_format, t); } } } @@ -830,12 +871,16 @@ void BaseRealSenseNode::fillMessageImage( const stream_index_pair& stream, unsigned int width, unsigned int height, - unsigned int bpp, + const rs2_format& stream_format, const rclcpp::Time& t, sensor_msgs::msg::Image* img_msg_ptr) { + if(rs_format_to_ros_format.find(stream_format) == rs_format_to_ros_format.end()) + { + ROS_ERROR_STREAM("Format " << rs2_format_to_string(stream_format) << " is not supported in ROS2 image messages"); + } // Convert the CV::Mat into a ROS image message (1 copy is done here) - cv_bridge::CvImage(std_msgs::msg::Header(), _encoding.at(bpp), cv_matrix_image).toImageMsg(*img_msg_ptr); + cv_bridge::CvImage(std_msgs::msg::Header(), rs_format_to_ros_format[stream_format], cv_matrix_image).toImageMsg(*img_msg_ptr); // Convert OpenCV Mat to ROS Image img_msg_ptr->header.frame_id = OPTICAL_FRAME_ID(stream); @@ -843,7 +888,7 @@ void BaseRealSenseNode::fillMessageImage( img_msg_ptr->height = height; img_msg_ptr->width = width; img_msg_ptr->is_bigendian = false; - img_msg_ptr->step = width * bpp; + img_msg_ptr->step = width * cv_matrix_image.elemSize(); } cv::Mat& BaseRealSenseNode::getCVMatImage( @@ -851,13 +896,21 @@ cv::Mat& BaseRealSenseNode::getCVMatImage( std::map& images, unsigned int width, unsigned int height, - unsigned int bpp, const stream_index_pair& stream) { auto& image = images[stream]; - if (image.size() != cv::Size(width, height) || image.depth() != _image_formats[bpp]) + auto stream_format = frame.get_profile().format(); + + if (rs_format_to_cv_format.find(stream_format) == rs_format_to_cv_format.end()) + { + ROS_ERROR_STREAM("Format " << rs2_format_to_string(stream_format) << " is not supported in realsense2_camera node"); + } + // we try to reduce image creation as much we can, so we check if the same image structure + // was already created before, and we fill this image next with the frame data + // image.create() should be called once per __ + if (image.size() != cv::Size(width, height) || CV_MAKETYPE(image.depth(), image.channels()) != rs_format_to_cv_format[stream_format]) { - image.create(height, width, _image_formats[bpp]); + image.create(height, width, rs_format_to_cv_format[stream_format]); } image.data = (uint8_t*)frame.get_data(); @@ -883,13 +936,13 @@ void BaseRealSenseNode::publishFrame( ROS_DEBUG("publishFrame(...)"); unsigned int width = 0; unsigned int height = 0; - unsigned int bpp = 1; + auto stream_format = f.get_profile().format(); if (f.is()) { auto timage = f.as(); width = timage.get_width(); height = timage.get_height(); - bpp = timage.get_bytes_per_pixel(); + stream_format = timage.get_profile().format(); } // Publish stream image @@ -901,7 +954,7 @@ void BaseRealSenseNode::publishFrame( // if rgbd has subscribers we fetch the CV image here if (_rgbd_publisher && 0 != _rgbd_publisher->get_subscription_count()) { - image_cv_matrix = getCVMatImage(f, images, width, height, bpp, stream); + image_cv_matrix = getCVMatImage(f, images, width, height, stream); } // if depth/color has subscribers, ask first if rgbd already fetched @@ -910,13 +963,13 @@ void BaseRealSenseNode::publishFrame( { if(image_cv_matrix.empty()) { - image_cv_matrix = getCVMatImage(f, images, width, height, bpp, stream); + image_cv_matrix = getCVMatImage(f, images, width, height, stream); } // Prepare image topic to be published // We use UniquePtr for allow intra-process publish when subscribers of that type are available sensor_msgs::msg::Image::UniquePtr img_msg_ptr(new sensor_msgs::msg::Image()); - fillMessageImage(image_cv_matrix, stream, width, height, bpp, t, img_msg_ptr.get()); + fillMessageImage(image_cv_matrix, stream, width, height, stream_format, t, img_msg_ptr.get()); if (!img_msg_ptr) { ROS_ERROR("sensor image message allocation failed, frame was dropped"); @@ -977,22 +1030,25 @@ void BaseRealSenseNode::publishFrame( } -void BaseRealSenseNode::publishRGBD(const cv::Mat& rgb_cv_matrix, const cv::Mat& depth_cv_matrix, const rclcpp::Time& t) +void BaseRealSenseNode::publishRGBD( + const cv::Mat& rgb_cv_matrix, + const rs2_format& color_format, + const cv::Mat& depth_cv_matrix, + const rs2_format& depth_format, + const rclcpp::Time& t) { if (_rgbd_publisher && 0 != _rgbd_publisher->get_subscription_count()) { ROS_DEBUG_STREAM("Publishing RGBD message"); unsigned int rgb_width = rgb_cv_matrix.size().width; unsigned int rgb_height = rgb_cv_matrix.size().height; - unsigned int rgb_bpp = rgb_cv_matrix.elemSize(); unsigned int depth_width = depth_cv_matrix.size().width; unsigned int depth_height = depth_cv_matrix.size().height; - unsigned int depth_bpp = depth_cv_matrix.elemSize(); realsense2_camera_msgs::msg::RGBD::UniquePtr msg(new realsense2_camera_msgs::msg::RGBD()); - fillMessageImage(rgb_cv_matrix, COLOR, rgb_width, rgb_height, rgb_bpp, t, &msg->rgb); - fillMessageImage(depth_cv_matrix, DEPTH, depth_width, depth_height, depth_bpp, t, &msg->depth); + fillMessageImage(rgb_cv_matrix, COLOR, rgb_width, rgb_height, color_format, t, &msg->rgb); + fillMessageImage(depth_cv_matrix, DEPTH, depth_width, depth_height, depth_format, t, &msg->depth); msg->header.frame_id = "camera_rgbd_optical_frame"; msg->header.stamp = t; From e05a75b0c6d37d595eb9dd91fafa28b5386b63be Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Thu, 10 Aug 2023 01:14:17 +0000 Subject: [PATCH 2/3] 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 @@ - - - - - - - - - - - - From 4412cdc1fc50e6d5a0b6d0cf20cf4852a6aa3c9e Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Mon, 11 Sep 2023 10:59:49 +0000 Subject: [PATCH 3/3] rebase from ros2-development --- README.md | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/README.md b/README.md index 6f8275983d..75822f5048 100644 --- a/README.md +++ b/README.md @@ -354,17 +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. -- **unite_imu_method**: - - D400 cameras have built in IMU components which produce 2 unrelated streams, each with it's own frequency: - - *gyro* - which shows angular velocity - - *accel* which shows linear acceleration. - - By default, 2 corresponding topics are available, each with only the relevant fields of the message sensor_msgs::Imu are filled out. - - Setting *unite_imu_method* creates a new topic, *imu*, that replaces the default *gyro* and *accel* topics. - - The *imu* topic is published at the rate of the gyro. - - All the fields of the Imu message under the *imu* topic are filled out. - - `unite_imu_method` parameter supported values are [0-2] meaning: [0 -> None, 1 -> Copy, 2 -> Linear_ interpolation] when: - - **linear_interpolation**: Every gyro message is attached by the an accel message interpolated to the gyro's timestamp. - - **copy**: Every gyro message is attached by the last accel message. - **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`