Skip to content

Commit 5b62c73

Browse files
maloelmaloel
authored
PR #13715 from Eran: adapter ROS2 node & parameters
2 parents 07171c5 + fdb21d5 commit 5b62c73

File tree

91 files changed

+17343
-33
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

91 files changed

+17343
-33
lines changed

‎third-party/realdds/include/realdds/dds-device-server.h

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -65,6 +65,7 @@ class dds_device_server
6565
dds_guid const & guid() const;
6666
std::shared_ptr< dds_participant > participant() const;
6767
std::shared_ptr< dds_subscriber > subscriber() const { return _subscriber; }
68+
std::shared_ptr< dds_publisher > publisher() const { return _publisher; }
6869
std::string const & topic_root() const { return _topic_root; }
6970
rsutils::string::slice debug_name() const;
7071

@@ -85,6 +86,7 @@ class dds_device_server
8586
bool operator!() const { return ! is_valid(); }
8687

8788
std::map< std::string, std::shared_ptr< dds_stream_server > > const & streams() const { return _stream_name_to_server; }
89+
dds_options const & options() const { return _options; }
8890

8991
void publish_notification( topics::flexible_msg && );
9092
void publish_metadata( rsutils::json && );
@@ -104,6 +106,7 @@ class dds_device_server
104106
// Same as find_options, except throws if not found
105107
std::shared_ptr< dds_option > get_option( std::string const & option_name, std::string const & stream_name ) const;
106108

109+
107110
private:
108111
struct control_sample;
109112

‎third-party/realdds/include/realdds/dds-stream-sensor-bridge.h

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -56,6 +56,9 @@ class dds_stream_sensor_bridge
5656
start_sensor_callback;
5757
typedef std::function< void( std::string const & sensor_name ) > stop_sensor_callback;
5858
typedef std::function< void( std::string const & error_string ) > on_error_callback;
59+
typedef std::function< void( std::shared_ptr< realdds::dds_stream_server > const &,
60+
std::shared_ptr< realdds::dds_stream_profile > const & ) >
61+
on_stream_profile_change_callback;
5962

6063
private:
6164
std::map< std::string, sensor_bridge > _sensors;
@@ -64,6 +67,7 @@ class dds_stream_sensor_bridge
6467
start_sensor_callback _on_start_sensor;
6568
stop_sensor_callback _on_stop_sensor;
6669
on_error_callback _on_error;
70+
on_stream_profile_change_callback _on_stream_profile_change;
6771

6872
public:
6973
dds_stream_sensor_bridge();
@@ -101,12 +105,16 @@ class dds_stream_sensor_bridge
101105
// Return true if the stream for the given server is currently streaming
102106
bool is_streaming( std::shared_ptr< dds_stream_server > const & ) const;
103107

108+
// Return the profile a server is set to, even if not yet streaming
109+
std::shared_ptr< dds_stream_profile > get_profile( std::shared_ptr< dds_stream_server > const & ) const;
110+
104111
// Notifications
105112
public:
106113
void on_readers_changed( readers_changed_callback callback ) { _on_readers_changed = std::move( callback ); }
107114
void on_start_sensor( start_sensor_callback callback ) { _on_start_sensor = std::move( callback ); }
108115
void on_stop_sensor( stop_sensor_callback callback ) { _on_stop_sensor = std::move( callback ); }
109116
void on_error( on_error_callback callback ) { _on_error = std::move( callback ); }
117+
void on_stream_profile_change( on_stream_profile_change_callback callback ) { _on_stream_profile_change = std::move( callback ); }
110118

111119
// Impl
112120
protected:

‎third-party/realdds/include/realdds/topics/dds-topic-names.h

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,29 @@ constexpr char const * METADATA_TOPIC_NAME = "/metadata";
2626
constexpr char const * DFU_TOPIC_NAME = "/dfu";
2727

2828

