Skip to content

Commit 372f8aa

Browse files
fferrifferri
committed
add qos param: createPublisher/createSubscription
1 parent 9f19bda commit 372f8aa

File tree

4 files changed

+112
-10
lines changed

4 files changed

+112
-10
lines changed

‎meta/callbacks.xml

+44-6
Original file line numberDiff line numberDiff line change
@@ -21,8 +21,11 @@
2121
<param name="topicCallback" type="func">
2222
<description>callback function, which will be called with a single argument of type table containing the message payload, e.g.: {linear={x=1.5, y=0.0, z=0.0}, angular={x=0.0, y=0.0, z=-2.3}}</description>
2323
</param>
24-
<param name="queueSize" type="int" default="1">
25-
<description>(optional) queue size</description>
24+
<param name="unused" type="int" default="0">
25+
<description>unused, set to zero</description>
26+
</param>
27+
<param name="qos" type="simros2_qos" nullable="true" default="nil">
28+
<description>QoS settings, see <struct-ref name="simros2_qos" /></description>
2629
</param>
2730
</params>
2831
<return>
@@ -78,11 +81,14 @@
7881
<param name="topicType" type="string">
7982
<description>topic type, e.g.: 'geometry_msgs/msg/Twist'</description>
8083
</param>
81-
<param name="queueSize" type="int" default="1">
82-
<description>(optional) queue size</description>
84+
<param name="unused" type="int" default="0">
85+
<description>unused, set to zero</description>
86+
</param>
87+
<param name="unused2" type="bool" default="false">
88+
<description>unused, set to false</description>
8389
</param>
84-
<param name="latch" type="bool" default="false">
85-
<description>(optional) latch topic</description>
90+
<param name="qos" type="simros2_qos" nullable="true" default="nil">
91+
<description>QoS settings, see <struct-ref name="simros2_qos" /></description>
8692
</param>
8793
</params>
8894
<return>
@@ -1117,4 +1123,36 @@
11171123
<item name="reject" />
11181124
<item name="accept" />
11191125
</enum>
1126+
<enum name="qos_history_policy" item-prefix="qos_history_policy_" base="12300">
1127+
<item name="system_default" />
1128+
<item name="keep_last" />
1129+
<item name="keep_all" />
1130+
</enum>
1131+
<enum name="qos_reliability_policy" item-prefix="qos_reliability_policy_" base="12400">
1132+
<item name="system_default" />
1133+
<item name="reliable" />
1134+
<item name="best_effort" />
1135+
</enum>
1136+
<enum name="qos_durability_policy" item-prefix="qos_durability_policy_" base="12500">
1137+
<item name="system_default" />
1138+
<item name="transient_local" />
1139+
<item name="volatile" />
1140+
</enum>
1141+
<enum name="qos_liveliness_policy" item-prefix="qos_liveliness_policy_" base="12600">
1142+
<item name="system_default" />
1143+
<item name="automatic" />
1144+
<item name="manual_by_topic" />
1145+
</enum>
1146+
<struct name="simros2_qos">
1147+
<description>QoS data structure</description>
1148+
<param name="history" type="int" default="simros2_qos_history_policy_system_default"></param>
1149+
<param name="depth" type="int" default="10"></param>
1150+
<param name="reliability" type="int" default="simros2_qos_reliability_policy_system_default"></param>
1151+
<param name="durability" type="int" default="simros2_qos_durability_policy_system_default"></param>
1152+
<param name="deadline" type="simros2_time"></param>
1153+
<param name="lifespan" type="simros2_time"></param>
1154+
<param name="liveliness" type="int" default="simros2_qos_liveliness_policy_system_default"></param>
1155+
<param name="liveliness_lease_duration" type="simros2_time"></param>
1156+
<param name="avoid_ros_namespace_conventions" type="bool" default="false"></param>
1157+
</struct>
11201158
</plugin>

‎src/sim_ros2_interface.cpp

+66
Original file line numberDiff line numberDiff line change
@@ -198,6 +198,72 @@ class Plugin : public sim::Plugin
198198
}
199199
}
200200

