Skip to content

Commit

Permalink
fix test compilation in ROS2
Browse files Browse the repository at this point in the history
  • Loading branch information
@facontidavide
facontidavide committed May 23, 2024
1 parent a7791ca commit 4cd89b2
Show file tree
Hide file tree
Showing 4 changed files with 237 additions and 5 deletions.
43 changes: 41 additions & 2 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,20 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON)

set(CMAKE_POSITION_INDEPENDENT_CODE ON)

find_package(ament_cmake QUIET)

if (ament_cmake_FOUND)
set(USING_ROS2 TRUE)
find_package(rosbag2_cpp REQUIRED)
find_package(ament_index_cpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(rclcpp REQUIRED)

set(EXTRA_SRC
src/ros_utils/message_definition_cache.cpp
src/ros_utils/ros2_helpers.cpp)
endif()

find_package(RapidJSON REQUIRED)
find_package(fastcdr QUIET)

Expand All @@ -34,14 +48,21 @@ add_library(rosx_introspection STATIC
src/ros_message.cpp
src/ros_parser.cpp
src/deserializer.cpp
${EXTRA_SRC}
)
target_link_libraries(rosx_introspection
fastcdr rapidjson)

target_link_libraries(rosx_introspection fastcdr)

target_include_directories(rosx_introspection PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
)

if(USING_ROS2)
ament_target_dependencies(rosx_introspection
ament_index_cpp
rclcpp
rosbag2_cpp)
endif()

install(TARGETS rosx_introspection
EXPORT rosx_introspection
Expand All @@ -50,3 +71,21 @@ install(TARGETS rosx_introspection
RUNTIME DESTINATION bin
INCLUDES DESTINATION include
)

###############################################
## Tests
###############################################

if(USING_ROS2)
add_executable(rosx_introspection_test
test/test_parser.cpp
test/test_ros2.cpp )

ament_target_dependencies(rosx_introspection_test sensor_msgs)

target_link_libraries(rosx_introspection_test rosx_introspection)

target_include_directories(rosx_introspection_test PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/doctest>)
endif()
7 changes: 6 additions & 1 deletion include/rosx_introspection/ros_parser.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -128,7 +128,7 @@ class Parser
* IMPORTANT: this approach is not meant to be used with use arrays such as maps,
* point clouds and images.For this reason the argument max_array_size is used.
*
* This funtion is almost always followed by CreateRenamedValues,
* This function is almost always followed by CreateRenamedValues,
* which provide a more human-readable key-value representation.
*
* @param buffer raw memory to be parsed.
Expand All @@ -142,6 +142,9 @@ class Parser
bool deserialize(Span<const uint8_t> buffer, FlatMessage* flat_output,
Deserializer* deserializer) const;

bool deserializeIntoJson(Span<const uint8_t> buffer, std::string* json_txt,
Deserializer* deserializer, bool ignore_constants) const;

typedef std::function<void(const ROSType&, Span<uint8_t>&)> VisitingCallback;

/**
Expand Down Expand Up @@ -186,6 +189,8 @@ class Parser
std::unique_ptr<Deserializer> _deserializer;
};

//--------------------------------------------------------------------------

typedef std::vector<std::pair<std::string, double>> RenamedValues;

template <class DeserializerT>
Expand Down
3 changes: 1 addition & 2 deletions package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,6 @@
<maintainer email="[email protected]">Davide Faconti</maintainer>
<license>MIT</license>

<url>http://www.ros.org/wiki/ros_type_introspection</url>
<author email="[email protected]">Davide Faconti</author>

<buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
Expand All @@ -18,7 +17,7 @@

<depend condition="$ROS_VERSION == 2">rclcpp</depend>
<depend condition="$ROS_VERSION == 2">rosbag2_cpp</depend>
<depend condition="$ROS_VERSION == 2">rosidl_typesupport_cpp</depend>
<depend condition="$ROS_VERSION == 2">ament_index_cpp</depend>

<depend>rapidjson-dev</depend>

Expand Down
189 changes: 189 additions & 0 deletions src/ros_parser.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,10 @@
#include "rosx_introspection/ros_parser.hpp"
#include "rosx_introspection/deserializer.hpp"

#include "rapidjson/document.h"
#include "rapidjson/stringbuffer.h"
#include "rapidjson/writer.h"

namespace RosMsgParser
{
inline bool operator==(const std::string& a, const std::string_view& b)
Expand Down Expand Up @@ -243,4 +247,189 @@ bool Parser::deserialize(Span<const uint8_t> buffer, FlatMessage* flat_container
return entire_message_parse;
}

bool Parser::deserializeIntoJson(Span<const uint8_t> buffer, std::string* json_txt,
Deserializer* deserializer, bool ignore_constants) const
{
deserializer->init(buffer);

rapidjson::Document json_document;
rapidjson::Document::AllocatorType& alloc = json_document.GetAllocator();

size_t buffer_offset = 0;

std::function<void(const ROSMessage*, rapidjson::Value&)> deserializeImpl;

deserializeImpl = [&](const ROSMessage* msg_node, rapidjson::Value& json_value) {
size_t index_s = 0;
size_t index_m = 0;

for (const ROSField& field : msg_node->fields())
{
if (field.isConstant() && ignore_constants)
{
continue;
}

const ROSType& field_type = field.type();
auto field_name = rapidjson::StringRef(field.name().data(), field.name().size());

int32_t array_size = field.arraySize();
if (array_size == -1)
{
array_size = deserializer->deserializeUInt32();
}

// Stop storing if it is a blob.
if (array_size > static_cast<int32_t>(_max_array_size))
{
if (buffer_offset + array_size > static_cast<std::size_t>(buffer.size()))
{
throw std::runtime_error("Buffer overrun in blob");
}
buffer_offset += array_size;
}
else // NOT a BLOB
{
rapidjson::Value array_value(rapidjson::kArrayType);

for (int i = 0; i < array_size; i++)
{
rapidjson::Value new_value;
new_value.SetObject();

switch (field_type.typeID())
{
case BOOL:
new_value.SetBool(
deserializer->deserialize(field_type.typeID()).convert<uint8_t>());
break;
case CHAR: {
char c = deserializer->deserialize(field_type.typeID()).convert<int8_t>();
new_value.SetString(&c, 1, alloc);
}
break;
case BYTE:
case UINT8:
case UINT16:
case UINT32:
new_value.SetUint(
(deserializer->deserialize(field_type.typeID()).convert<uint32_t>()));
break;
case UINT64:
new_value.SetUint64(
(deserializer->deserialize(field_type.typeID()).convert<uint64_t>()));
break;
case INT8:
case INT16:
case INT32:
new_value.SetInt(
(deserializer->deserialize(field_type.typeID()).convert<int32_t>()));
break;
case INT64:
new_value.SetInt64(
(deserializer->deserialize(field_type.typeID()).convert<int64_t>()));
break;
case FLOAT32:
new_value.SetFloat(
(deserializer->deserialize(field_type.typeID()).convert<float>()));
break;
case FLOAT64:
new_value.SetDouble(
(deserializer->deserialize(field_type.typeID()).convert<double>()));
break;
case TIME: {
int sec = deserializer->deserialize(INT32).convert<int32_t>();
int nsec = deserializer->deserialize(INT32).convert<int32_t>();
rapidjson::Value sec_Value;
sec_Value.SetObject();
sec_Value.SetInt(sec);
new_value.AddMember("secs", sec_Value, alloc);

rapidjson::Value nsec_value;
nsec_value.SetObject();
nsec_value.SetInt(nsec);
new_value.AddMember("nsecs", nsec_value, alloc);
}
break;
case DURATION: {
int sec = deserializer->deserialize(INT32).convert<int32_t>();
int nsec = deserializer->deserialize(INT32).convert<int32_t>();
rapidjson::Value sec_Value;
sec_Value.SetObject();
sec_Value.SetInt(sec);
new_value.AddMember("secs", sec_Value, alloc);

rapidjson::Value nsec_value;
nsec_value.SetObject();
nsec_value.SetInt(nsec);
new_value.AddMember("nsecs", nsec_value, alloc);
}
break;

case STRING: {
std::string s;
deserializer->deserializeString(s);
new_value.SetString(s.c_str(), s.length(), alloc);
}
break;
case OTHER: {
auto msg_node_child = field.getMessagePtr(_schema->msg_library);
deserializeImpl(msg_node_child.get(), new_value);
}
break;
} // end switch

if (field.isArray())
{
array_value.PushBack(new_value, alloc);
}
else
{
json_value.AddMember(field_name, new_value, alloc);
}
} // end for array

if (field.isArray())
{
json_value.AddMember(field_name, array_value, alloc);
}
} // end for array_size

if (field_type.typeID() == OTHER)
{
index_m++;
}
index_s++;
} // end for fields
}; // end of lambda

FieldLeaf rootnode;
rootnode.node = _schema->field_tree.croot();
auto root_msg =
_schema->field_tree.croot()->value()->getMessagePtr(_schema->msg_library);

json_document.SetObject();
rapidjson::Value json_node;
json_node.SetObject();

deserializeImpl(root_msg.get(), json_node);

auto topic_name = rapidjson::StringRef(_topic_name.data(), _topic_name.size());
json_document.AddMember("topic", topic_name, alloc);
json_document.AddMember("msg", json_node, alloc);

rapidjson::StringBuffer json_buffer;
json_buffer.Reserve(2048);

rapidjson::Writer<rapidjson::StringBuffer, rapidjson::UTF8<>, rapidjson::UTF8<>,
rapidjson::CrtAllocator,
rapidjson::kWriteDefaultFlags | rapidjson::kWriteNanAndInfFlag>
json_writer(json_buffer);
json_document.Accept(json_writer);

*json_txt = json_buffer.GetString();

return true;
}

} // namespace RosMsgParser

0 comments on commit 4cd89b2

Please sign in to comment.