Skip to content

Support D555 and its motion profiles #3222

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

Merged
SamerKhshiboun merged 3 commits into from
Oct 9, 2024
Merged

Support D555 and its motion profiles #3222

Show file tree
Hide file tree
Changes from 1 commit
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
Next Next commit
Add D555 PID
  • Loading branch information
@SamerKhshiboun
SamerKhshiboun committed Oct 8, 2024
commit 10f9c62b84fe30612d43ac7b1840282cf0f902cf
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) # a workaround, till librealsense2 cmake config will be fixed
Comment thread
Nir-Az marked this conversation as resolved.
Outdated

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
55 changes: 45 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,35 @@ 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());
if (IMU == stream_index)
{
// Expecting two float3 objects: first for accel, second for gyro
const float3 &accel_data = crnt_reading[0];
Comment thread
Nir-Az marked this conversation as resolved.
Outdated
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;
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
Copy Markdown
Collaborator

@Nir-Az Nir-Az Oct 9, 2024

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);
Copy link
Copy Markdown
Collaborator

@Nir-Az Nir-Az Oct 8, 2024

Choose a reason for hiding this comment

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

std::string pid_str(_device.get_info(RS2_CAMERA_INFO_PRODUCT_ID));
can be placed at the else scope only right?

Copy link
Copy Markdown
Collaborator

@Nir-Az Nir-Az Oct 8, 2024

Choose a reason for hiding this comment

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

Also we have 2 lines here.
both init a string both use different ways.
std::string x = y;
std::string x(y);
Any special reason?

Copy link
Copy Markdown
Contributor Author

@SamerKhshiboun SamerKhshiboun Oct 8, 2024

Choose a reason for hiding this comment

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

fixed, unified the calls to be x(y) on both.
we cannot init it in the else, since it used down below in the switch statement.

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
10 changes: 8 additions & 2 deletions 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";
_imu_info_publishers[sip] = _node.create_publisher<IMUInfo>(info_topic_name.str(),
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(info_qos), info_qos));

Copy link
Copy Markdown
Collaborator

@Nir-Az Nir-Az Oct 8, 2024

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
Copy Markdown
Contributor Author

@SamerKhshiboun SamerKhshiboun Oct 8, 2024

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
Copy Markdown
Contributor Author

@SamerKhshiboun SamerKhshiboun Oct 8, 2024

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.

// 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<IMUInfo>(info_topic_name.str(), qos);

IMUInfo info_msg = getImuInfo(profile);
_imu_info_publishers[sip]->publish(info_msg);
}
Expand Down