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 b6b05ae
Show file tree
Hide file tree
Showing 7 changed files with 101 additions and 28 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
7 changes: 5 additions & 2 deletions realsense2_camera/include/base_realsense_node.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -118,7 +118,8 @@ namespace realsense2_camera
BaseRealSenseNode(rclcpp::Node& node,
rs2::device dev,
std::shared_ptr<Parameters> parameters,
bool use_intra_process = false);
bool use_intra_process = false,
bool is_dds_device = false);
~BaseRealSenseNode();
void publishTopics();

Expand Down Expand Up @@ -324,7 +325,8 @@ namespace realsense2_camera
std::vector<geometry_msgs::msg::TransformStamped> _static_tf_msgs;
std::shared_ptr<std::thread> _tf_t;

bool _use_intra_process;
bool _use_intra_process;
bool _is_dds_device;
std::map<stream_index_pair, std::shared_ptr<image_publisher>> _image_publishers;

std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr> _imu_publishers;
Expand All @@ -349,6 +351,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
60 changes: 50 additions & 10 deletions realsense2_camera/src/base_realsense_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,7 +91,8 @@ size_t SyncedImuPublisher::getNumSubscribers()
BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node,
rs2::device dev,
std::shared_ptr<Parameters> parameters,
bool use_intra_process) :
bool use_intra_process,
bool is_dds_device) :
_is_running(true),
_node(node),
_logger(node.get_logger()),
Expand All @@ -107,6 +108,7 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node,
_tf_publish_rate(TF_PUBLISH_RATE),
_diagnostics_period(0),
_use_intra_process(use_intra_process),
_is_dds_device(is_dds_device),
_is_initialized_time_base(false),
_camera_time_base(0),
_sync_frames(SYNC_FRAMES),
Expand All @@ -115,6 +117,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 +483,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 +516,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
45 changes: 33 additions & 12 deletions realsense2_camera/src/realsense_node_factory.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -101,30 +101,38 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list)
}
auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
ROS_INFO_STREAM("Device with serial number " << sn << " was found."<<std::endl);

std::string pn = dev.get_info(RS2_CAMERA_INFO_PHYSICAL_PORT);
std::string name = dev.get_info(RS2_CAMERA_INFO_NAME);
ROS_INFO_STREAM("Device with physical ID " << pn << " was found.");
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 +361,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,9 +391,13 @@ void RealSenseNodeFactory::startDevice()
case RS435i_RGB_PID:
case RS455_PID:
case RS457_PID:
case RS555_USB_PID:
case RS_USB2_PID:
_realSenseNode = std::unique_ptr<BaseRealSenseNode>(new BaseRealSenseNode(*this, _device, _parameters, this->get_node_options().use_intra_process_comms()));
break;
case RS555_DDS_FAKE_PID: // D555 in ethernet mode (DDS)
_realSenseNode = std::unique_ptr<BaseRealSenseNode>(new BaseRealSenseNode(*this, _device, _parameters, this->get_node_options().use_intra_process_comms(), true));
break;
default:
ROS_FATAL_STREAM("Unsupported device!" << " Product ID: 0x" << pid_str);
rclcpp::shutdown();
Expand Down
11 changes: 8 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,20 @@ 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));

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 b6b05ae

Please sign in to comment.