niklas/rmw_cyclonedds
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

update to match ROS2 Dashing interface

Browse source 
passes a decent subset of the tests ...

fixes:
* sequences of simple types: remove accidental alignment
* trigger graph guard on any built-in topic
* create a participant for each node, with node name/namespace in user data

It is still only a proof-of-concept, but it might now actually be usable ...
This commit is contained in:
Erik Boasson 2019-05-12 13:52:26 +02:00
parent e520cb4d63
commit 24726b4685
4 changed files with 328 additions and 67 deletions
Download patch file Download diff file Expand all files Collapse all files

15
rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/MessageTypeSupport_impl.hpp
View file

@ -17,6 +17,8 @@
#include <cassert> #include <cassert>
#include <memory> #include <memory>
#include <regex>
#include <sstream>
#include <string> #include <string>
#include "rmw_cyclonedds_cpp/MessageTypeSupport.hpp" #include "rmw_cyclonedds_cpp/MessageTypeSupport.hpp"
@ -31,9 +33,16 @@ MessageTypeSupport<MembersType>::MessageTypeSupport(const MembersType * members)
assert(members); assert(members);
this->members_ = members; this->members_ = members;
std::string name = std::string(members->package_name_) + "::msg::dds_::" + std::ostringstream ss;
members->message_name_ + "_"; std::string message_namespace(this->members_->message_namespace_);
this->setName(name.c_str()); std::string message_name(this->members_->message_name_);
if (!message_namespace.empty()) {
// Find and replace C namespace separator with C++, in case this is using C typesupport
message_namespace = std::regex_replace(message_namespace, std::regex("__"), "::");
ss << message_namespace << "::";
}
ss << "dds_::" << message_name << "_";
this->setName(ss.str().c_str());
} }
} // namespace rmw_cyclonedds_cpp } // namespace rmw_cyclonedds_cpp

29
rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/ServiceTypeSupport_impl.hpp
View file

@ -16,6 +16,8 @@
#define RMW_CYCLONEDDS_CPP__SERVICETYPESUPPORT_IMPL_HPP_ #define RMW_CYCLONEDDS_CPP__SERVICETYPESUPPORT_IMPL_HPP_
#include <cassert> #include <cassert>
#include <regex>
#include <sstream>
#include <string> #include <string>
#include "rmw_cyclonedds_cpp/ServiceTypeSupport.hpp" #include "rmw_cyclonedds_cpp/ServiceTypeSupport.hpp"
@ -36,9 +38,16 @@ RequestTypeSupport<ServiceMembersType, MessageMembersType>::RequestTypeSupport(
assert(members); assert(members);
this->members_ = members->request_members_; this->members_ = members->request_members_;
std::string name = std::string(members->package_name_) + "::srv::dds_::" + std::ostringstream ss;
members->service_name_ + "_Request_"; std::string service_namespace(members->service_namespace_);
this->setName(name.c_str()); std::string service_name(members->service_name_);
if (!service_namespace.empty()) {
// Find and replace C namespace separator with C++, in case this is using C typesupport
service_namespace = std::regex_replace(service_namespace, std::regex("__"), "::");
ss << service_namespace << "::";
}
ss << "dds_::" << service_name << "_Request_";
this->setName(ss.str().c_str());
} }
template<typename ServiceMembersType, typename MessageMembersType> template<typename ServiceMembersType, typename MessageMembersType>
@ -48,9 +57,17 @@ ResponseTypeSupport<ServiceMembersType, MessageMembersType>::ResponseTypeSupport
assert(members); assert(members);
this->members_ = members->response_members_; this->members_ = members->response_members_;
std::string name = std::string(members->package_name_) + "::srv::dds_::" +
members->service_name_ + "_Response_"; std::ostringstream ss;
this->setName(name.c_str()); std::string service_namespace(members->service_namespace_);
std::string service_name(members->service_name_);
if (!service_namespace.empty()) {
// Find and replace C namespace separator with C++, in case this is using C typesupport
service_namespace = std::regex_replace(service_namespace, std::regex("__"), "::");
ss << service_namespace << "::";
}
ss << "dds_::" << service_name << "_Response_";
this->setName(ss.str().c_str());
} }
} // namespace rmw_cyclonedds_cpp } // namespace rmw_cyclonedds_cpp

26
rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdes.hpp
View file

