Skip to content

Commit

Permalink
all tests pass now
Browse files Browse the repository at this point in the history
Signed-off-by: Knese Karsten <karsten.knese@us.bosch.com>
  • Loading branch information
Karsten1987 committed May 23, 2020
1 parent 6fca711 commit 3ba11fa
Show file tree
Hide file tree
Showing 53 changed files with 871 additions and 1,121 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -19,11 +19,9 @@ struct rosidl_message_type_support_t;

namespace rmw_iceoryx_cpp
{

// TODO(karsten1987): This should be `uint8`, really
void deserialize(
const char * serialized_msg,
const rosidl_message_type_support_t * type_supports,
const char * serialized_msg, const rosidl_message_type_support_t * type_supports,
void * ros_message);

} // namespace rmw_iceoryx_cpp
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,6 @@ class rosidl_message_type_support_t;

namespace rmw_iceoryx_cpp
{

/// Get the pair of ROS topic and type from a given iceoryx service triplet.
/**
* Given a triplet of `service`, `instance`, `event`, convert these to a ROS2 conform
Expand All @@ -36,11 +35,8 @@ namespace rmw_iceoryx_cpp
* \param event the iceoryx event description
* \return tuple of topic and type
*/
std::tuple<std::string, std::string>
get_name_n_type_from_service_description(
const std::string & service,
const std::string & instance,
const std::string & event);
std::tuple<std::string, std::string> get_name_n_type_from_service_description(
const std::string & service, const std::string & instance, const std::string & event);

/// Get the iceoryx service triplet description from a given pair of ROS topic and type.
/**
Expand All @@ -51,15 +47,11 @@ get_name_n_type_from_service_description(
* \param event the iceoryx event description
* \return triple of iceoryx `service`, `instance`, `event`.
*/
std::tuple<std::string, std::string, std::string>
get_service_description_from_name_n_type(
const std::string & topic_name,
const std::string & type_name);
std::tuple<std::string, std::string, std::string> get_service_description_from_name_n_type(
const std::string & topic_name, const std::string & type_name);

iox::capro::ServiceDescription
get_iceoryx_service_description(
const std::string & topic,
const rosidl_message_type_support_t * type_supports);
iox::capro::ServiceDescription get_iceoryx_service_description(
const std::string & topic, const rosidl_message_type_support_t * type_supports);

} // namespace rmw_iceoryx_cpp
#endif // RMW_ICEORYX_CPP__ICEORYX_NAME_CONVERSION_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -21,10 +21,8 @@ struct rosidl_message_type_support_t;

namespace rmw_iceoryx_cpp
{

void serialize(
const void * ros_message,
const rosidl_message_type_support_t * type_supports,
const void * ros_message, const rosidl_message_type_support_t * type_supports,
std::vector<char> & payload_vector);

} // namespace rmw_iceoryx_cpp
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -26,20 +26,17 @@

namespace rmw_iceoryx_cpp
{

std::map<std::string, std::string> get_topic_names_and_types();

std::map<std::string, std::vector<std::string>> get_nodes_and_publishers();

std::map<std::string, std::vector<std::string>> get_nodes_and_subscribers();

std::map<std::string, std::string> get_publisher_names_and_types_of_node(
const char * node_name,
const char * node_namespace);
const char * node_name, const char * node_namespace);

std::map<std::string, std::string> get_subscription_names_and_types_of_node(
const char * node_name,
const char * node_namespace);
const char * node_name, const char * node_namespace);