201+
rclcpp::QoS get_qos(const std::optional<simros2_qos> &opt_qos)
202+
{
203+
if(!opt_qos.has_value())
204+
return rclcpp::QoS {10};
205+
206+
const simros2_qos &qos = opt_qos.value();
207+
rmw_qos_profile_t profile;
208+
switch(qos.history)
209+
{
210+
case simros2_qos_history_policy_system_default:
211+
profile.history = RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT;
212+
break;
213+
case simros2_qos_history_policy_keep_last:
214+
profile.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST;
215+
break;
216+
case simros2_qos_history_policy_keep_all:
217+
profile.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL;
218+
break;
219+
}
220+
profile.depth = qos.depth;
221+
switch(qos.reliability)
222+
{
223+
case simros2_qos_reliability_policy_system_default:
224+
profile.reliability = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
225+
break;
226+
case simros2_qos_reliability_policy_reliable:
227+
profile.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
228+
break;
229+
case simros2_qos_reliability_policy_best_effort:
230+
profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
231+
break;
232+
}
233+
switch(qos.durability)
234+
{
235+
case simros2_qos_durability_policy_system_default:
236+
profile.durability = RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT;
237+
break;
238+
case simros2_qos_durability_policy_transient_local:
239+
profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
240+
break;
241+
case simros2_qos_durability_policy_volatile:
242+
profile.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
243+
break;
244+
}
245+
profile.deadline.sec = qos.deadline.sec;
246+
profile.deadline.nsec = qos.deadline.nanosec;
247+
profile.lifespan.sec = qos.lifespan.sec;
248+
profile.lifespan.nsec = qos.lifespan.nanosec;
249+
switch(qos.liveliness)
250+
{
251+
case simros2_qos_liveliness_policy_system_default:
252+
profile.liveliness = RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT;
253+
break;
254+
case simros2_qos_liveliness_policy_automatic:
255+
profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC;
256+
break;
257+
case simros2_qos_liveliness_policy_manual_by_topic:
258+
profile.liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC;
259+
break;
260+
}
261+
profile.liveliness_lease_duration.sec = qos.liveliness_lease_duration.sec;
262+
profile.liveliness_lease_duration.nsec = qos.liveliness_lease_duration.nanosec;
263+
profile.avoid_ros_namespace_conventions = qos.avoid_ros_namespace_conventions;
264+
return rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(profile), profile);
265+
}
266+
201267
void createSubscription(createSubscription_in *in, createSubscription_out *out)
202268
{
203269
SubscriptionProxy *subscriptionProxy = new SubscriptionProxy();

‎templates/pub_new.cpp

+1-2
Original file line numberDiff line numberDiff line change
@@ -4,8 +4,7 @@
44
#py if interface.tag == 'msg':
55
else if(in->topicType == "`interface.full_name`")
66
{
7-
rclcpp::QoS qos = 10;
8-
publisherProxy->publisher = node->create_publisher<`interface.cpp_type`>(in->topicName, qos);
7+
publisherProxy->publisher = node->create_publisher<`interface.cpp_type`>(in->topicName, get_qos(in->qos));
98
}
109
#py endif
1110
#py endfor

‎templates/sub_new.cpp

+1-2
Original file line numberDiff line numberDiff line change
@@ -5,8 +5,7 @@
55
else if(in->topicType == "`interface.full_name`")
66
{
77
auto cb = [=](const `interface.cpp_type`::SharedPtr msg) { ros_callback__`interface.cpp_type_normalized`(msg, subscriptionProxy); };
8-
rclcpp::QoS qos = 10;
9-
subscriptionProxy->subscription = node->create_subscription<`interface.cpp_type`>(in->topicName, qos, cb);
8+
subscriptionProxy->subscription = node->create_subscription<`interface.cpp_type`>(in->topicName, get_qos(in->qos), cb);
109
}
1110
#py endif
1211
#py endfor

0 commit comments

Comments
 (0)