Skip to content

Commit

Permalink
Add D555 PID
Browse files Browse the repository at this point in the history
  • Loading branch information
@SamerKhshiboun
SamerKhshiboun committed Oct 8, 2024
1 parent d2e9f15 commit 10f9c62
Show file tree
Hide file tree
Showing 7 changed files with 90 additions and 25 deletions.
1 change: 1 addition & 0 deletions realsense2_camera/CMakeLists.txt
View file
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

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
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
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
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
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];
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
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.");
}
}
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
10 changes: 8 additions & 2 deletions realsense2_camera/src/rs_node_setup.cpp
View file
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));

// 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

0 comments on commit 10f9c62

Please sign in to comment.