From b6b05ae9b7d61d8490b92aec1c75669842a22831 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Sat, 5 Oct 2024 23:33:29 +0300 Subject: [PATCH] Add D555 PID --- realsense2_camera/CMakeLists.txt | 1 + .../include/base_realsense_node.h | 7 ++- realsense2_camera/include/constants.h | 4 +- realsense2_camera/include/ros_utils.h | 1 + realsense2_camera/src/base_realsense_node.cpp | 60 +++++++++++++++---- .../src/realsense_node_factory.cpp | 45 ++++++++++---- realsense2_camera/src/rs_node_setup.cpp | 11 +++- 7 files changed, 101 insertions(+), 28 deletions(-) diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 3453a665de..9a708ab5d8 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -118,6 +118,7 @@ find_package(tf2_ros REQUIRED) find_package(tf2 REQUIRED) find_package(diagnostic_updater REQUIRED) find_package(OpenCV REQUIRED COMPONENTS core) +find_package(fastrtps REQUIRED) find_package(realsense2 2.56.0) if (BUILD_ACCELERATE_GPU_WITH_GLSL) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index e6b4287a81..988db76e96 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -118,7 +118,8 @@ namespace realsense2_camera BaseRealSenseNode(rclcpp::Node& node, rs2::device dev, std::shared_ptr parameters, - bool use_intra_process = false); + bool use_intra_process = false, + bool is_dds_device = false); ~BaseRealSenseNode(); void publishTopics(); @@ -324,7 +325,8 @@ namespace realsense2_camera std::vector _static_tf_msgs; std::shared_ptr _tf_t; - bool _use_intra_process; + bool _use_intra_process; + bool _is_dds_device; std::map> _image_publishers; std::map::SharedPtr> _imu_publishers; @@ -349,6 +351,7 @@ namespace realsense2_camera bool _is_depth_enabled; bool _is_accel_enabled; bool _is_gyro_enabled; + bool _is_imu_enabled; bool _pointcloud; imu_sync_method _imu_sync_method; stream_index_pair _pointcloud_texture; diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index 7f5b28cee1..08f7b8609e 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -65,7 +65,9 @@ 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 RS457_PID = 0xABCD; // D457 + const uint16_t RS555_USB_PID = 0x0B56; // D555 USB + const uint16_t RS555_DDS_FAKE_PID = 0xFFFF; //D555 DDS (Dummy PID) const bool ALLOW_NO_TEXTURE_POINTS = false; const bool ORDERED_PC = false; diff --git a/realsense2_camera/include/ros_utils.h b/realsense2_camera/include/ros_utils.h index d31b0d42bf..937162dcb9 100644 --- a/realsense2_camera/include/ros_utils.h +++ b/realsense2_camera/include/ros_utils.h @@ -33,6 +33,7 @@ namespace realsense2_camera const stream_index_pair INFRA2{RS2_STREAM_INFRARED, 2}; const stream_index_pair GYRO{RS2_STREAM_GYRO, 0}; const stream_index_pair ACCEL{RS2_STREAM_ACCEL, 0}; + const stream_index_pair IMU{RS2_STREAM_MOTION, 0}; bool isValidCharInName(char c); diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 4b88ec7df1..0fe433caf7 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -91,7 +91,8 @@ size_t SyncedImuPublisher::getNumSubscribers() BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node, rs2::device dev, std::shared_ptr parameters, - bool use_intra_process) : + bool use_intra_process, + bool is_dds_device) : _is_running(true), _node(node), _logger(node.get_logger()), @@ -107,6 +108,7 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node, _tf_publish_rate(TF_PUBLISH_RATE), _diagnostics_period(0), _use_intra_process(use_intra_process), + _is_dds_device(is_dds_device), _is_initialized_time_base(false), _camera_time_base(0), _sync_frames(SYNC_FRAMES), @@ -115,6 +117,7 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node, _is_depth_enabled(false), _is_accel_enabled(false), _is_gyro_enabled(false), + _is_imu_enabled(false), _pointcloud(false), _imu_sync_method(imu_sync_method::NONE), _is_profile_changed(false), @@ -480,7 +483,25 @@ void BaseRealSenseNode::imu_callback(rs2::frame frame) frame.get_profile().stream_index(), rs2_timestamp_domain_to_string(frame.get_frame_timestamp_domain())); - auto stream_index = (stream == GYRO.first)?GYRO:ACCEL; + stream_index_pair stream_index; + + if(stream == GYRO.first) + { + stream_index = GYRO; + } + else if(stream == ACCEL.first) + { + stream_index = ACCEL; + } + else if(stream == IMU.first) + { + stream_index = IMU; + } + else + { + throw std::runtime_error("unknown IMU stream."); + } + rclcpp::Time t(frameSystemTimeSec(frame)); if(_imu_publishers.find(stream_index) == _imu_publishers.end()) @@ -495,18 +516,37 @@ void BaseRealSenseNode::imu_callback(rs2::frame frame) ImuMessage_AddDefaultValues(imu_msg); imu_msg.header.frame_id = OPTICAL_FRAME_ID(stream_index); - auto crnt_reading = *(reinterpret_cast(frame.get_data())); - if (GYRO == stream_index) + const float3 *crnt_reading = reinterpret_cast(frame.get_data()); + if (IMU == stream_index) { - imu_msg.angular_velocity.x = crnt_reading.x; - imu_msg.angular_velocity.y = crnt_reading.y; - imu_msg.angular_velocity.z = crnt_reading.z; + // Expecting two float3 objects: first for accel, second for gyro + const float3 &accel_data = crnt_reading[0]; + const float3 &gyro_data = crnt_reading[1]; + + // Fill the IMU ROS2 message with both accel and gyro data + imu_msg.linear_acceleration.x = accel_data.x; + imu_msg.linear_acceleration.y = accel_data.y; + imu_msg.linear_acceleration.z = accel_data.z; + + imu_msg.angular_velocity.x = gyro_data.x; + imu_msg.angular_velocity.y = gyro_data.y; + imu_msg.angular_velocity.z = gyro_data.z; + } + else if (GYRO == stream_index) + { + imu_msg.angular_velocity.x = crnt_reading->x; + imu_msg.angular_velocity.y = crnt_reading->y; + imu_msg.angular_velocity.z = crnt_reading->z; } else if (ACCEL == stream_index) { - imu_msg.linear_acceleration.x = crnt_reading.x; - imu_msg.linear_acceleration.y = crnt_reading.y; - imu_msg.linear_acceleration.z = crnt_reading.z; + imu_msg.linear_acceleration.x = crnt_reading->x; + imu_msg.linear_acceleration.y = crnt_reading->y; + imu_msg.linear_acceleration.z = crnt_reading->z; + } + else + { + ROS_ERROR("undefined stream index for IMU"); } imu_msg.header.stamp = t; _imu_publishers[stream_index]->publish(imu_msg); diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index f7ea18bca7..2e4db68116 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -101,30 +101,38 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list) } auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER); ROS_INFO_STREAM("Device with serial number " << sn << " was found."< results; ROS_INFO_STREAM("Device with name " << name << " was found."); std::string port_id = parseUsbPort(pn); - if (port_id.empty()) + + std::string pid_str(dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID)); + if(pid_str != "DDS") { - std::stringstream msg; - msg << "Error extracting usb port from device with physical ID: " << pn << std::endl << "Please report on github issue at https://github.com/IntelRealSense/realsense-ros"; - if (_usb_port_id.empty()) + if (port_id.empty()) { - ROS_WARN_STREAM(msg.str()); + std::stringstream msg; + msg << "Error extracting usb port from device with physical ID: " << pn << std::endl + << "Please report on github issue at https://github.com/IntelRealSense/realsense-ros"; + if (_usb_port_id.empty()) + { + ROS_WARN_STREAM(msg.str()); + } + else + { + ROS_ERROR_STREAM(msg.str()); + ROS_ERROR_STREAM("Please use serial number instead of usb port."); + } } else { - ROS_ERROR_STREAM(msg.str()); - ROS_ERROR_STREAM("Please use serial number instead of usb port."); + ROS_INFO_STREAM("Device with port number " << port_id << " was found."); } } - else - { - ROS_INFO_STREAM("Device with port number " << port_id << " was found."); - } + bool found_device_type(true); if (!_device_type.empty()) { @@ -353,7 +361,16 @@ void RealSenseNodeFactory::startDevice() { if (_realSenseNode) _realSenseNode.reset(); std::string pid_str(_device.get_info(RS2_CAMERA_INFO_PRODUCT_ID)); - uint16_t pid = std::stoi(pid_str, 0, 16); + uint16_t pid; + + if (pid_str == "DDS") + { + pid = RS555_DDS_FAKE_PID; + } + else + { + pid = std::stoi(pid_str, 0, 16); + } try { switch(pid) @@ -374,9 +391,13 @@ void RealSenseNodeFactory::startDevice() case RS435i_RGB_PID: case RS455_PID: case RS457_PID: + case RS555_USB_PID: case RS_USB2_PID: _realSenseNode = std::unique_ptr(new BaseRealSenseNode(*this, _device, _parameters, this->get_node_options().use_intra_process_comms())); break; + case RS555_DDS_FAKE_PID: // D555 in ethernet mode (DDS) + _realSenseNode = std::unique_ptr(new BaseRealSenseNode(*this, _device, _parameters, this->get_node_options().use_intra_process_comms(), true)); + break; default: ROS_FATAL_STREAM("Unsupported device!" << " Product ID: 0x" << pid_str); rclcpp::shutdown(); diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index d95c8b52f9..8e26f15c10 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -201,6 +201,7 @@ void BaseRealSenseNode::stopPublishers(const std::vector& profil { _is_accel_enabled = false; _is_gyro_enabled = false; + _is_imu_enabled = false; _synced_imu_publisher.reset(); _imu_publishers.erase(sip); _imu_info_publishers.erase(sip); @@ -222,7 +223,6 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi { stream_index_pair sip(profile.stream_type(), profile.stream_index()); std::string stream_name(STREAM_NAME(sip)); - rmw_qos_profile_t qos = sensor.getQOS(sip); rmw_qos_profile_t info_qos = sensor.getInfoQOS(sip); @@ -288,15 +288,20 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi _is_accel_enabled = true; else if (profile.stream_type() == RS2_STREAM_GYRO) _is_gyro_enabled = true; + else if(profile.stream_type() == RS2_STREAM_MOTION) + _is_imu_enabled = true; std::stringstream data_topic_name, info_topic_name; data_topic_name << "~/" << stream_name << "/sample"; _imu_publishers[sip] = _node.create_publisher(data_topic_name.str(), rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos), qos)); + // Publish Intrinsics: info_topic_name << "~/" << stream_name << "/imu_info"; - _imu_info_publishers[sip] = _node.create_publisher(info_topic_name.str(), - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(info_qos), info_qos)); + + auto qos = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(); + _imu_info_publishers[sip] = _node.create_publisher(info_topic_name.str(), qos); + IMUInfo info_msg = getImuInfo(profile); _imu_info_publishers[sip]->publish(info_msg); }