Skip to content

Commit

Permalink
Fixed bugs, fixed linters
Browse files Browse the repository at this point in the history
Fixed image tools issues

Signed-off-by: Gonzalo de Pedro <gonzalo@depedro.com.ar>

Fix linting. (ros2#12)

Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>

Fix the test for any_subscription_callback. (ros2#13)

In particular, change it so that the tests for TypeAdaptation
with any_subscription_callback always pass the custom type
to dispatch_intra_process().  That means that when using
TypeAdaptation, it is not possible to call dispatch_intra_process()
with the ROS message type.  But since this is a low-level
interface, this should be fine; the public-facing APIs handle
this case for us.

This also requires us to remove the test for inter-process
publishing with type adaptation.  That's because we didn't change
the signature of AnySubscriptionCallback::dispath() to take
in the custom type, so it always expects the ROS message type.

Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>

Reenable the test_intra_process* tests. (ros2#14)

Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>

Remove debugging print statements. (ros2#11)

Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>
Signed-off-by: Gonzalo de Pedro <gonzalo@depedro.com.ar>
  • Loading branch information
Gonzalo de Pedro authored and clalancette committed Dec 20, 2021
1 parent 89705cc commit e8dcd09
Show file tree
Hide file tree
Showing 9 changed files with 307 additions and 416 deletions.
344 changes: 91 additions & 253 deletions rclcpp/include/rclcpp/experimental/intra_process_manager.hpp

Large diffs are not rendered by default.

Original file line number Diff line number Diff line change
Expand Up @@ -15,16 +15,13 @@
#ifndef RCLCPP__EXPERIMENTAL__ROS_MESSAGE_INTRA_PROCESS_BUFFER_HPP_
#define RCLCPP__EXPERIMENTAL__ROS_MESSAGE_INTRA_PROCESS_BUFFER_HPP_

#include <functional>
#include <map>
#include <memory>
#include <stdexcept>
#include <string>
#include <utility>

#include "rcl/error_handling.h"

#include "rclcpp/any_subscription_callback.hpp"
#include "rclcpp/context.hpp"
#include "rclcpp/experimental/subscription_intra_process_base.hpp"
#include "tracetools/tracetools.h"

Expand All @@ -36,15 +33,19 @@ namespace experimental
template<
typename RosMessageT,
typename Alloc = std::allocator<void>,
typename Deleter = std::default_delete<RosMessageT>
typename Deleter = std::default_delete<void>
>
class ROSMessageIntraProcessBuffer : public SubscriptionIntraProcessBase
{
public:
RCLCPP_SMART_PTR_DEFINITIONS(ROSMessageIntraProcessBuffer)

using ROSMessageTypeAllocatorTraits = allocator::AllocRebind<RosMessageT, Alloc>;
using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type;
using ROSMessageTypeDeleter = allocator::Deleter<ROSMessageTypeAllocator, RosMessageT>;

using ConstMessageSharedPtr = std::shared_ptr<const RosMessageT>;
using MessageUniquePtr = std::unique_ptr<RosMessageT, Deleter>;
using MessageUniquePtr = std::unique_ptr<RosMessageT, ROSMessageTypeDeleter>;

ROSMessageIntraProcessBuffer(
rclcpp::Context::SharedPtr context,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -46,14 +46,12 @@ template<
>
class SubscriptionIntraProcess
: public SubscriptionIntraProcessBuffer<
MessageT,
SubscribedType,
SubscribedTypeAlloc,
SubscribedTypeDeleter
>
{
using SubscriptionIntraProcessBufferT = SubscriptionIntraProcessBuffer<
MessageT,
SubscribedType,
SubscribedTypeAlloc,
SubscribedTypeDeleter
Expand All @@ -65,8 +63,8 @@ class SubscriptionIntraProcess
using MessageAllocTraits =
typename SubscriptionIntraProcessBufferT::SubscribedTypeAllocatorTraits;
using MessageAlloc = typename SubscriptionIntraProcessBufferT::SubscribedTypeAllocator;
using ConstMessageSharedPtr = typename SubscriptionIntraProcessBufferT::ConstMessageSharedPtr;
using MessageUniquePtr = typename SubscriptionIntraProcessBufferT::MessageUniquePtr;
using ConstMessageSharedPtr = typename SubscriptionIntraProcessBufferT::ConstDataSharedPtr;
using MessageUniquePtr = typename SubscriptionIntraProcessBufferT::SubscribedTypeUniquePtr;
using BufferUniquePtr = typename SubscriptionIntraProcessBufferT::BufferUniquePtr;

SubscriptionIntraProcess(
Expand All @@ -76,7 +74,7 @@ class SubscriptionIntraProcess
const std::string & topic_name,
const rclcpp::QoS & qos_profile,
rclcpp::IntraProcessBufferType buffer_type)
: SubscriptionIntraProcessBuffer<MessageT, SubscribedType, SubscribedTypeAlloc,
: SubscriptionIntraProcessBuffer<SubscribedType, SubscribedTypeAlloc,
SubscribedTypeDeleter>(
allocator,
context,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,6 @@
#ifndef RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BUFFER_HPP_
#define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BUFFER_HPP_

#include <iostream> // TODO(anyone) remove
#include <memory>
#include <string>
#include <stdexcept>
Expand All @@ -38,7 +37,6 @@ namespace experimental
{

template<
typename MessageT,
typename SubscribedType,
typename Alloc = std::allocator<void>,
typename Deleter = std::default_delete<SubscribedType>,
Expand All @@ -64,7 +62,7 @@ class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBuffer<ROSMe
using MessageUniquePtr = std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>;

using ConstDataSharedPtr = std::shared_ptr<const SubscribedType>;
using DataUniquePtr = std::unique_ptr<SubscribedType, SubscribedTypeDeleter>;
using SubscribedTypeUniquePtr = std::unique_ptr<SubscribedType, SubscribedTypeDeleter>;

using BufferUniquePtr = typename rclcpp::experimental::buffers::IntraProcessBuffer<
SubscribedType,
Expand All @@ -78,9 +76,12 @@ class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBuffer<ROSMe
const std::string & topic_name,
const rclcpp::QoS & qos_profile,
rclcpp::IntraProcessBufferType buffer_type)
: ROSMessageIntraProcessBuffer<ROSMessageType, Alloc, ROSMessageTypeDeleter>(context, topic_name,
qos_profile)
: ROSMessageIntraProcessBuffer<ROSMessageType, Alloc, Deleter>(context, topic_name,
qos_profile),
subscribed_type_allocator_(*allocator)
{
allocator::set_allocator_for_deleter(&subscribed_type_deleter_, &subscribed_type_allocator_);

// Create the intra-process buffer.
buffer_ = rclcpp::experimental::create_intra_process_buffer<SubscribedType, Alloc,
SubscribedTypeDeleter>(
Expand All @@ -96,52 +97,55 @@ class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBuffer<ROSMe
return buffer_->has_data();
}

SubscribedTypeUniquePtr
convert_ros_message_to_subscribed_type_unique_ptr(const ROSMessageType & msg)
{
if constexpr (!std::is_same<SubscribedType, ROSMessageType>::value) {
auto ptr = SubscribedTypeAllocatorTraits::allocate(subscribed_type_allocator_, 1);
SubscribedTypeAllocatorTraits::construct(subscribed_type_allocator_, ptr);
rclcpp::TypeAdapter<SubscribedType, ROSMessageType>::convert_to_custom(msg, *ptr);
return SubscribedTypeUniquePtr(ptr, subscribed_type_deleter_);
} else {
throw std::runtime_error(
"convert_ros_message_to_subscribed_type_unique_ptr "
"unexpectedly called without TypeAdapter");
}
}

void
provide_intra_process_message(ConstMessageSharedPtr message)
{
std::cout << "--------------Provide Intra Process Message (ConstMessageSharedPtr)" << std::endl;

if constexpr (!rclcpp::TypeAdapter<SubscribedType>::is_specialized::value) {
if constexpr (std::is_same<SubscribedType, ROSMessageType>::value) {
buffer_->add_shared(std::move(message));
trigger_guard_condition();
} else {
// auto ptr = SubscribedTypeAllocatorTraits::allocate(subscribed_type_allocator_, 1);
// SubscribedTypeAllocatorTraits::construct(subscribed_type_allocator_, ptr, *message);
// buffer_->add_shared(std::unique_ptr<SubscribedType, SubscribedTypeDeleter>(
// ptr, subscribed_type_deleter_));
// trigger_guard_condition();
buffer_->add_shared(convert_ros_message_to_subscribed_type_unique_ptr(*message));
trigger_guard_condition();
}
}

void
provide_intra_process_message(MessageUniquePtr message)
{
std::cout << "--------------Provide Intra Process Message (MessageUniquePtr)" << std::endl;
if constexpr (!rclcpp::TypeAdapter<SubscribedType>::is_specialized::value) {
if constexpr (std::is_same<SubscribedType, ROSMessageType>::value) {
buffer_->add_unique(std::move(message));
trigger_guard_condition();
} else {
// auto ptr = SubscribedTypeAllocatorTraits::allocate(subscribed_type_allocator_, 1);
// SubscribedTypeAllocatorTraits::construct(subscribed_type_allocator_, ptr);
// rclcpp::TypeAdapter<SubscribedType>::convert_to_custom(*message, *ptr);
// buffer_->add_unique(std::unique_ptr<SubscribedType, SubscribedTypeDeleter>(
// ptr, subscribed_type_deleter_));
// trigger_guard_condition();
buffer_->add_unique(convert_ros_message_to_subscribed_type_unique_ptr(*message));
trigger_guard_condition();
}
}

void
provide_intra_process_data(ConstDataSharedPtr message)
{
std::cout << "--------------Provide Intra Process DATA (ConstDataSharedPtr)" << std::endl;
buffer_->add_shared(std::move(message));
trigger_guard_condition();
}

void
provide_intra_process_data(DataUniquePtr message)
provide_intra_process_data(SubscribedTypeUniquePtr message)
{
std::cout << "--------------Provide Intra Process DATA (DataUniquePtr)" << std::endl;
buffer_->add_unique(std::move(message));
trigger_guard_condition();
}
Expand All @@ -160,6 +164,8 @@ class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBuffer<ROSMe
}

BufferUniquePtr buffer_;
SubscribedTypeAllocator subscribed_type_allocator_;
SubscribedTypeDeleter subscribed_type_deleter_;
};

} // namespace experimental
Expand Down
Loading

0 comments on commit e8dcd09

Please sign in to comment.