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 7, 2024
1 parent d2e9f15 commit f94cc9d
Show file tree
Hide file tree
Showing 7 changed files with 94 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)

find_package(realsense2 2.56.0)
if (BUILD_ACCELERATE_GPU_WITH_GLSL)
Expand Down
1 change: 1 addition & 0 deletions realsense2_camera/include/base_realsense_node.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -349,6 +349,7 @@ namespace realsense2_camera
bool _is_depth_enabled;
bool _is_accel_enabled;
bool _is_gyro_enabled;
bool _is_imu_enabled;
bool _pointcloud;
imu_sync_method _imu_sync_method;
stream_index_pair _pointcloud_texture;
Expand Down
4 changes: 3 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,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;
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
56 changes: 47 additions & 9 deletions realsense2_camera/src/base_realsense_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -115,6 +115,7 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node,
_is_depth_enabled(false),
_is_accel_enabled(false),
_is_gyro_enabled(false),
_is_imu_enabled(false),
_pointcloud(false),
_imu_sync_method(imu_sync_method::NONE),
_is_profile_changed(false),
Expand Down Expand Up @@ -480,7 +481,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())
Expand All @@ -495,18 +514,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<const float3*>(frame.get_data()));
if (GYRO == stream_index)
const float3 *crnt_reading = reinterpret_cast<const float3 *>(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);
Expand Down
42 changes: 30 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 @@ -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)
Expand All @@ -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<BaseRealSenseNode>(new BaseRealSenseNode(*this, _device, _parameters, this->get_node_options().use_intra_process_comms()));
break;
Expand Down
14 changes: 11 additions & 3 deletions realsense2_camera/src/rs_node_setup.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -201,6 +201,7 @@ void BaseRealSenseNode::stopPublishers(const std::vector<stream_profile>& profil
{
_is_accel_enabled = false;
_is_gyro_enabled = false;
_is_imu_enabled = false;
_synced_imu_publisher.reset();
_imu_publishers.erase(sip);
_imu_info_publishers.erase(sip);
Expand All @@ -222,7 +223,6 @@ void BaseRealSenseNode::startPublishers(const std::vector<stream_profile>& 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);

Expand Down Expand Up @@ -288,15 +288,23 @@ void BaseRealSenseNode::startPublishers(const std::vector<stream_profile>& profi
_is_accel_enabled = true;
else if (profile.stream_type() == RS2_STREAM_GYRO)
_is_gyro_enabled = true;
else if(profile.stream_type() == RS2_STREAM_MOTION)
_is_imu_enabled = true;

std::stringstream data_topic_name, info_topic_name;
data_topic_name << "~/" << stream_name << "/sample";
_imu_publishers[sip] = _node.create_publisher<sensor_msgs::msg::Imu>(data_topic_name.str(),
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 f94cc9d

Please sign in to comment.