Skip to content

Commit 05c3a3d

Browse files
mhubiimhubii
authored
Merge pull request #78 from roboreg/task/roboreg-ros2-58/default-qos-profiles
remove qos from params (#58)
2 parents dae5bf9 + c3d0e69 commit 05c3a3d

11 files changed

Lines changed: 151 additions & 486 deletions

File tree

‎roboreg_base/roboreg_base/monocular_depth_node.py‎

Lines changed: 40 additions & 122 deletions
Original file line numberDiff line numberDiff line change
@@ -4,182 +4,100 @@
44
from rcl_interfaces.msg import Parameter, SetParametersResult
55
from sensor_msgs.msg import CameraInfo, CompressedImage, Image, JointState
66

7-
from .parameters import QoSParams, TopicParams
8-
from .qos_profile_factory import qos_profile_factory
7+
from .qos_profiles import JOINT_STATES_QOS, SENSOR_QOS
98
from .roboreg_node import RoboregNode
109

1110

1211
class MonocularDepthNode(RoboregNode):
1312
@dataclass
1413
class _ExtraParams:
15-
image_topic: TopicParams
16-
camera_info_topic: TopicParams
17-
depth_topic: TopicParams
18-
depth_camera_info_topic: TopicParams
19-
joint_state_topic: TopicParams
14+
image_topic: str
15+
camera_info_topic: str
16+
depth_topic: str
17+
depth_camera_info_topic: str
18+
joint_state_topic: str
2019

2120
def _register_synced_subscribers(self):
22-
qos_profile = qos_profile_factory(self._extra_params.image_topic.qos)
2321
self._data_collector.subscribers["camera.image"] = Subscriber(
2422
self,
2523
(
2624
CompressedImage
27-
if "compressed" in self._extra_params.image_topic.name
25+
if "compressed" in self._extra_params.image_topic
2826
else Image
2927
),
30-
self._extra_params.image_topic.name,
31-
qos_profile=qos_profile,
28+
self._extra_params.image_topic,
29+
qos_profile=SENSOR_QOS,
3230
)
33-
qos_profile = qos_profile_factory(self._extra_params.camera_info_topic.qos)
3431
self._data_collector.subscribers["camera.image.camera_info"] = Subscriber(
3532
self,
3633
CameraInfo,
37-
self._extra_params.camera_info_topic.name,
38-
qos_profile=qos_profile,
34+
self._extra_params.camera_info_topic,
35+
qos_profile=SENSOR_QOS,
3936
)
40-
qos_profile = qos_profile_factory(self._extra_params.depth_topic.qos)
4137
self._data_collector.subscribers["camera.depth"] = Subscriber(
4238
self,
4339
(
4440
CompressedImage
45-
if "compressed" in self._extra_params.depth_topic.name
41+
if "compressed" in self._extra_params.depth_topic
4642
else Image
4743
),
48-
self._extra_params.depth_topic.name,
49-
qos_profile=qos_profile,
50-
)
51-
qos_profile = qos_profile_factory(
52-
self._extra_params.depth_camera_info_topic.qos
44+
self._extra_params.depth_topic,
45+
qos_profile=SENSOR_QOS,
5346
)
5447
self._data_collector.subscribers["camera.depth.camera_info"] = Subscriber(
5548
self,
5649
CameraInfo,
57-
self._extra_params.depth_camera_info_topic.name,
58-
qos_profile=qos_profile,
50+
self._extra_params.depth_camera_info_topic,
51+
qos_profile=SENSOR_QOS,
5952
)
60-
qos_profile = qos_profile_factory(self._extra_params.joint_state_topic.qos)
6153
self._data_collector.subscribers["joint_states"] = Subscriber(
6254
self,
6355
JointState,
64-
self._extra_params.joint_state_topic.name,
65-
qos_profile=qos_profile,
56+
self._extra_params.joint_state_topic,
57+
qos_profile=JOINT_STATES_QOS,
6658
)
6759

6860
def _declare_extra_parameters(self):
6961
self.declare_parameters(
7062
namespace="",
7163
parameters=[
72-
("topics.image.name", "/camera/image_rect_color"),
73-
("topics.image.qos.reliability", "BEST_EFFORT"),
74-
("topics.image.qos.durability", "VOLATILE"),
75-
(
76-
"topics.image.camera_info.name",
77-
"/camera/image_rect_color/camera_info",
78-
),
79-
("topics.image.camera_info.qos.reliability", "BEST_EFFORT"),
80-
("topics.image.camera_info.qos.durability", "VOLATILE"),
81-
("topics.depth.name", "/camera/depth_registered"),
82-
("topics.depth.qos.reliability", "BEST_EFFORT"),
83-
("topics.depth.qos.durability", "VOLATILE"),
84-
(
85-
"topics.depth.camera_info.name",
86-
"/camera/depth_registered/camera_info",
87-
),
88-
("topics.depth.camera_info.qos.reliability", "BEST_EFFORT"),
89-
("topics.depth.camera_info.qos.durability", "VOLATILE"),
90-
("topics.joint_state.name", "/joint_states"),
91-
("topics.joint_state.qos.reliability", "BEST_EFFORT"),
92-
("topics.joint_state.qos.durability", "VOLATILE"),
64+
("topics.image", "/camera/image_rect_color"),
65+
("topics.camera_info", "/camera/image_rect_color/camera_info"),
66+
("topics.depth", "/camera/depth_registered"),
67+
("topics.depth_camera_info", "/camera/depth_registered/camera_info"),
68+
("topics.joint_state", "/joint_states"),
9369
],
9470
)
9571

9672
def _get_extra_parameters(self):
9773
self._extra_params = self._ExtraParams(
98-
image_topic=TopicParams(
99-
name=self.get_parameter("topics.image.name")
100-
.get_parameter_value()
101-
.string_value,
102-
qos=QoSParams(
103-
reliability=self.get_parameter("topics.image.qos.reliability")
104-
.get_parameter_value()
105-
.string_value,
106-
durability=self.get_parameter("topics.image.qos.durability")
107-
.get_parameter_value()
108-
.string_value,
109-
),
110-
),
111-
camera_info_topic=TopicParams(
112-
name=self.get_parameter("topics.image.camera_info.name")
113-
.get_parameter_value()
114-
.string_value,
115-
qos=QoSParams(
116-
reliability=self.get_parameter(
117-
"topics.image.camera_info.qos.reliability"
118-
)
119-
.get_parameter_value()
120-
.string_value,
121-
durability=self.get_parameter(
122-
"topics.image.camera_info.qos.durability"
123-
)
124-
.get_parameter_value()
125-
.string_value,
126-
),
127-
),
128-
depth_topic=TopicParams(
129-
name=self.get_parameter("topics.depth.name")
130-
.get_parameter_value()
131-
.string_value,
132-
qos=QoSParams(
133-
reliability=self.get_parameter("topics.depth.qos.reliability")
134-
.get_parameter_value()
135-
.string_value,
136-
durability=self.get_parameter("topics.depth.qos.durability")
137-
.get_parameter_value()
138-
.string_value,
139-
),
140-
),
141-
depth_camera_info_topic=TopicParams(
142-
name=self.get_parameter("topics.depth.camera_info.name")
143-
.get_parameter_value()
144-
.string_value,
145-
qos=QoSParams(
146-
reliability=self.get_parameter(
147-
"topics.depth.camera_info.qos.reliability"
148-
)
149-
.get_parameter_value()
150-
.string_value,
151-
durability=self.get_parameter(
152-
"topics.depth.camera_info.qos.durability"
153-
)
154-
.get_parameter_value()
155-
.string_value,
156-
),
157-
),
158-
joint_state_topic=TopicParams(
159-
name=self.get_parameter("topics.joint_state.name")
160-
.get_parameter_value()
161-
.string_value,
162-
qos=QoSParams(
163-
reliability=self.get_parameter("topics.joint_state.qos.reliability")
164-
.get_parameter_value()
165-
.string_value,
166-
durability=self.get_parameter("topics.joint_state.qos.durability")
167-
.get_parameter_value()
168-
.string_value,
169-
),
170-
),
74+
image_topic=self.get_parameter("topics.image")
75+
.get_parameter_value()
76+
.string_value,
77+
camera_info_topic=self.get_parameter("topics.camera_info")
78+
.get_parameter_value()
79+
.string_value,
80+
depth_topic=self.get_parameter("topics.depth")
81+
.get_parameter_value()
82+
.string_value,
83+
depth_camera_info_topic=self.get_parameter("topics.depth_camera_info")
84+
.get_parameter_value()
85+
.string_value,
86+
joint_state_topic=self.get_parameter("topics.joint_state")
87+
.get_parameter_value()
88+
.string_value,
17189
)
17290

17391
def _on_set_extra_parameters_impl(
17492
self, paramaters: list[Parameter]
17593
) -> SetParametersResult:
17694
result = SetParametersResult(successful=True)
17795
for parameter in paramaters:
178-
if parameter.name == "topics.joint_state.name":
96+
if parameter.name == "topics.joint_state":
17997
self.get_logger().info(
18098
f"Setting joint state topic to {parameter.value}"
18199
)
182-
self._extra_params.joint_state_topic.name = parameter.value
100+
self._extra_params.joint_state_topic = parameter.value
183101
self._reload_synced_subscribers()
184102
else:
185103
continue

‎roboreg_base/roboreg_base/parameters.py‎

Lines changed: 1 addition & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
from dataclasses import dataclass, field
1+
from dataclasses import dataclass
22

33

44
@dataclass
@@ -17,12 +17,6 @@ class RobotDataParams:
1717
collision_meshes: bool = False
1818

1919

20-
@dataclass
21-
class QoSParams:
22-
reliability: str = "BEST_EFFORT"
23-
durability: str = "VOLATILE"
24-
25-
2620
@dataclass
2721
class TFBroadcasterParams:
2822
parent_frame: str
@@ -36,13 +30,3 @@ def __post_init__(self):
3630
raise ValueError("child_frame must not be empty.")
3731
if not self.target_child_frame:
3832
raise ValueError("target_child_frame must not be empty.")
39-
40-
41-
@dataclass
42-
class TopicParams:
43-
name: str
44-
qos: QoSParams = field(default_factory=QoSParams)
45-
46-
def __post_init__(self):
47-
if not self.name:
48-
raise ValueError("Topic name must not be empty.")

‎roboreg_base/roboreg_base/qos_profile_factory.py‎

Lines changed: 0 additions & 15 deletions
This file was deleted.

‎roboreg_base/roboreg_base/qos_profiles.py‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
from rclpy.qos import (
2+
DurabilityPolicy,
3+
HistoryPolicy,
4+
QoSProfile,
5+
ReliabilityPolicy,
6+
qos_profile_sensor_data,
7+
)
8+
9+
SENSOR_QOS = qos_profile_sensor_data
10+
JOINT_STATES_QOS = qos_profile_sensor_data
11+
ROBOT_DESCRIPTION_QOS = QoSProfile(
12+
history=HistoryPolicy.KEEP_LAST,
13+
depth=1,
14+
reliability=ReliabilityPolicy.RELIABLE,
15+
durability=DurabilityPolicy.TRANSIENT_LOCAL,
16+
)

‎roboreg_base/roboreg_base/roboreg_node.py‎

Lines changed: 9 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -15,12 +15,10 @@
1515
from .data.synchronized_collector import SynchronizedCollector
1616
from .parameters import (
1717
FilterParams,
18-
QoSParams,
1918
RobotDataParams,
2019
TFBroadcasterParams,
21-
TopicParams,
2220
)
23-
from .qos_profile_factory import qos_profile_factory
21+
from .qos_profiles import ROBOT_DESCRIPTION_QOS
2422

2523

2624
class RoboregNode(Node, ABC):
@@ -64,12 +62,11 @@ def __init__(self, node_name: str) -> None:
6462
def _create_robot_description_sub(self) -> None:
6563
if self._robot_description_sub is not None:
6664
self.destroy_subscription(self._robot_description_sub)
67-
qos_profile = qos_profile_factory(self._robot_description_topic.qos)
6865
self._robot_description_sub = self.create_subscription(
6966
String,
70-
self._robot_description_topic.name,
67+
self._robot_description_topic,
7168
self._on_robot_description,
72-
qos_profile,
69+
ROBOT_DESCRIPTION_QOS,
7370
)
7471

7572
def _on_tf_broadcast(self, _, res: Trigger.Response) -> Trigger.Response:
@@ -144,9 +141,7 @@ def _declare_common_parameters(self) -> None:
144141
self.declare_parameters(
145142
namespace="",
146143
parameters=[
147-
("topics.robot_description.name", "/robot_description"),
148-
("topics.robot_description.qos.reliability", "RELIABLE"),
149-
("topics.robot_description.qos.durability", "TRANSIENT_LOCAL"),
144+
("topics.robot_description", "/robot_description"),
150145
],
151146
)
152147
self.declare_parameters(
@@ -173,20 +168,10 @@ def _declare_common_parameters(self) -> None:
173168
)
174169

175170
def _get_common_parameters(self) -> None:
176-
self._robot_description_topic = TopicParams(
177-
name=self.get_parameter("topics.robot_description.name")
171+
self._robot_description_topic = (
172+
self.get_parameter("topics.robot_description")
178173
.get_parameter_value()
179-
.string_value,
180-
qos=QoSParams(
181-
reliability=self.get_parameter(
182-
"topics.robot_description.qos.reliability"
183-
)
184-
.get_parameter_value()
185-
.string_value,
186-
durability=self.get_parameter("topics.robot_description.qos.durability")
187-
.get_parameter_value()
188-
.string_value,
189-
),
174+
.string_value
190175
)
191176
self._filter_params = FilterParams(
192177
sync_accuracy=self.get_parameter("filters.sync_accuracy")
@@ -221,11 +206,11 @@ def _on_set_common_parameters_impl(
221206
) -> SetParametersResult:
222207
result = SetParametersResult(successful=True)
223208
for parameter in paramaters:
224-
if parameter.name == "topics.robot_description.name":
209+
if parameter.name == "topics.robot_description":
225210
self.get_logger().info(
226211
f"Setting robot description topic to {parameter.value}"
227212
)
228-
self._robot_description_topic.name = parameter.value
213+
self._robot_description_topic = parameter.value
229214
self._create_robot_description_sub()
230215
else:
231216
continue

0 commit comments

Comments
 (0)