29+
namespace ros2 {
30+
31+
constexpr char const * NAMESPACE = "/realsense"; // must begin with /
32+
33+
constexpr char const * ROOT = "rt/";
34+
constexpr size_t ROOT_LEN = 3;
35+
constexpr char const * SERVICE_REQUEST_ROOT = "rq/";
36+
constexpr char const * SERVICE_RESPONSE_ROOT = "rr/";
37+
38+
constexpr char const * GET_PARAMETERS_NAME = "/get_parameters";
39+
constexpr char const * SET_PARAMETERS_NAME = "/set_parameters";
40+
constexpr char const * LIST_PARAMETERS_NAME = "/list_parameters";
41+
constexpr char const * DESCRIBE_PARAMETERS_NAME = "/describe_parameters";
42+
43+
constexpr char const * REQUEST_SUFFIX = "Request";
44+
constexpr char const * RESPONSE_SUFFIX = "Reply";
45+
46+
constexpr char const * DISCOVERY_INFO = "ros_discovery_info";
47+
constexpr char const * PARAMETER_EVENTS_NAME = "parameter_events";
48+
49+
} // namespace ros2
50+
51+
2952
namespace notification {
3053
namespace key {
3154
extern std::string const id;

‎third-party/realdds/include/realdds/topics/readme.md

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,7 @@ The various topics are arranged hierarchically:
1616

1717
The code for each message type in this directory is generated from IDL files if the message format is defined by RealDDS.
1818

19-
For [ROS](#ros2), the IDLs are not available.
19+
For [ROS](#ros2), the IDLs are available in the `/opt/ros/<distro>/share/` installation folder.
2020

2121
# Generation
2222

‎third-party/realdds/include/realdds/topics/ros2/describe-parameters-msg.h

Lines changed: 117 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,117 @@
1+
// License: Apache 2.0. See LICENSE file in root directory.
2+
// Copyright(c) 2025 Intel Corporation. All Rights Reserved.
3+
#pragma once
4+
5+
#include <realdds/dds-defines.h>
6+
7+
#include <realdds/topics/ros2/rcl_interfaces/srv/DescribeParameters.h>
8+
#include <realdds/topics/ros2/rcl_interfaces/msg/ParameterDescriptor.h>
9+
10+
#include <string>
11+
#include <memory>
12+
#include <vector>
13+
14+
15+
namespace rcl_interfaces {
16+
namespace srv {
17+
class DescribeParameters_RequestPubSubType;
18+
class DescribeParameters_ResponsePubSubType;
19+
} // namespace srv
20+
} // namespace rcl_interfaces
21+
22+
23+
namespace realdds {
24+
25+
26+
class dds_participant;
27+
class dds_topic;
28+
class dds_topic_reader;
29+
class dds_topic_writer;
30+
31+
32+
namespace topics {
33+
namespace ros2 {
34+
35+
36+
class describe_parameters_request_msg
37+
{
38+
rcl_interfaces::srv::DescribeParameters_Request _raw;
39+
40+
public:
41+
using type = rcl_interfaces::srv::DescribeParameters_RequestPubSubType;
42+
43+
void clear() { _raw.names().clear(); }
44+
std::vector< std::string > const & names() const { return _raw.names(); }
45+
void add( std::string const & name ) { _raw.names().push_back( name ); }
46+
47+
bool is_valid() const { return ! names().empty(); }
48+
void invalidate() { clear(); }
49+
50+
51+
static std::shared_ptr< dds_topic > create_topic( std::shared_ptr< dds_participant > const & participant,
52+
char const * topic_name );
53+
static std::shared_ptr< dds_topic > create_topic( std::shared_ptr< dds_participant > const & participant,
54+
std::string const & topic_name )
55+
{
56+
return create_topic( participant, topic_name.c_str() );
57+
}
58+
59+
// This helper method will take the next sample from a reader.
60+
//
61+
// Returns true if successful. Make sure you still check is_valid() in case the sample info isn't!
62+
// Returns false if no more data is available.
63+
// Will throw if an unexpected error occurs.
64+
//
65+
//Note - copies the data.
66+
//TODO - add an API for a function that loans the data and enables the user to free it later.
67+
bool take_next( dds_topic_reader &, dds_sample * optional_sample = nullptr );
68+
};
69+
70+
71+
class describe_parameters_response_msg
72+
{
73+
rcl_interfaces::srv::DescribeParameters_Response _raw;
74+
75+
public:
76+
using type = rcl_interfaces::srv::DescribeParameters_ResponsePubSubType;
77+
78+
using descriptor_type = rcl_interfaces::msg::ParameterDescriptor;
79+
80+
std::vector< descriptor_type > const & descriptors() const { return _raw.descriptors(); }
81+
bool is_valid() const { return ! descriptors().empty(); }
82+
void invalidate() { _raw.descriptors().clear(); }
83+
84+
void add( descriptor_type const & descriptor )
85+
{
86+
_raw.descriptors().push_back( descriptor );
87+
}
88+
89+
90+
static std::shared_ptr< dds_topic > create_topic( std::shared_ptr< dds_participant > const & participant,
91+
char const * topic_name );
92+
static std::shared_ptr< dds_topic > create_topic( std::shared_ptr< dds_participant > const & participant,
93+
std::string const & topic_name )
94+
{
95+
return create_topic( participant, topic_name.c_str() );
96+
}
97+
98+
// Sends out the response to the given writer
99+
// The request sample is needed for ROS2 request-response mechanism to connect the two
100+
// Returns a unique (to the writer) identifier for the sample that was sent, or 0 if unsuccessful
101+
dds_sequence_number respond_to( dds_sample const & request_sample, dds_topic_writer & ) const;
102+
103+
// This helper method will take the next sample from a reader.
104+
//
105+
// Returns true if successful. Make sure you still check is_valid() in case the sample info isn't!
106+
// Returns false if no more data is available.
107+
// Will throw if an unexpected error occurs.
108+
//
109+
//Note - copies the data.
110+
//TODO - add an API for a function that loans the data and enables the user to free it later.
111+
bool take_next( dds_topic_reader &, dds_sample * optional_sample = nullptr );
112+
};
113+
114+
115+
} // namespace ros2
116+
} // namespace topics
117+
} // namespace realdds

‎third-party/realdds/include/realdds/topics/ros2/get-parameters-msg.h

Lines changed: 112 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,112 @@
1+
// License: Apache 2.0. See LICENSE file in root directory.
2+
// Copyright(c) 2025 Intel Corporation. All Rights Reserved.
3+
#pragma once
4+
5+
#include <realdds/dds-defines.h>
6+
7+
#include <realdds/topics/ros2/rcl_interfaces/srv/GetParameters.h>
8+
#include <realdds/topics/ros2/rcl_interfaces/msg/ParameterValue.h>
9+
10+
#include <string>
11+
#include <memory>
12+
#include <vector>
13+
14+
15+
namespace rcl_interfaces {
16+
namespace srv {
17+
class GetParameters_RequestPubSubType;
18+
class GetParameters_ResponsePubSubType;
19+
} // namespace srv
20+
} // namespace rcl_interfaces
21+
22+
23+
namespace realdds {
24+
25+
26+
class dds_participant;
27+
class dds_topic;
28+
class dds_topic_reader;
29+
class dds_topic_writer;
30+
31+
32+
namespace topics {
33+
namespace ros2 {
34+
35+
36+
class get_parameters_request_msg
37+
{
38+
rcl_interfaces::srv::GetParameters_Request _raw;
39+
40+
public:
41+
using type = rcl_interfaces::srv::GetParameters_RequestPubSubType;
42+
43+
void clear() { _raw.names().clear(); }
44+
std::vector< std::string > const & names() const { return _raw.names(); }
45+
void add( std::string const & name ) { _raw.names().push_back( name ); }
46+
47+
bool is_valid() const { return ! names().empty(); }
48+
void invalidate() { clear(); }
49+
50+
51+
static std::shared_ptr< dds_topic > create_topic( std::shared_ptr< dds_participant > const & participant,
52+
char const * topic_name );
53+
static std::shared_ptr< dds_topic > create_topic( std::shared_ptr< dds_participant > const & participant,
54+
std::string const & topic_name )
55+
{
56+
return create_topic( participant, topic_name.c_str() );
57+
}
58+
59+
// This helper method will take the next sample from a reader.
60+
//
61+
// Returns true if successful. Make sure you still check is_valid() in case the sample info isn't!
62+
// Returns false if no more data is available.
63+
// Will throw if an unexpected error occurs.
64+
//
65+
//Note - copies the data.
66+
//TODO - add an API for a function that loans the data and enables the user to free it later.
67+
bool take_next( dds_topic_reader &, dds_sample * optional_sample = nullptr );
68+
};
69+
70+
71+
class get_parameters_response_msg
72+
{
73+
rcl_interfaces::srv::GetParameters_Response _raw;
74+
75+
public:
76+
using type = rcl_interfaces::srv::GetParameters_ResponsePubSubType;
77+
78+
using value_type = rcl_interfaces::msg::ParameterValue;
79+
80+
std::vector< value_type > const & values() const { return _raw.values(); }
81+
void add( value_type const & value ) { _raw.values().push_back( value ); }
82+
bool is_valid() const { return ! values().empty(); }
83+
void invalidate() { _raw.values().clear(); }
84+
85+
// Sends out the response to the given writer
86+
// The request sample is needed for ROS2 request-response mechanism to connect the two
87+
// Returns a unique (to the writer) identifier for the sample that was sent, or 0 if unsuccessful
88+
dds_sequence_number respond_to( dds_sample const & request_sample, dds_topic_writer & ) const;
89+
90+
static std::shared_ptr< dds_topic > create_topic( std::shared_ptr< dds_participant > const & participant,
91+
char const * topic_name );
92+
static std::shared_ptr< dds_topic > create_topic( std::shared_ptr< dds_participant > const & participant,
93+
std::string const & topic_name )
94+
{
95+
return create_topic( participant, topic_name.c_str() );
96+
}
97+
98+
// This helper method will take the next sample from a reader.
99+
//
100+
// Returns true if successful. Make sure you still check is_valid() in case the sample info isn't!
101+
// Returns false if no more data is available.
102+
// Will throw if an unexpected error occurs.
103+
//
104+
//Note - copies the data.
105+
//TODO - add an API for a function that loans the data and enables the user to free it later.
106+
bool take_next( dds_topic_reader &, dds_sample * optional_sample = nullptr );
107+
};
108+
109+
110+
} // namespace ros2
111+
} // namespace topics
112+
} // namespace realdds

0 commit comments

Comments
 (0)