From da04a3a9dc01058051261ff5e0b27960af98ae8d Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Sun, 29 Jan 2023 04:37:10 +0200 Subject: [PATCH 1/3] remove T265 device from ROS Wrapper - step1 --- README.md | 24 +--- realsense2_camera/CMakeLists.txt | 2 - realsense2_camera/include/constants.h | 1 - .../include/t265_realsense_node.h | 32 ----- .../launch/rs_d400_and_t265_launch.py | 4 - realsense2_camera/launch/rs_launch.py | 1 - realsense2_camera/package.xml | 2 +- .../src/realsense_node_factory.cpp | 4 - realsense2_camera/src/t265_realsense_node.cpp | 127 ------------------ 9 files changed, 6 insertions(+), 191 deletions(-) delete mode 100644 realsense2_camera/include/t265_realsense_node.h delete mode 100644 realsense2_camera/src/t265_realsense_node.cpp diff --git a/README.md b/README.md index 6f78cf632f..946cf8de46 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,5 @@ # ROS2 Wrapper for Intel® RealSense™ Devices -These are packages for using Intel RealSense cameras (D400 and L500 series, SR300 camera and T265 Tracking Module) with ROS2. +These are packages for using Intel RealSense cameras (D400, L500 and SR300 cameras) with ROS2. This version supports ROS2 Dashing, Eloquent, Foxy, Galactic and Rolling. @@ -157,7 +157,7 @@ Enabling stream adds matching topics. For instance, enabling the gyro and accel To turn them off: `ros2 param set /camera/camera enable_infra false` The "/camera" prefix is the namesapce specified in the given launch file. -When using D435 or D415, the gyro and accel topics wont be available. Likewise, other topics will be available when using T265 (see below). +When using D435 or D415, the gyro and accel topics wont be available. ### The metadata topic: The metadata messages store the camera's available metadata in a *json* format. To learn more, a dedicated script for echoing a metadata topic in runtime is attached. For instance, use the following command to echo the camera/depth/metadata topic: @@ -230,14 +230,14 @@ For setting a new value for a parameter use `ros2 param set 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) -- **linear_accel_cov**, **angular_velocity_cov**: sets the variance given to the Imu readings. For the T265, these values are being modified by the inner confidence value. +- **linear_accel_cov**, **angular_velocity_cov**: sets the variance given to the Imu readings. - **hold_back_imu_for_frames**: Images processing takes time. Therefor there is a time gap between the moment the image arrives at the wrapper and the moment the image is published to the ROS environment. During this time, Imu messages keep on arriving and a situation is created where an image with earlier timestamp is published after Imu message with later timestamp. If that is a problem, setting *hold_back_imu_for_frames* to *true* will hold the Imu messages back while processing the images and then publish them all in a burst, thus keeping the order of publication as the order of arrival. Note that in either case, the timestamp in each message's header reflects the time of it's origin. -- **topic_odom_in**: For T265, add wheel odometry information through this topic. The code refers only to the *twist.linear* field in the message. +- **topic_odom_in**: The code refers only to the *twist.linear* field in the message. - **calib_odom_file**: For the T265 to include odometry input, it must be given a [configuration file](https://github.com/IntelRealSense/librealsense/blob/master/unit-tests/resources/calibration_odometry.json). Explanations can be found [here](https://github.com/IntelRealSense/librealsense/pull/3462). The calibration is done in ROS coordinates system. - **publish_tf**: boolean, publish or not TF at all. Defaults to True. - **diagnostics_period**: double, positive values set the period between diagnostics updates on the `/diagnostics` topic. 0 or negative values mean no diagnostics topic is published. Defaults to 0.
@@ -248,20 +248,6 @@ The `/diagnostics` topic includes information regarding the device temperatures ### Available services: - device_info : retrieve information about the device - serial_number, firmware_version etc. Type `ros2 interface show realsense2_camera_msgs/srv/DeviceInfo` for the full list. Call example: `ros2 service call /camera/device_info realsense2_camera_msgs/srv/DeviceInfo` - Note that for **ROS2 Dashing** the command is `ros2 srv show realsense2_camera_msgs/srv/DeviceInfo` -## Using T265 ## - -### Start the camera node -To start the camera node: - -```bash -ros2 run realsense2_camera realsense2_camera_node --ros-args -p enable_pose:=true -p device_type:=t265 -``` -or, if you also have a d4xx connected, you can try out the launch file: -```bash -ros2 launch realsense2_camera rs_d400_and_t265_launch.py enable_fisheye12:=true enable_fisheye22:=true -``` -- note: the parameters are called `enable_fisheye12` and `enable_fisheye22`. The node knows them as `enable_fisheye1` and `enable_fisheye2` but launch file runs 2 nodes and these parameters refer to the second one. - ## Efficient intra-process communication: diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 84de9fd5dc..c6f7f45bb2 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -118,7 +118,6 @@ set(node_plugins "") set(SOURCES src/realsense_node_factory.cpp src/base_realsense_node.cpp - src/t265_realsense_node.cpp src/parameters.cpp src/rs_node_setup.cpp src/ros_sensor.cpp @@ -165,7 +164,6 @@ set(INCLUDES include/constants.h include/realsense_node_factory.h include/base_realsense_node.h - include/t265_realsense_node.h include/ros_sensor.h include/ros_utils.h include/dynamic_params.h diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index f640bab854..1c1961f827 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -68,7 +68,6 @@ namespace realsense2_camera const uint16_t RS416_RGB_PID = 0x0B52; // F416 RGB const uint16_t RS405_PID = 0x0B5B; // DS5U const uint16_t RS455_PID = 0x0B5C; // D455 - const uint16_t RS_T265_PID = 0x0b37; // const uint16_t RS_L515_PID_PRE_PRQ = 0x0B3D; // const uint16_t RS_L515_PID = 0x0B64; // const uint16_t RS_L535_PID = 0x0b68; diff --git a/realsense2_camera/include/t265_realsense_node.h b/realsense2_camera/include/t265_realsense_node.h deleted file mode 100644 index c8e1f312ca..0000000000 --- a/realsense2_camera/include/t265_realsense_node.h +++ /dev/null @@ -1,32 +0,0 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. - -#pragma once - -#include -#include "std_msgs/msg/string.hpp" - -namespace realsense2_camera -{ - class T265RealsenseNode : public BaseRealSenseNode - { - public: - T265RealsenseNode(rclcpp::Node& node, - rs2::device dev, - std::shared_ptr parameters, - bool use_intra_process = false); - void publishTopics(); - - protected: - void calcAndPublishStaticTransform(const rs2::stream_profile& profile, const rs2::stream_profile& base_profile) override; - - private: - void initializeOdometryInput(); - void setupSubscribers(); - void odom_in_callback(const nav_msgs::msg::Odometry::SharedPtr msg); - - rclcpp::Subscription::SharedPtr _odom_subscriber; - rs2::wheel_odometer _wo_snr; - bool _use_odom_in; - }; -} diff --git a/realsense2_camera/launch/rs_d400_and_t265_launch.py b/realsense2_camera/launch/rs_d400_and_t265_launch.py index 54ea3f927b..a62731affe 100644 --- a/realsense2_camera/launch/rs_d400_and_t265_launch.py +++ b/realsense2_camera/launch/rs_d400_and_t265_launch.py @@ -13,10 +13,6 @@ local_parameters = [{'name': 'camera_name1', 'default': 'D400', 'description': 'camera unique name'}, {'name': 'device_type1', 'default': 'd4.', 'description': 'choose device by type'}, - {'name': 'camera_name2', 'default': 'T265', 'description': 'camera unique name'}, - {'name': 'device_type2', 'default': 't265', 'description': 'choose device by type'}, - {'name': 'enable_fisheye12', 'default': 'false', 'description': 'topic for T265 wheel odometry'}, - {'name': 'enable_fisheye22', 'default': 'false', 'description': 'topic for T265 wheel odometry'}, ] def generate_launch_description(): diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index f5a03317dd..79dadb0e52 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -48,7 +48,6 @@ {'name': 'allow_no_texture_points', 'default': 'false', 'description': "''"}, {'name': 'pointcloud.ordered_pc', 'default': 'false', 'description': ''}, {'name': 'calib_odom_file', 'default': "''", 'description': "''"}, - {'name': 'topic_odom_in', 'default': "''", 'description': 'topic for T265 wheel odometry'}, {'name': 'tf_publish_rate', 'default': '0.0', 'description': 'Rate of publishing static_tf'}, {'name': 'diagnostics_period', 'default': '0.0', 'description': 'Rate of publishing diagnostics. 0=Disabled'}, {'name': 'decimation_filter.enable', 'default': 'false', 'description': 'Rate of publishing static_tf'}, diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index b5cedbe649..0556f105ba 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -3,7 +3,7 @@ realsense2_camera 4.51.1 - RealSense camera package allowing access to Intel T265 Tracking module and SR300 and D400 3D cameras + RealSense camera package allowing access to Intel SR300 and D400 3D cameras LibRealSense ROS Team Apache License 2.0 diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 9c071b366c..b356b501db 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -3,7 +3,6 @@ #include "../include/realsense_node_factory.h" #include "../include/base_realsense_node.h" -#include "../include/t265_realsense_node.h" #include #include #include @@ -380,9 +379,6 @@ void RealSenseNodeFactory::startDevice() case RS_L535_PID: _realSenseNode = std::unique_ptr(new BaseRealSenseNode(*this, _device, _parameters, this->get_node_options().use_intra_process_comms())); break; - case RS_T265_PID: - _realSenseNode = std::unique_ptr(new T265RealsenseNode(*this, _device, _parameters, this->get_node_options().use_intra_process_comms())); - break; default: ROS_FATAL_STREAM("Unsupported device!" << " Product ID: 0x" << pid_str); rclcpp::shutdown(); diff --git a/realsense2_camera/src/t265_realsense_node.cpp b/realsense2_camera/src/t265_realsense_node.cpp deleted file mode 100644 index 523363282d..0000000000 --- a/realsense2_camera/src/t265_realsense_node.cpp +++ /dev/null @@ -1,127 +0,0 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. - -#include "../include/t265_realsense_node.h" -#include - -using namespace realsense2_camera; - -T265RealsenseNode::T265RealsenseNode(rclcpp::Node& node, - rs2::device dev, - std::shared_ptr parameters, - bool use_intra_process) : - BaseRealSenseNode(node, dev, parameters, use_intra_process), - _wo_snr(dev.first()), - _use_odom_in(false) - { - _monitor_options = {RS2_OPTION_ASIC_TEMPERATURE, RS2_OPTION_MOTION_MODULE_TEMPERATURE}; - initializeOdometryInput(); - } - -void T265RealsenseNode::initializeOdometryInput() -{ - std::string calib_odom_file; - std::string param_name("calib_odom_file"); - calib_odom_file = _parameters->setParam(param_name, ""); - if (calib_odom_file.empty()) - { - ROS_INFO("No calib_odom_file. No input odometry accepted."); - return; - } - _parameters_names.push_back(param_name); - - std::ifstream calibrationFile(calib_odom_file); - if (!calibrationFile) - { - ROS_FATAL_STREAM("calibration_odometry file not found. calib_odom_file = " << calib_odom_file); - throw std::runtime_error("calibration_odometry file not found" ); - } - const std::string json_str((std::istreambuf_iterator(calibrationFile)), - std::istreambuf_iterator()); - const std::vector wo_calib(json_str.begin(), json_str.end()); - - if (!_wo_snr.load_wheel_odometery_config(wo_calib)) - { - ROS_FATAL_STREAM("Format error in calibration_odometry file: " << calib_odom_file); - throw std::runtime_error("Format error in calibration_odometry file" ); - } - _use_odom_in = true; -} - -void T265RealsenseNode::publishTopics() -{ - BaseRealSenseNode::publishTopics(); - setupSubscribers(); -} - -void T265RealsenseNode::setupSubscribers() -{ - if (!_use_odom_in) return; - - std::string topic_odom_in = _parameters->setParam("topic_odom_in", DEFAULT_TOPIC_ODOM_IN); - ROS_INFO_STREAM("Subscribing to in_odom topic: " << topic_odom_in); - - _odom_subscriber = _node.create_subscription(topic_odom_in, 1, [this](const nav_msgs::msg::Odometry::SharedPtr msg){odom_in_callback(msg);}); -} - -void T265RealsenseNode::odom_in_callback(const nav_msgs::msg::Odometry::SharedPtr msg) -{ - ROS_DEBUG("Got in_odom message"); - rs2_vector velocity {-(float)(msg->twist.twist.linear.y), - (float)(msg->twist.twist.linear.z), - -(float)(msg->twist.twist.linear.x)}; - - ROS_DEBUG_STREAM("Add odom: " << velocity.x << ", " << velocity.y << ", " << velocity.z); - _wo_snr.send_wheel_odometry(0, 0, velocity); -} - -void T265RealsenseNode::calcAndPublishStaticTransform(const rs2::stream_profile& profile, const rs2::stream_profile& base_profile) -{ - // Transform base to stream - stream_index_pair sip(profile.stream_type(), profile.stream_index()); - - tf2::Quaternion quaternion_optical; - quaternion_optical.setRPY(M_PI / 2, 0.0, -M_PI / 2); //Pose To ROS - float3 zero_trans{0, 0, 0}; - - rclcpp::Time transform_ts_ = _node.now(); - - rs2_extrinsics ex; - try - { - ex = base_profile.get_extrinsics_to(profile); - } - catch (std::exception& e) - { - if (!strcmp(e.what(), "Requested extrinsics are not available!")) - { - ROS_WARN_STREAM("(" << rs2_stream_to_string(profile.stream_type()) << ", " << profile.stream_index() << ") -> (" << rs2_stream_to_string(base_profile.stream_type()) << ", " << base_profile.stream_index() << "): " << e.what() << " : using unity as default."); - ex = rs2_extrinsics({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0,0,0}}); - } - else - { - throw e; - } - } - - auto Q = rotationMatrixToQuaternion(ex.rotation); - Q = quaternion_optical * Q * quaternion_optical.inverse(); - float3 trans{ex.translation[0], ex.translation[1], ex.translation[2]}; - - if (sip == POSE) - { - Q = Q.inverse(); - publish_static_tf(transform_ts_, trans, Q, FRAME_ID(sip), _base_frame_id); - } - else - { - publish_static_tf(transform_ts_, trans, Q, _base_frame_id, FRAME_ID(sip)); - publish_static_tf(transform_ts_, zero_trans, quaternion_optical, FRAME_ID(sip), OPTICAL_FRAME_ID(sip)); - - if (profile.is() && profile.stream_type() != RS2_STREAM_DEPTH && profile.stream_index() == 1) - { - publish_static_tf(transform_ts_, trans, Q, _base_frame_id, ALIGNED_DEPTH_TO_FRAME_ID(sip)); - publish_static_tf(transform_ts_, zero_trans, quaternion_optical, ALIGNED_DEPTH_TO_FRAME_ID(sip), OPTICAL_FRAME_ID(sip)); - } - } -} From ec1d83e3f7c961a6a1692cbb2d207e23d60c0f59 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Tue, 31 Jan 2023 12:32:01 +0200 Subject: [PATCH 2/3] remove T265 - edit launch files and README --- README.md | 2 -- realsense2_camera/include/constants.h | 1 - .../launch/rs_d400_and_t265_launch.py | 26 ------------------- realsense2_camera/launch/rs_launch.py | 1 - 4 files changed, 30 deletions(-) delete mode 100644 realsense2_camera/launch/rs_d400_and_t265_launch.py diff --git a/README.md b/README.md index 946cf8de46..6659fdb695 100644 --- a/README.md +++ b/README.md @@ -237,8 +237,6 @@ Setting *unite_imu_method* creates a new topic, *imu*, that replaces the default - **clip_distance**: remove from the depth image all values above a given value (meters). Disable by giving negative value (default) - **linear_accel_cov**, **angular_velocity_cov**: sets the variance given to the Imu readings. - **hold_back_imu_for_frames**: Images processing takes time. Therefor there is a time gap between the moment the image arrives at the wrapper and the moment the image is published to the ROS environment. During this time, Imu messages keep on arriving and a situation is created where an image with earlier timestamp is published after Imu message with later timestamp. If that is a problem, setting *hold_back_imu_for_frames* to *true* will hold the Imu messages back while processing the images and then publish them all in a burst, thus keeping the order of publication as the order of arrival. Note that in either case, the timestamp in each message's header reflects the time of it's origin. -- **topic_odom_in**: The code refers only to the *twist.linear* field in the message. -- **calib_odom_file**: For the T265 to include odometry input, it must be given a [configuration file](https://github.com/IntelRealSense/librealsense/blob/master/unit-tests/resources/calibration_odometry.json). Explanations can be found [here](https://github.com/IntelRealSense/librealsense/pull/3462). The calibration is done in ROS coordinates system. - **publish_tf**: boolean, publish or not TF at all. Defaults to True. - **diagnostics_period**: double, positive values set the period between diagnostics updates on the `/diagnostics` topic. 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. diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index 1c1961f827..387ffd3515 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -102,7 +102,6 @@ namespace realsense2_camera const std::string DEFAULT_UNITE_IMU_METHOD = ""; const std::string DEFAULT_FILTERS = ""; - const std::string DEFAULT_TOPIC_ODOM_IN = ""; const float ROS_DEPTH_SCALE = 0.001; diff --git a/realsense2_camera/launch/rs_d400_and_t265_launch.py b/realsense2_camera/launch/rs_d400_and_t265_launch.py deleted file mode 100644 index a62731affe..0000000000 --- a/realsense2_camera/launch/rs_d400_and_t265_launch.py +++ /dev/null @@ -1,26 +0,0 @@ -# License: Apache 2.0. See LICENSE file in root directory. -# Copyright(c) 2022 Intel Corporation. All Rights Reserved. - -"""Launch realsense2_camera node without rviz2.""" -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.substitutions import ThisLaunchFileDir -from launch.launch_description_sources import PythonLaunchDescriptionSource -import sys -import pathlib -sys.path.append(str(pathlib.Path(__file__).parent.absolute())) -import rs_launch - -local_parameters = [{'name': 'camera_name1', 'default': 'D400', 'description': 'camera unique name'}, - {'name': 'device_type1', 'default': 'd4.', 'description': 'choose device by type'}, - ] - -def generate_launch_description(): - return LaunchDescription( - rs_launch.declare_configurable_parameters(local_parameters) + - [ - IncludeLaunchDescription( - PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/rs_multi_camera_launch.py']), - launch_arguments=rs_launch.set_configurable_parameters(local_parameters).items(), - ), - ]) diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index 79dadb0e52..e2e5bbc24d 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -47,7 +47,6 @@ {'name': 'initial_reset', 'default': 'false', 'description': "''"}, {'name': 'allow_no_texture_points', 'default': 'false', 'description': "''"}, {'name': 'pointcloud.ordered_pc', 'default': 'false', 'description': ''}, - {'name': 'calib_odom_file', 'default': "''", 'description': "''"}, {'name': 'tf_publish_rate', 'default': '0.0', 'description': 'Rate of publishing static_tf'}, {'name': 'diagnostics_period', 'default': '0.0', 'description': 'Rate of publishing diagnostics. 0=Disabled'}, {'name': 'decimation_filter.enable', 'default': 'false', 'description': 'Rate of publishing static_tf'}, From 80d9bb707d0f0ac3778aa119690c266f3a87a191 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Tue, 31 Jan 2023 14:06:19 +0200 Subject: [PATCH 3/3] remove T265 leftovers --- realsense2_camera/launch/rs_launch.py | 1 - realsense2_camera/src/realsense_node_factory.cpp | 6 ------ 2 files changed, 7 deletions(-) diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index e2e5bbc24d..fd0310f9a6 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -26,7 +26,6 @@ {'name': 'enable_infra1', 'default': 'false', 'description': 'enable infra1 stream'}, {'name': 'enable_infra2', 'default': 'false', 'description': 'enable infra2 stream'}, {'name': 'infra_rgb', 'default': 'false', 'description': 'enable infra2 stream'}, - {'name': 'tracking_module.profile', 'default': '0,0,0', 'description': 'fisheye width'}, {'name': 'enable_fisheye1', 'default': 'true', 'description': 'enable fisheye1 stream'}, {'name': 'enable_fisheye2', 'default': 'true', 'description': 'enable fisheye2 stream'}, {'name': 'enable_confidence', 'default': 'true', 'description': 'enable depth stream'}, diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 51ea0f8b37..b5f68ef2b2 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -174,12 +174,6 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list) } } - bool remove_tm2_handle(_device && RS_T265_PID != std::stoi(_device.get_info(RS2_CAMERA_INFO_PRODUCT_ID), 0, 16)); - if (remove_tm2_handle) - { - _ctx.unload_tracking_module(); - } - if (_device && _initial_reset) { _initial_reset = false;