Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Support D555 and its motion profiles #3222

Merged
merged 3 commits into from
Oct 9, 2024
Merged

Support D555 and its motion profiles #3222

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
10f9c62
Add D555 PID
SamerKhshiboun Oct 5, 2024
f57a617
adjusments to DDS Support for ROS Wrapper
SamerKhshiboun Oct 8, 2024
9f37e29
support latched QoS for imu_info
SamerKhshiboun Oct 8, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions realsense2_camera/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
3 changes: 2 additions & 1 deletion realsense2_camera/include/constants.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
1 change: 1 addition & 0 deletions realsense2_camera/include/ros_utils.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down
1 change: 1 addition & 0 deletions realsense2_camera/launch/rs_launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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': "''"},
Expand Down
68 changes: 58 additions & 10 deletions realsense2_camera/src/base_realsense_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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())
Expand All @@ -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<const float3*>(frame.get_data()));
if (GYRO == stream_index)
const float3 *crnt_reading = reinterpret_cast<const float3 *>(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());
Expand Down
44 changes: 32 additions & 12 deletions realsense2_camera/src/realsense_node_factory.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -107,24 +107,31 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list)
std::vector<std::string> 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.");
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The user asked for a USB port?
From the code it looks like the code is using it

}
}
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())
{
Expand Down Expand Up @@ -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)
Expand All @@ -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<BaseRealSenseNode>(new BaseRealSenseNode(*this, _device, _parameters, this->get_node_options().use_intra_process_comms()));
break;
Expand Down
8 changes: 7 additions & 1 deletion realsense2_camera/src/rs_node_setup.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -295,8 +295,14 @@ void BaseRealSenseNode::startPublishers(const std::vector<stream_profile>& profi
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos), qos));
// Publish Intrinsics:
info_topic_name << "~/" << stream_name << "/imu_info";

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Where did we took the QOS before?
why do we need this change?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

before, we took it from "info_qos", but this was a buggy thing for imu_info.
This is the only topic in the whole ros-wrapper that defines the publisher and immediately send one message down below it (look for this _imu_info_publishers[sip]->publish(info_msg); )

so, I tested it on different cameras, and it's not working as expected.
it should publish the same static message for all subscribers, never mind when the subscriber is requesting this topic, but from my sanity checks, it echoes the relevant data only when we restart the node.
This is a bug not related to D555, but was reproducible also on other devices.

So, I changed the QoS to be manually setted.
For other INFO topics, the "info_qos" is still good, because they publish it wherever there are subscribers, not only once when defining the publisher.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Fixed to used rmw_qos_profile_latched, like the extrinsics topics.

// 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<std::allocator<void>> options;
options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable;
_imu_info_publishers[sip] = _node.create_publisher<IMUInfo>(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);
}
Expand Down
Loading