rmw_ret_t fill_rmw_names_and_types(
rmw_names_and_types_t * rmw_topic_names_and_types,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,6 @@ class rosidl_message_type_support_t;

namespace rmw_iceoryx_cpp
{

bool iceoryx_is_fixed_size(const rosidl_message_type_support_t * type_supports);

bool iceoryx_is_valid_type_support(const rosidl_message_type_support_t * type_supports);
Expand All @@ -32,13 +31,9 @@ std::string iceoryx_get_message_name(const rosidl_message_type_support_t * type_

std::string iceoryx_get_message_namespace(const rosidl_message_type_support_t * type_supports);

void iceoryx_init_message(
const rosidl_message_type_support_t * type_supports,
void * message);
void iceoryx_init_message(const rosidl_message_type_support_t * type_supports, void * message);

void iceoryx_fini_message(
const rosidl_message_type_support_t * type_supports,
void * message);
void iceoryx_fini_message(const rosidl_message_type_support_t * type_supports, void * message);

} // namespace rmw_iceoryx_cpp
#endif // RMW_ICEORYX_CPP__ICEORYX_TYPE_INFO_INTROSPECTION_HPP_
3 changes: 1 addition & 2 deletions rmw_iceoryx_cpp/src/iceoryx_identifier.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,8 +15,7 @@
#ifndef ICEORYX_IDENTIFIER_HPP_
#define ICEORYX_IDENTIFIER_HPP_

extern "C"
{
extern "C" {
extern const char * const rmw_iceoryx_cpp_identifier;
} // extern "C"
#endif // ICEORYX_IDENTIFIER_HPP_
3 changes: 1 addition & 2 deletions rmw_iceoryx_cpp/src/iceoryx_serialization_format.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,8 +15,7 @@
#ifndef ICEORYX_SERIALIZATION_FORMAT_HPP_
#define ICEORYX_SERIALIZATION_FORMAT_HPP_

extern "C"
{
extern "C" {
extern const char * const iceoryx_serialization_format;
} // extern "C"
#endif // ICEORYX_SERIALIZATION_FORMAT_HPP_
12 changes: 4 additions & 8 deletions rmw_iceoryx_cpp/src/internal/iceoryx_deserialize.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -27,26 +27,22 @@

namespace rmw_iceoryx_cpp
{

void deserialize(
const char * serialized_msg,
const rosidl_message_type_support_t * type_supports,
const char * serialized_msg, const rosidl_message_type_support_t * type_supports,
void * ros_message)
{
// serialize with cpp typesupport
auto ts_cpp = get_message_typesupport_handle(
type_supports,
rosidl_typesupport_introspection_cpp::typesupport_identifier);
type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier);
if (ts_cpp != nullptr) {
auto members =
static_cast<const rosidl_typesupport_introspection_cpp::MessageMembers *>(ts_cpp->data);
rmw_iceoryx_cpp::details_cpp::deserialize(serialized_msg, members, ros_message);
}

// serialize with c typesupport
auto ts_c = get_message_typesupport_handle(
type_supports,
rosidl_typesupport_introspection_c__identifier);
auto ts_c =
get_message_typesupport_handle(type_supports, rosidl_typesupport_introspection_c__identifier);
if (ts_c != nullptr) {
auto members =
static_cast<const rosidl_typesupport_introspection_c__MessageMembers *>(ts_c->data);
Expand Down
87 changes: 31 additions & 56 deletions rmw_iceoryx_cpp/src/internal/iceoryx_deserialize_typesupport_c.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#pragma once
#ifndef INTERNAL__ICEORYX_DESERIALIZE_TYPESUPPORT_C_HPP_
#define INTERNAL__ICEORYX_DESERIALIZE_TYPESUPPORT_C_HPP_

#include <array>
#include <cassert>
Expand All @@ -33,14 +34,8 @@ namespace rmw_iceoryx_cpp
{
namespace details_c
{

template<
class T,
size_t SizeT = sizeof(T)
>
const char * deserialize_element(
const char * serialized_msg,
void * ros_message_field)
template<class T, size_t SizeT = sizeof(T)>
const char * deserialize_element(const char * serialized_msg, void * ros_message_field)
{
T * data = reinterpret_cast<T *>(ros_message_field);
memcpy(data, serialized_msg, SizeT);
Expand All @@ -51,8 +46,7 @@ const char * deserialize_element(

template<>
const char * deserialize_element<rosidl_runtime_c__String, sizeof(rosidl_runtime_c__String)>(
const char * serialized_msg,
void * ros_message_field)
const char * serialized_msg, void * ros_message_field)
{
uint32_t string_size = 0;
std::tie(serialized_msg, string_size) = pop_sequence_size(serialized_msg);
Expand All @@ -65,10 +59,7 @@ const char * deserialize_element<rosidl_runtime_c__String, sizeof(rosidl_runtime
}

template<typename T>
const char * deserialize_array(
const char * serialized_msg,
void * ros_message_field,
uint32_t size)
const char * deserialize_array(const char * serialized_msg, void * ros_message_field, uint32_t size)
{
auto array = reinterpret_cast<T *>(ros_message_field);
for (size_t i = 0; i < size; ++i) {
Expand All @@ -78,10 +69,7 @@ const char * deserialize_array(
return serialized_msg;
}

template<
class T,
size_t SizeT = sizeof(T)
>
template<class T, size_t SizeT = sizeof(T)>
const char * deserialize_sequence(const char * serialized_msg, void * ros_message_field)
{
uint32_t array_size = 0;
Expand All @@ -100,8 +88,7 @@ const char * deserialize_sequence(const char * serialized_msg, void * ros_messag

template<>
const char * deserialize_sequence<char, sizeof(char)>(
const char * serialized_msg,
void * ros_message_field)
const char * serialized_msg, void * ros_message_field)
{
uint32_t array_size = 0;
std::tie(serialized_msg, array_size) = pop_sequence_size(serialized_msg);
Expand All @@ -119,8 +106,8 @@ const char * deserialize_sequence<char, sizeof(char)>(

template<typename T>
const char * deserialize_message_field(
const rosidl_typesupport_introspection_c__MessageMember * member,
const char * serialized_msg, void * ros_message_field)
const rosidl_typesupport_introspection_c__MessageMember * member, const char * serialized_msg,
void * ros_message_field)
{
debug_log("deserializing %s\n", member->name_);
if (!member->is_array_) {
Expand All @@ -133,8 +120,7 @@ const char * deserialize_message_field(
}

const char * deserialize(
const char * serialized_msg,
const rosidl_typesupport_introspection_c__MessageMembers * members,
const char * serialized_msg, const rosidl_typesupport_introspection_c__MessageMembers * members,
void * ros_message)
{
for (uint32_t i = 0; i < members->member_count_; ++i) {
Expand All @@ -146,9 +132,8 @@ const char * deserialize(
break;
case ::rosidl_typesupport_introspection_c__ROS_TYPE_BYTE:
case ::rosidl_typesupport_introspection_c__ROS_TYPE_UINT8:
serialized_msg = deserialize_message_field<uint8_t>(
member, serialized_msg,
ros_message_field);
serialized_msg =
deserialize_message_field<uint8_t>(member, serialized_msg, ros_message_field);
break;
case ::rosidl_typesupport_introspection_c__ROS_TYPE_CHAR:
case ::rosidl_typesupport_introspection_c__ROS_TYPE_INT8:
Expand All @@ -163,42 +148,34 @@ const char * deserialize(
deserialize_message_field<double>(member, serialized_msg, ros_message_field);
break;
case ::rosidl_typesupport_introspection_c__ROS_TYPE_INT16:
serialized_msg = deserialize_message_field<int16_t>(
member, serialized_msg,
ros_message_field);
serialized_msg =
deserialize_message_field<int16_t>(member, serialized_msg, ros_message_field);
break;
case ::rosidl_typesupport_introspection_c__ROS_TYPE_UINT16:
serialized_msg = deserialize_message_field<uint16_t>(
member, serialized_msg,
ros_message_field);
serialized_msg =
deserialize_message_field<uint16_t>(member, serialized_msg, ros_message_field);
break;
case ::rosidl_typesupport_introspection_c__ROS_TYPE_INT32:
serialized_msg = deserialize_message_field<int32_t>(
member, serialized_msg,
ros_message_field);
serialized_msg =
deserialize_message_field<int32_t>(member, serialized_msg, ros_message_field);
break;
case ::rosidl_typesupport_introspection_c__ROS_TYPE_UINT32:
serialized_msg = deserialize_message_field<uint32_t>(
member, serialized_msg,
ros_message_field);
serialized_msg =
deserialize_message_field<uint32_t>(member, serialized_msg, ros_message_field);
break;
case ::rosidl_typesupport_introspection_c__ROS_TYPE_INT64:
serialized_msg = deserialize_message_field<int64_t>(
member, serialized_msg,
ros_message_field);
serialized_msg =
deserialize_message_field<int64_t>(member, serialized_msg, ros_message_field);
break;
case ::rosidl_typesupport_introspection_c__ROS_TYPE_UINT64:
serialized_msg = deserialize_message_field<uint64_t>(
member, serialized_msg,
ros_message_field);
serialized_msg =
deserialize_message_field<uint64_t>(member, serialized_msg, ros_message_field);
break;
case ::rosidl_typesupport_introspection_c__ROS_TYPE_STRING:
serialized_msg = deserialize_message_field<rosidl_runtime_c__String>(
member, serialized_msg,
ros_message_field);
member, serialized_msg, ros_message_field);
break;
case ::rosidl_typesupport_introspection_c__ROS_TYPE_MESSAGE:
{
case ::rosidl_typesupport_introspection_c__ROS_TYPE_MESSAGE: {
auto sub_members =
(const rosidl_typesupport_introspection_c__MessageMembers *)member->members_->data;

Expand All @@ -217,10 +194,8 @@ const char * deserialize(

std::tie(serialized_msg, sequence_size) = pop_sequence_size(serialized_msg);

auto sequence =
const_cast<rosidl_runtime_c__char__Sequence *>(reinterpret_cast<const
rosidl_runtime_c__char__Sequence
*>(ros_message_field));
auto sequence = const_cast<rosidl_runtime_c__char__Sequence *>(
reinterpret_cast<const rosidl_runtime_c__char__Sequence *>(ros_message_field));

subros_message = reinterpret_cast<void *>(sequence->data);
}
Expand All @@ -229,8 +204,7 @@ const char * deserialize(
serialized_msg = deserialize(serialized_msg, sub_members, subros_message);
subros_message = static_cast<char *>(subros_message) + sub_members_size;
}
}
break;
} break;
default:
throw std::runtime_error("unknown type");
}
Expand All @@ -240,3 +214,4 @@ const char * deserialize(

} // namespace details_c
} // namespace rmw_iceoryx_cpp
#endif // INTERNAL__ICEORYX_DESERIALIZE_TYPESUPPORT_C_HPP_
Loading

0 comments on commit 3ba11fa

Please sign in to comment.