From a6dd2d3eb50e532cb0cb71698d9309234eeeb642 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 + realsense2_camera/include/constants.h | 4 +- realsense2_camera/include/ros_utils.h | 1 + realsense2_camera/src/base_realsense_node.cpp | 55 ++++++++++++++++--- .../src/realsense_node_factory.cpp | 42 ++++++++++---- realsense2_camera/src/rs_node_setup.cpp | 12 +++- 6 files changed, 90 insertions(+), 25 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/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..ffbfb9aae9 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -480,7 +480,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 +513,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..6e9ca647d3 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -107,24 +107,31 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list) std::vector 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 +360,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,6 +390,8 @@ void RealSenseNodeFactory::startDevice() case RS435i_RGB_PID: case RS455_PID: case RS457_PID: + case RS555_USB_PID: + case RS555_DDS_FAKE_PID: case RS_USB2_PID: _realSenseNode = std::unique_ptr(new BaseRealSenseNode(*this, _device, _parameters, this->get_node_options().use_intra_process_comms())); break; diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index d95c8b52f9..f80a16f45d 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -222,7 +222,7 @@ 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); @@ -295,8 +295,14 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi 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)); + + // QoS settings for Latching-like behavior for the imu_info topic + // History: KeepLast(1) - Retains only the last message + // Durability: Transient Local - Ensures that late subscribers get the last message that was published + // Reliability: Ensures reliable delivery of messages + 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); }