Support D555 and its motion profiles #3222
Conversation
SamerKhshiboun
commented
Oct 5, 2024
•
SamerKhshiboun
commented
- RSDEV-991
- LRS-847
- Support D555 in USB and DDS Mode
- Add new Motion stream for D555 DDS Mode
- Fixed a bug in IMU_INFO (not related to DDS/D555 - reproducible also in other devices)
SamerKhshiboun
force-pushed
the
support_d555
branch
3 times, most recently
from
e732d32 to
b6b05ae
Compare
SamerKhshiboun
force-pushed
the
support_d555
branch
10 times, most recently
from
2c70387 to
6877793
Compare
SamerKhshiboun
force-pushed
the
support_d555
branch
from
6877793 to
10f9c62
Compare
| @@ -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); | |||
There was a problem hiding this comment.
std::string pid_str(_device.get_info(RS2_CAMERA_INFO_PRODUCT_ID));
can be placed at the else scope only right?
There was a problem hiding this comment.
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?
There was a problem hiding this comment.
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.
| @@ -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)); | |||
|
|
|||
There was a problem hiding this comment.
Where did we took the QOS before?
why do we need this change?
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
Fixed to used rmw_qos_profile_latched, like the extrinsics topics.
| else | ||
| { | ||
| ROS_ERROR_STREAM(msg.str()); | ||
| ROS_ERROR_STREAM("Please use serial number instead of usb port."); |
There was a problem hiding this comment.
The user asked for a USB port?
From the code it looks like the code is using it
SamerKhshiboun
merged commit c586628
into
IntelRealSense:ros2-development