@ -85,12 +85,14 @@ public:
} }
#define SIMPLEA(T) inline void serializeA (const T *x, size_t cnt) { \ #define SIMPLEA(T) inline void serializeA (const T *x, size_t cnt) { \
if ((off % sizeof (T)) != 0) { \ if (cnt > 0) { \
off += sizeof (T) - (off % sizeof (T)); \ if ((off % sizeof (T)) != 0) { \
off += sizeof (T) - (off % sizeof (T)); \
} \
resize (off + cnt * sizeof (T)); \
memcpy (data () + off, (void *) x, cnt * sizeof (T)); \
off += cnt * sizeof (T); \
} \ } \
resize (off + cnt * sizeof (T)); \
memcpy (data () + off, (void *) x, cnt * sizeof (T)); \
off += cnt * sizeof (T); \
} }
SIMPLEA (char); SIMPLEA (char);
SIMPLEA (int8_t); SIMPLEA (int8_t);
@ -111,7 +113,7 @@ public:
template<class T> inline void serialize (const std::vector<T>& x) template<class T> inline void serialize (const std::vector<T>& x)
{ {
serialize (static_cast<uint32_t> (x.size ())); serialize (static_cast<uint32_t> (x.size ()));
if (x.size () > 0) serializeA (x.data (), x.size ()); serializeA (x.data (), x.size ());
} }
inline void serialize (const std::vector<bool>& x) { inline void serialize (const std::vector<bool>& x) {
serialize (static_cast<uint32_t> (x.size ())); serialize (static_cast<uint32_t> (x.size ()));
@ -121,7 +123,7 @@ public:
template<class T> inline void serializeS (const T *x, size_t cnt) template<class T> inline void serializeS (const T *x, size_t cnt)
{ {
serialize (static_cast<uint32_t> (cnt)); serialize (static_cast<uint32_t> (cnt));
if (cnt > 0) serializeA (x, cnt); serializeA (x, cnt);
} }
private: private:
@ -192,9 +194,11 @@ public:
} }
#define SIMPLEA(T) inline void deserializeA (T *x, size_t cnt) { \ #define SIMPLEA(T) inline void deserializeA (T *x, size_t cnt) { \
align (sizeof (T)); \ if (cnt > 0) { \
memcpy ((void *) x, (void *) (data + pos), (cnt) * sizeof (T)); \ align (sizeof (T)); \
pos += (cnt) * sizeof (T); \ memcpy ((void *) x, (void *) (data + pos), (cnt) * sizeof (T)); \
pos += (cnt) * sizeof (T); \
} \
} }
SIMPLEA (char); SIMPLEA (char);
SIMPLEA (int8_t); SIMPLEA (int8_t);
@ -215,7 +219,7 @@ public:
template<class T> inline void deserialize (std::vector<T>& x) { template<class T> inline void deserialize (std::vector<T>& x) {
const uint32_t sz = deserialize32 (); const uint32_t sz = deserialize32 ();
x.resize (sz); x.resize (sz);
if (sz > 0) deserializeA (x.data (), sz); deserializeA (x.data (), sz);
} }
inline void deserialize (std::vector<bool>& x) { inline void deserialize (std::vector<bool>& x) {
const uint32_t sz = deserialize32 (); const uint32_t sz = deserialize32 ();

325
rmw_cyclonedds_cpp/src/rmw_node.cpp
View file

@ -40,6 +40,7 @@
#include <map> #include <map>
#include <set> #include <set>
#include <atomic> #include <atomic>
#include <regex>
#include "rcutils/logging_macros.h" #include "rcutils/logging_macros.h"
#include "rcutils/strdup.h" #include "rcutils/strdup.h"
@ -97,6 +98,7 @@ public:
}; };
struct CddsNode { struct CddsNode {
dds_entity_t pp;
rmw_guard_condition_t *graph_guard_condition; rmw_guard_condition_t *graph_guard_condition;
}; };
@ -166,6 +168,26 @@ extern "C" const char *rmw_get_serialization_format()
return eclipse_cyclonedds_serialization_format; return eclipse_cyclonedds_serialization_format;
} }
extern "C" rmw_ret_t rmw_set_log_severity (rmw_log_severity_t severity __attribute__ ((unused)))
{
RMW_SET_ERROR_MSG ("unimplemented");
return RMW_RET_ERROR;
}
extern "C" rmw_ret_t rmw_context_fini (rmw_context_t *context)
{
RCUTILS_CHECK_ARGUMENT_FOR_NULL (context, RMW_RET_INVALID_ARGUMENT);
RMW_CHECK_TYPE_IDENTIFIERS_MATCH (
context,
context->implementation_identifier,
eclipse_cyclonedds_identifier,
return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
// context impl is explicitly supposed to be nullptr for now, see rmw_init's code
// RCUTILS_CHECK_ARGUMENT_FOR_NULL(context->impl, RMW_RET_INVALID_ARGUMENT);
*context = rmw_get_zero_initialized_context ();
return RMW_RET_OK;
}
extern "C" rmw_ret_t rmw_init_options_init (rmw_init_options_t *init_options, rcutils_allocator_t allocator) extern "C" rmw_ret_t rmw_init_options_init (rmw_init_options_t *init_options, rcutils_allocator_t allocator)
{ {
RMW_CHECK_ARGUMENT_FOR_NULL (init_options, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL (init_options, RMW_RET_INVALID_ARGUMENT);
@ -226,6 +248,12 @@ extern "C" rmw_ret_t rmw_init (const rmw_init_options_t *options __attribute__ (
return RMW_RET_OK; return RMW_RET_OK;
} }
extern "C" rmw_ret_t rmw_node_assert_liveliness (const rmw_node_t *node)
{
RET_WRONG_IMPLID (node);
return RMW_RET_OK;
}
extern "C" rmw_ret_t rmw_shutdown (rmw_context_t *context) extern "C" rmw_ret_t rmw_shutdown (rmw_context_t *context)
{ {
RCUTILS_CHECK_ARGUMENT_FOR_NULL (context, RMW_RET_INVALID_ARGUMENT); RCUTILS_CHECK_ARGUMENT_FOR_NULL (context, RMW_RET_INVALID_ARGUMENT);
@ -268,6 +296,17 @@ static void unref_ppant ()
/////////// /////////// /////////// ///////////
///////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////
static void ggcallback (dds_entity_t rd, void *varg)
{
auto node_impl = static_cast<CddsNode *> (varg);
void *msg = 0;
dds_sample_info_t info;
while (dds_take_mask (rd, &msg, &info, 1, 1, DDS_ANY_SAMPLE_STATE | DDS_ANY_VIEW_STATE | DDS_NOT_ALIVE_DISPOSED_INSTANCE_STATE | DDS_NOT_ALIVE_NO_WRITERS_INSTANCE_STATE) > 0) {
dds_return_loan (rd, &msg, 1);
}
(void) rmw_trigger_guard_condition (node_impl->graph_guard_condition);
}
extern "C" rmw_node_t *rmw_create_node (rmw_context_t *context __attribute__ ((unused)), const char *name, const char *namespace_, size_t domain_id, const rmw_node_security_options_t *security_options) extern "C" rmw_node_t *rmw_create_node (rmw_context_t *context __attribute__ ((unused)), const char *name, const char *namespace_, size_t domain_id, const rmw_node_security_options_t *security_options)
{ {
RET_NULL_X (name, return nullptr); RET_NULL_X (name, return nullptr);
@ -275,19 +314,40 @@ extern "C" rmw_node_t *rmw_create_node (rmw_context_t *context __attribute__ ((u
(void) domain_id; (void) domain_id;
(void) security_options; (void) security_options;
dds_entity_t pp = ref_ppant (); dds_qos_t *qos = dds_create_qos ();
std::string user_data = (std::string ("ros2_name=") + std::string (name) +
std::string (";ros2_namespace=") + std::string (namespace_));
dds_qset_userdata (qos, user_data.c_str (), user_data.size ());
dds_entity_t pp = dds_create_participant (DDS_DOMAIN_DEFAULT, qos, nullptr);
dds_delete_qos (qos);
if (pp < 0) { if (pp < 0) {
return nullptr; return nullptr;
} }
auto *node_impl = new CddsNode (); auto *node_impl = new CddsNode ();
rmw_node_t *node_handle = nullptr; rmw_node_t *node_handle = nullptr;
dds_listener_t *gglistener = nullptr;
RET_ALLOC_X (node_impl, goto fail_node_impl); RET_ALLOC_X (node_impl, goto fail_node_impl);
rmw_guard_condition_t *graph_guard_condition; rmw_guard_condition_t *graph_guard_condition;
if (!(graph_guard_condition = rmw_create_guard_condition (context))) { if (!(graph_guard_condition = rmw_create_guard_condition (context))) {
goto fail_ggc; goto fail_ggc;
} }
node_impl->pp = pp;
node_impl->graph_guard_condition = graph_guard_condition; node_impl->graph_guard_condition = graph_guard_condition;
//
static const dds_entity_t bts[] = {
DDS_BUILTIN_TOPIC_DCPSPARTICIPANT,
DDS_BUILTIN_TOPIC_DCPSSUBSCRIPTION,
DDS_BUILTIN_TOPIC_DCPSPUBLICATION
};
gglistener = dds_create_listener (node_impl);
dds_lset_data_available (gglistener, ggcallback);
for (size_t i = 0; i < sizeof (bts) / sizeof (bts[0]); i++) {
dds_create_reader (pp, bts[i], NULL, gglistener);
}
dds_delete_listener (gglistener);
//
node_handle = rmw_node_allocate (); node_handle = rmw_node_allocate ();
RET_ALLOC_X (node_handle, goto fail_node_handle); RET_ALLOC_X (node_handle, goto fail_node_handle);
node_handle->implementation_identifier = eclipse_cyclonedds_identifier; node_handle->implementation_identifier = eclipse_cyclonedds_identifier;
@ -317,7 +377,7 @@ extern "C" rmw_node_t *rmw_create_node (rmw_context_t *context __attribute__ ((u
fail_ggc: fail_ggc:
delete node_impl; delete node_impl;
fail_node_impl: fail_node_impl:
unref_ppant (); dds_delete (pp);
return nullptr; return nullptr;
} }
@ -334,8 +394,8 @@ extern "C" rmw_ret_t rmw_destroy_node (rmw_node_t *node)
RMW_SET_ERROR_MSG ("failed to destroy graph guard condition"); RMW_SET_ERROR_MSG ("failed to destroy graph guard condition");
result_ret = RMW_RET_ERROR; result_ret = RMW_RET_ERROR;
} }
dds_delete (node_impl->pp);
delete node_impl; delete node_impl;
unref_ppant ();
return result_ret; return result_ret;
} }
@ -356,6 +416,12 @@ extern "C" const rmw_guard_condition_t *rmw_node_get_graph_guard_condition (cons
using MessageTypeSupport_c = rmw_cyclonedds_cpp::MessageTypeSupport<rosidl_typesupport_introspection_c__MessageMembers>; using MessageTypeSupport_c = rmw_cyclonedds_cpp::MessageTypeSupport<rosidl_typesupport_introspection_c__MessageMembers>;
using MessageTypeSupport_cpp = rmw_cyclonedds_cpp::MessageTypeSupport<rosidl_typesupport_introspection_cpp::MessageMembers>; using MessageTypeSupport_cpp = rmw_cyclonedds_cpp::MessageTypeSupport<rosidl_typesupport_introspection_cpp::MessageMembers>;
extern "C" rmw_ret_t rmw_get_serialized_message_size (const rosidl_message_type_support_t *type_support __attribute__ ((unused)), const rosidl_message_bounds_t *message_bounds __attribute__ ((unused)), size_t *size __attribute__ ((unused)))
{
RMW_SET_ERROR_MSG ("rmw_get_serialized_message_size: unimplemented");
return RMW_RET_ERROR;
}
extern "C" rmw_ret_t rmw_serialize (const void *ros_message, const rosidl_message_type_support_t *type_support, rmw_serialized_message_t *serialized_message) extern "C" rmw_ret_t rmw_serialize (const void *ros_message, const rosidl_message_type_support_t *type_support, rmw_serialized_message_t *serialized_message)
{ {
std::vector<unsigned char> data; std::vector<unsigned char> data;
@ -409,7 +475,7 @@ extern "C" rmw_ret_t rmw_deserialize (const rmw_serialized_message_t *serialized
/////////// /////////// /////////// ///////////
///////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////
extern "C" rmw_ret_t rmw_publish (const rmw_publisher_t *publisher, const void *ros_message) extern "C" rmw_ret_t rmw_publish (const rmw_publisher_t *publisher, const void *ros_message, rmw_publisher_allocation_t *allocation __attribute__ ((unused)))
{ {
RET_WRONG_IMPLID (publisher); RET_WRONG_IMPLID (publisher);
RET_NULL (ros_message); RET_NULL (ros_message);
@ -418,14 +484,12 @@ extern "C" rmw_ret_t rmw_publish (const rmw_publisher_t *publisher, const void *
if (dds_write (pub->pubh, ros_message) >= 0) { if (dds_write (pub->pubh, ros_message) >= 0) {
return RMW_RET_OK; return RMW_RET_OK;
} else { } else {
/* FIXME: what is the expected behavior when it times out? */ RMW_SET_ERROR_MSG ("failed to publish data");
RMW_SET_ERROR_MSG ("cannot publish data"); return RMW_RET_ERROR;
//return RMW_RET_ERROR;
return RMW_RET_OK;
} }
} }
extern "C" rmw_ret_t rmw_publish_serialized_message (const rmw_publisher_t *publisher, const rmw_serialized_message_t *serialized_message) extern "C" rmw_ret_t rmw_publish_serialized_message (const rmw_publisher_t *publisher, const rmw_serialized_message_t *serialized_message, rmw_publisher_allocation_t *allocation __attribute__ ((unused)))
{ {
RET_WRONG_IMPLID (publisher); RET_WRONG_IMPLID (publisher);
RET_NULL (serialized_message); RET_NULL (serialized_message);
@ -467,6 +531,7 @@ static dds_qos_t *create_readwrite_qos (const rmw_qos_profile_t *qos_policies, b
dds_qos_t *qos = dds_create_qos (); dds_qos_t *qos = dds_create_qos ();
switch (qos_policies->history) { switch (qos_policies->history) {
case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT:
case RMW_QOS_POLICY_HISTORY_UNKNOWN:
case RMW_QOS_POLICY_HISTORY_KEEP_LAST: case RMW_QOS_POLICY_HISTORY_KEEP_LAST:
if (qos_policies->depth > INT32_MAX) { if (qos_policies->depth > INT32_MAX) {
RMW_SET_ERROR_MSG ("unsupported history depth"); RMW_SET_ERROR_MSG ("unsupported history depth");
@ -481,8 +546,9 @@ static dds_qos_t *create_readwrite_qos (const rmw_qos_profile_t *qos_policies, b
} }
switch (qos_policies->reliability) { switch (qos_policies->reliability) {
case RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT: case RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT:
case RMW_QOS_POLICY_RELIABILITY_UNKNOWN:
case RMW_QOS_POLICY_RELIABILITY_RELIABLE: case RMW_QOS_POLICY_RELIABILITY_RELIABLE:
dds_qset_reliability (qos, DDS_RELIABILITY_RELIABLE, DDS_SECS (1)); dds_qset_reliability (qos, DDS_RELIABILITY_RELIABLE, DDS_INFINITY);
break; break;
case RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT: case RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT:
dds_qset_reliability (qos, DDS_RELIABILITY_BEST_EFFORT, 0); dds_qset_reliability (qos, DDS_RELIABILITY_BEST_EFFORT, 0);
@ -490,6 +556,7 @@ static dds_qos_t *create_readwrite_qos (const rmw_qos_profile_t *qos_policies, b
} }
switch (qos_policies->durability) { switch (qos_policies->durability) {
case RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT: case RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT:
case RMW_QOS_POLICY_DURABILITY_UNKNOWN:
case RMW_QOS_POLICY_DURABILITY_VOLATILE: case RMW_QOS_POLICY_DURABILITY_VOLATILE:
dds_qset_durability (qos, DDS_DURABILITY_VOLATILE); dds_qset_durability (qos, DDS_DURABILITY_VOLATILE);
break; break;
@ -503,6 +570,77 @@ static dds_qos_t *create_readwrite_qos (const rmw_qos_profile_t *qos_policies, b
return qos; return qos;
} }
static bool get_readwrite_qos (dds_entity_t handle, rmw_qos_profile_t *qos_policies)
{
dds_qos_t *qos = dds_create_qos ();
if (dds_get_qos (handle, qos) < 0) {
RMW_SET_ERROR_MSG ("get_readwrite_qos: invalid handle");
goto error;
}
{
dds_history_kind_t kind;
int32_t depth;
if (!dds_qget_history (qos, &kind, &depth)) {
RMW_SET_ERROR_MSG ("get_readwrite_qos: history not set");
goto error;
}
switch (kind) {
case DDS_HISTORY_KEEP_LAST:
qos_policies->history = RMW_QOS_POLICY_HISTORY_KEEP_LAST;
qos_policies->depth = (uint32_t) depth;
break;
case DDS_HISTORY_KEEP_ALL:
qos_policies->history = RMW_QOS_POLICY_HISTORY_KEEP_ALL;
qos_policies->depth = 0;
break;
}
}
{
dds_reliability_kind_t kind;
dds_duration_t max_blocking_time;
if (!dds_qget_reliability (qos, &kind, &max_blocking_time)) {
RMW_SET_ERROR_MSG ("get_readwrite_qos: history not set");
goto error;
}
switch (kind) {
case DDS_RELIABILITY_BEST_EFFORT:
qos_policies->reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
break;
case DDS_RELIABILITY_RELIABLE:
qos_policies->reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
break;
}
}
{
dds_durability_kind_t kind;
if (!dds_qget_durability (qos, &kind)){
RMW_SET_ERROR_MSG ("get_readwrite_qos: durability not set");
goto error;
}
switch (kind)
{
case DDS_DURABILITY_VOLATILE:
qos_policies->durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
break;
case DDS_DURABILITY_TRANSIENT_LOCAL:
qos_policies->durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
break;
case DDS_DURABILITY_TRANSIENT:
case DDS_DURABILITY_PERSISTENT:
qos_policies->durability = RMW_QOS_POLICY_DURABILITY_UNKNOWN;
break;
}
}
dds_delete_qos (qos);
return true;
error:
dds_delete_qos (qos);
return false;
}
static CddsPublisher *create_cdds_publisher (const rmw_node_t *node, const rosidl_message_type_support_t *type_supports, const char *topic_name, const rmw_qos_profile_t *qos_policies) static CddsPublisher *create_cdds_publisher (const rmw_node_t *node, const rosidl_message_type_support_t *type_supports, const char *topic_name, const rmw_qos_profile_t *qos_policies)
{ {
RET_WRONG_IMPLID_X (node, return nullptr); RET_WRONG_IMPLID_X (node, return nullptr);
@ -554,6 +692,18 @@ static CddsPublisher *create_cdds_publisher (const rmw_node_t *node, const rosid
return nullptr; return nullptr;
} }
extern "C" rmw_ret_t rmw_init_publisher_allocation (const rosidl_message_type_support_t *type_support __attribute__ ((unused)), const rosidl_message_bounds_t *message_bounds __attribute__ ((unused)), rmw_publisher_allocation_t *allocation __attribute__ ((unused)))
{
RMW_SET_ERROR_MSG ("rmw_init_publisher_allocation: unimplemented");
return RMW_RET_ERROR;
}
extern "C" rmw_ret_t rmw_fini_publisher_allocation (rmw_publisher_allocation_t *allocation __attribute__ ((unused)))
{
RMW_SET_ERROR_MSG ("rmw_fini_publisher_allocation: unimplemented");
return RMW_RET_ERROR;
}
extern "C" rmw_publisher_t *rmw_create_publisher (const rmw_node_t *node, const rosidl_message_type_support_t *type_supports, const char *topic_name, const rmw_qos_profile_t *qos_policies) extern "C" rmw_publisher_t *rmw_create_publisher (const rmw_node_t *node, const rosidl_message_type_support_t *type_supports, const char *topic_name, const rmw_qos_profile_t *qos_policies)
{ {
CddsPublisher *pub; CddsPublisher *pub;
@ -617,6 +767,24 @@ extern "C" rmw_ret_t rmw_publisher_count_matched_subscriptions (const rmw_publis
} }
} }
rmw_ret_t rmw_publisher_assert_liveliness (const rmw_publisher_t *publisher)
{
RET_WRONG_IMPLID (publisher);
return RMW_RET_OK;
}
rmw_ret_t rmw_publisher_get_actual_qos (const rmw_publisher_t *publisher, rmw_qos_profile_t *qos)
{
RET_NULL (qos);
RET_WRONG_IMPLID (publisher);
auto pub = static_cast<CddsPublisher *> (publisher->data);
if (get_readwrite_qos (pub->pubh, qos)) {
return RMW_RET_OK;
} else {
return RMW_RET_ERROR;
}
}
extern "C" rmw_ret_t rmw_destroy_publisher (rmw_node_t *node, rmw_publisher_t *publisher) extern "C" rmw_ret_t rmw_destroy_publisher (rmw_node_t *node, rmw_publisher_t *publisher)
{ {
RET_WRONG_IMPLID (node); RET_WRONG_IMPLID (node);
@ -691,6 +859,18 @@ static CddsSubscription *create_cdds_subscription (const rmw_node_t *node, const
return nullptr; return nullptr;
} }
extern "C" rmw_ret_t rmw_init_subscription_allocation (const rosidl_message_type_support_t *type_support __attribute__ ((unused)), const rosidl_message_bounds_t *message_bounds __attribute__ ((unused)), rmw_subscription_allocation_t *allocation __attribute__ ((unused)))
{
RMW_SET_ERROR_MSG ("rmw_init_subscription_allocation: unimplemented");
return RMW_RET_ERROR;
}
extern "C" rmw_ret_t rmw_fini_subscription_allocation (rmw_subscription_allocation_t *allocation __attribute__ ((unused)))
{
RMW_SET_ERROR_MSG ("rmw_fini_subscription_allocation: unimplemented");
return RMW_RET_ERROR;
}
extern "C" rmw_subscription_t *rmw_create_subscription (const rmw_node_t *node, const rosidl_message_type_support_t *type_supports, const char *topic_name, const rmw_qos_profile_t *qos_policies, bool ignore_local_publications) extern "C" rmw_subscription_t *rmw_create_subscription (const rmw_node_t *node, const rosidl_message_type_support_t *type_supports, const char *topic_name, const rmw_qos_profile_t *qos_policies, bool ignore_local_publications)
{ {
CddsSubscription *sub; CddsSubscription *sub;
@ -815,26 +995,33 @@ static rmw_ret_t rmw_take_ser_int (const rmw_subscription_t *subscription, rmw_s
return RMW_RET_OK; return RMW_RET_OK;
} }
extern "C" rmw_ret_t rmw_take (const rmw_subscription_t *subscription, void *ros_message, bool *taken) extern "C" rmw_ret_t rmw_take (const rmw_subscription_t *subscription, void *ros_message, bool *taken, rmw_subscription_allocation_t *allocation __attribute__ ((unused)))
{ {
return rmw_take_int (subscription, ros_message, taken, nullptr); return rmw_take_int (subscription, ros_message, taken, nullptr);
} }
extern "C" rmw_ret_t rmw_take_with_info (const rmw_subscription_t *subscription, void *ros_message, bool *taken, rmw_message_info_t *message_info) extern "C" rmw_ret_t rmw_take_with_info (const rmw_subscription_t *subscription, void *ros_message, bool *taken, rmw_message_info_t *message_info, rmw_subscription_allocation_t *allocation __attribute__ ((unused)))
{ {
return rmw_take_int (subscription, ros_message, taken, message_info); return rmw_take_int (subscription, ros_message, taken, message_info);
} }
extern "C" rmw_ret_t rmw_take_serialized_message (const rmw_subscription_t *subscription, rmw_serialized_message_t *serialized_message, bool *taken) extern "C" rmw_ret_t rmw_take_serialized_message (const rmw_subscription_t *subscription, rmw_serialized_message_t *serialized_message, bool *taken, rmw_subscription_allocation_t *allocation __attribute__ ((unused)))
{ {
return rmw_take_ser_int (subscription, serialized_message, taken, nullptr); return rmw_take_ser_int (subscription, serialized_message, taken, nullptr);
} }
extern "C" rmw_ret_t rmw_take_serialized_message_with_info (const rmw_subscription_t *subscription, rmw_serialized_message_t *serialized_message, bool *taken, rmw_message_info_t *message_info) extern "C" rmw_ret_t rmw_take_serialized_message_with_info (const rmw_subscription_t *subscription, rmw_serialized_message_t *serialized_message, bool *taken, rmw_message_info_t *message_info, rmw_subscription_allocation_t *allocation __attribute__ ((unused)))
{ {
return rmw_take_ser_int (subscription, serialized_message, taken, message_info); return rmw_take_ser_int (subscription, serialized_message, taken, message_info);
} }
extern "C" rmw_ret_t rmw_take_event (const rmw_events_t *event_handle __attribute__ ((unused)), void *event_info __attribute__ ((unused)), bool *taken)
{
RET_NULL (taken);
*taken = false;
return RMW_RET_OK;
}
///////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////
/////////// /////////// /////////// ///////////
/////////// GUARDS AND WAITSETS /////////// /////////// GUARDS AND WAITSETS ///////////
@ -884,7 +1071,7 @@ extern "C" rmw_ret_t rmw_trigger_guard_condition (const rmw_guard_condition_t *g
return RMW_RET_OK; return RMW_RET_OK;
} }
extern "C" rmw_wait_set_t *rmw_create_wait_set (size_t max_conditions) extern "C" rmw_wait_set_t *rmw_create_wait_set (rmw_context_t *context __attribute__ ((unused)), size_t max_conditions)
{ {
(void) max_conditions; (void) max_conditions;
rmw_wait_set_t *wait_set = rmw_wait_set_allocate (); rmw_wait_set_t *wait_set = rmw_wait_set_allocate ();
@ -978,7 +1165,7 @@ static void clean_waitset_caches ()
} }
} }
extern "C" rmw_ret_t rmw_wait (rmw_subscriptions_t *subs, rmw_guard_conditions_t *gcs, rmw_services_t *srvs, rmw_clients_t *cls, rmw_wait_set_t *wait_set, const rmw_time_t *wait_timeout) extern "C" rmw_ret_t rmw_wait (rmw_subscriptions_t *subs, rmw_guard_conditions_t *gcs, rmw_services_t *srvs, rmw_clients_t *cls, rmw_events_t *evs __attribute__ ((unused)), rmw_wait_set_t *wait_set, const rmw_time_t *wait_timeout)
{ {
RET_NULL (wait_set); RET_NULL (wait_set);
CddsWaitset *ws = static_cast<CddsWaitset *> (wait_set->data); CddsWaitset *ws = static_cast<CddsWaitset *> (wait_set->data);
@ -1078,7 +1265,7 @@ static rmw_ret_t rmw_take_response_request (CddsCS *cs, rmw_request_id_t *reques
while (dds_take (cs->sub->subh, &wrap_ptr, &info, 1, 1) == 1) { while (dds_take (cs->sub->subh, &wrap_ptr, &info, 1, 1) == 1) {
if (info.valid_data) { if (info.valid_data) {
memset (request_header, 0, sizeof (wrap.header)); memset (request_header, 0, sizeof (wrap.header));
assert (sizeof (wrap.header.guid) < sizeof (wrap.header.guid)); assert (sizeof (wrap.header.guid) <= sizeof (request_header->writer_guid));
memcpy (request_header->writer_guid, &wrap.header.guid, sizeof (wrap.header.guid)); memcpy (request_header->writer_guid, &wrap.header.guid, sizeof (wrap.header.guid));
request_header->sequence_number = wrap.header.seq; request_header->sequence_number = wrap.header.seq;
if (srcfilter == 0 || srcfilter == wrap.header.guid) { if (srcfilter == 0 || srcfilter == wrap.header.guid) {
@ -1111,10 +1298,8 @@ static rmw_ret_t rmw_send_response_request (CddsCS *cs, const cdds_request_heade
if (dds_write (cs->pub->pubh, static_cast<const void *> (&wrap)) >= 0) { if (dds_write (cs->pub->pubh, static_cast<const void *> (&wrap)) >= 0) {
return RMW_RET_OK; return RMW_RET_OK;
} else { } else {
/* FIXME: what is the expected behavior when it times out? */
RMW_SET_ERROR_MSG ("cannot publish data"); RMW_SET_ERROR_MSG ("cannot publish data");
//return RMW_RET_ERROR; return RMW_RET_ERROR;
return RMW_RET_OK;
} }
} }
@ -1337,41 +1522,87 @@ extern "C" rmw_ret_t rmw_destroy_service (rmw_node_t *node, rmw_service_t *servi
extern "C" rmw_ret_t rmw_get_node_names (const rmw_node_t *node, rcutils_string_array_t *node_names, rcutils_string_array_t *node_namespaces) extern "C" rmw_ret_t rmw_get_node_names (const rmw_node_t *node, rcutils_string_array_t *node_names, rcutils_string_array_t *node_namespaces)
{ {
#if 0 // NIY dds_entity_t rd;
std::set< std::pair<std::string, std::string> > ns;
RET_WRONG_IMPLID (node); RET_WRONG_IMPLID (node);
if (rmw_check_zero_rmw_string_array (node_names) != RMW_RET_OK) { if (rmw_check_zero_rmw_string_array(node_names) != RMW_RET_OK ||
rmw_check_zero_rmw_string_array(node_namespaces) != RMW_RET_OK) {
return RMW_RET_ERROR; return RMW_RET_ERROR;
} }
auto impl = static_cast<CddsNode *> (node->data); {
if ((rd = dds_create_reader (ref_ppant (), DDS_BUILTIN_TOPIC_DCPSPARTICIPANT, NULL, NULL)) < 0) {
// FIXME: sorry, can't do it with current Zenoh unref_ppant ();
auto participant_names = std::vector<std::string>{}; RMW_SET_ERROR_MSG ("rmw_get_node_names: failed to create reader");
rcutils_allocator_t allocator = rcutils_get_default_allocator (); return RMW_RET_ERROR;
rcutils_ret_t rcutils_ret = }
rcutils_string_array_init (node_names, participant_names.size (), &allocator); dds_sample_info_t info;
if (rcutils_ret != RCUTILS_RET_OK) { void *msg = NULL;
RMW_SET_ERROR_MSG (rcutils_get_error_string_safe ()) int32_t n;
return rmw_convert_rcutils_ret_to_rmw_ret (rcutils_ret); const auto re = std::regex ("^ros2_name=(.*);ros2_namespace=(.*)$");
} while ((n = dds_take (rd, &msg, &info, 1, 1)) == 1) {
for (size_t i = 0; i < participant_names.size (); ++i) { if (info.valid_data && info.instance_state == DDS_IST_ALIVE) {
node_names->data[i] = rcutils_strdup (participant_names[i].c_str (), allocator); auto sample = static_cast<const dds_builtintopic_participant_t *> (msg);
if (!node_names->data[i]) { void *ud;
RMW_SET_ERROR_MSG ("failed to allocate memory for node name") size_t udsz;
rcutils_ret = rcutils_string_array_fini (node_names); if (dds_qget_userdata (sample->qos, &ud, &udsz)) {
if (rcutils_ret != RCUTILS_RET_OK) { std::cmatch cm;
RCUTILS_LOG_ERROR_NAMED ( /* CycloneDDS guarantees a null-terminated user data, so no need to bother with the length */
"rmw_cyclonedds_cpp", if (std::regex_match (static_cast<char *> (ud), cm, re, std::regex_constants::match_default)) {
"failed to cleanup during error handling: %s", rcutils_get_error_string_safe ()); ns.insert (std::make_pair (std::string (cm[1]), std::string (cm[2])));
}
dds_free (ud);
}
} }
return RMW_RET_BAD_ALLOC; dds_return_loan (rd, &msg, n);
}
dds_delete (rd);
unref_ppant ();
if (n < 0) {
RMW_SET_ERROR_MSG ("rmw_get_node_names: error reading participants");
return RMW_RET_ERROR;
}
}
{
rcutils_allocator_t allocator = rcutils_get_default_allocator ();
if (rcutils_string_array_init (node_names, ns.size (), &allocator) != RCUTILS_RET_OK ||
rcutils_string_array_init (node_namespaces, ns.size(), &allocator) != RCUTILS_RET_OK) {
RMW_SET_ERROR_MSG (rcutils_get_error_string ().str);
goto fail_alloc;
}
size_t i = 0;
for (auto&& n : ns) {
node_names->data[i] = rcutils_strdup (n.first.c_str (), allocator);
node_namespaces->data[i] = rcutils_strdup (n.second.c_str (), allocator);
if (!node_names->data[i] || !node_namespaces->data[i]) {
RMW_SET_ERROR_MSG ("rmw_get_node_names for name/namespace");
goto fail_alloc;
}
i++;
} }
} }
return RMW_RET_OK; return RMW_RET_OK;
#else
(void) node; (void) node_names; (void) node_namespaces; fail_alloc:
return RMW_RET_TIMEOUT; if (node_names) {
#endif if (rcutils_string_array_fini (node_names) != RCUTILS_RET_OK) {
RCUTILS_LOG_ERROR_NAMED (
"rmw_cyclonedds_cpp",
"failed to cleanup during error handling: %s", rcutils_get_error_string ().str);
rcutils_reset_error();
}
}
if (node_namespaces) {
if (rcutils_string_array_fini(node_namespaces) != RCUTILS_RET_OK) {
RCUTILS_LOG_ERROR_NAMED(
"rmw_cyclonedds_cpp",
"failed to cleanup during error handling: %s", rcutils_get_error_string ().str);
rcutils_reset_error();
}
}
return RMW_RET_BAD_ALLOC;
} }
static rmw_ret_t rmw_collect_tptyp_for_kind (std::map<std::string, std::set<std::string>>& tt, dds_entity_t builtin_topic) static rmw_ret_t rmw_collect_tptyp_for_kind (std::map<std::string, std::set<std::string>>& tt, dds_entity_t builtin_topic)