diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 3453a665de..0e78eb1957 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) # TODO: LRS-1203 - This is a workaround, till librealsense2 cmake config will be fixed 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..c8904a14eb 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -65,7 +65,8 @@ 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_PID = 0x0B56; // D555 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/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index a79df0f84b..14f3139c4b 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -62,6 +62,7 @@ {'name': 'enable_gyro', 'default': 'false', 'description': "'enable gyro stream'"}, {'name': 'enable_accel', 'default': 'false', 'description': "'enable accel stream'"}, {'name': 'gyro_fps', 'default': '0', 'description': "''"}, + {'name': 'enable_motion', 'default': 'false', 'description': "'enable motion stream (IMU) for DDS devices'"}, {'name': 'accel_fps', 'default': '0', 'description': "''"}, {'name': 'unite_imu_method', 'default': "0", 'description': '[0-None, 1-copy, 2-linear_interpolation]'}, {'name': 'clip_distance', 'default': '-2.', 'description': "''"}, diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 4b88ec7df1..72cc1c3ee0 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -480,7 +480,26 @@ 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 + { + ROS_ERROR("Unknown IMU stream type."); + return; + } + rclcpp::Time t(frameSystemTimeSec(frame)); if(_imu_publishers.find(stream_index) == _imu_publishers.end()) @@ -495,19 +514,48 @@ 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()); + size_t data_size = frame.get_data_size(); + + if (IMU == stream_index) + { + // Expecting two float3 objects: first for accel, second for gyro + // Check if we have at least two float3s + if (data_size >= 2 * sizeof(float3)) + { + 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 + { + ROS_ERROR_STREAM("Insufficient data for IMU (Motion) frame: expected at least " + << 2 * sizeof(float3) << " bytes, but got " + << data_size << " bytes."); + return; + } + } + 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; + 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) + else // 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; } + imu_msg.header.stamp = t; _imu_publishers[stream_index]->publish(imu_msg); ROS_DEBUG("Publish %s stream", ros_stream_to_string(frame.get_profile().stream_type()).c_str()); diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index f7ea18bca7..f80ed002e4 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()) { @@ -352,8 +359,20 @@ void RealSenseNodeFactory::init() void RealSenseNodeFactory::startDevice() { if (_realSenseNode) _realSenseNode.reset(); + std::string device_name(_device.get_info(RS2_CAMERA_INFO_NAME)); 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 (device_name == "Intel RealSense D555") + { + // currently the PID of DDS devices is hardcoded as "DDS" + // need to be fixed in librealsense + pid = RS555_PID; + } + else + { + pid = std::stoi(pid_str, 0, 16); + } try { switch(pid) @@ -374,6 +393,7 @@ void RealSenseNodeFactory::startDevice() case RS435i_RGB_PID: case RS455_PID: case RS457_PID: + case RS555_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..df0c15d8c7 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -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 will have latched QoS, and it will publish its data only once during the ROS Node lifetime. + // intra-process do not support latched QoS, so we need to disable intra-process for this topic + rclcpp::PublisherOptionsWithAllocator> options; + options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; _imu_info_publishers[sip] = _node.create_publisher(info_topic_name.str(), - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(info_qos), info_qos)); + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_latched), rmw_qos_profile_latched), + std::move(options)); IMUInfo info_msg = getImuInfo(profile); _imu_info_publishers[sip]->publish(info_msg); }