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

Cpplint and cleanup

Browse source
This commit is contained in:
Jackie Kay 2015-10-27 19:08:21 -07:00
parent fc48cf5fa2
commit 24fb204192
19 changed files with 368 additions and 376 deletions
Download patch file Download diff file Expand all files Collapse all files

16
rclcpp/include/rclcpp/allocator/allocator_common.hpp
View file

@ -12,22 +12,22 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP__ALLOCATOR_COMMON__HPP_
#define RCLCPP_RCLCPP__ALLOCATOR_COMMON__HPP_
#ifndef RCLCPP__ALLOCATOR__ALLOCATOR_COMMON_HPP_
#define RCLCPP__ALLOCATOR__ALLOCATOR_COMMON_HPP_
#include <memory>
#include <rclcpp/allocator/allocator_deleter.hpp>
#include "rclcpp/allocator/allocator_deleter.hpp"
namespace rclcpp
{
namespace allocator
{
template<typename U, typename Alloc>
using AllocRebind = typename std::allocator_traits<Alloc>::template rebind_traits<U>;
template<typename T, typename Alloc>
using AllocRebind = typename std::allocator_traits<Alloc>::template rebind_traits<T>;
}
}
} // namespace allocator
} // namespace rclcpp
#endif
#endif // RCLCPP__ALLOCATOR__ALLOCATOR_COMMON_HPP_

33
rclcpp/include/rclcpp/allocator/allocator_deleter.hpp
View file

@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_ALLOCATOR_DELETER_HPP_
#define RCLCPP_RCLCPP_ALLOCATOR_DELETER_HPP_
#ifndef RCLCPP__ALLOCATOR__ALLOCATOR_DELETER_HPP_
#define RCLCPP__ALLOCATOR__ALLOCATOR_DELETER_HPP_
#include <memory>
@ -26,33 +26,32 @@ namespace allocator
template<typename Allocator>
class AllocatorDeleter
{
template<typename U>
using AllocRebind = typename std::allocator_traits<Allocator>::template rebind_alloc<U>;
template<typename T>
using AllocRebind = typename std::allocator_traits<Allocator>::template rebind_alloc<T>;
public:
AllocatorDeleter()
: allocator_(NULL)
: allocator_(nullptr)
{
}
AllocatorDeleter(Allocator * a)
explicit AllocatorDeleter(Allocator * a)
: allocator_(a)
{
}
template<typename U>
AllocatorDeleter(const AllocatorDeleter<U> & a)
template<typename T>
AllocatorDeleter(const AllocatorDeleter<T> & a)
{
allocator_ = a.get_allocator();
}
template<typename U>
void operator()(U * ptr)
template<typename T>
void operator()(T * ptr)
{
std::allocator_traits<AllocRebind<U>>::destroy(*allocator_, ptr);
// TODO: figure out if we're guaranteed to be destroying only 1 item here
std::allocator_traits<AllocRebind<U>>::deallocate(*allocator_, ptr, 1);
ptr = NULL;
std::allocator_traits<AllocRebind<T>>::destroy(*allocator_, ptr);
std::allocator_traits<AllocRebind<T>>::deallocate(*allocator_, ptr, 1);
ptr = nullptr;
}
Allocator * get_allocator() const
@ -100,7 +99,7 @@ using Deleter = typename std::conditional<
std::default_delete<T>,
AllocatorDeleter<Alloc>
>::type;
}
}
} // namespace allocator
} // namespace rclcpp
#endif
#endif // RCLCPP__ALLOCATOR__ALLOCATOR_DELETER_HPP_

18
rclcpp/include/rclcpp/any_subscription_callback.hpp
View file

@ -12,17 +12,17 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_ANY_SUBSCRIPTION_CALLBACK_HPP_
#define RCLCPP_RCLCPP_ANY_SUBSCRIPTION_CALLBACK_HPP_
#ifndef RCLCPP__ANY_SUBSCRIPTION_CALLBACK_HPP_
#define RCLCPP__ANY_SUBSCRIPTION_CALLBACK_HPP_
#include <rclcpp/allocator/allocator_common.hpp>
#include <rclcpp/function_traits.hpp>
#include <rmw/types.h>
#include <functional>
#include <memory>
#include <type_traits>
#include <rmw/types.h>
#include "rclcpp/allocator/allocator_common.hpp"
#include "rclcpp/function_traits.hpp"
namespace rclcpp
{
@ -56,7 +56,7 @@ class AnySubscriptionCallback
UniquePtrWithInfoCallback unique_ptr_with_info_callback_;
public:
AnySubscriptionCallback(std::shared_ptr<Alloc> allocator)
explicit AnySubscriptionCallback(std::shared_ptr<Alloc> allocator)
: shared_ptr_callback_(nullptr), shared_ptr_with_info_callback_(nullptr),
const_shared_ptr_callback_(nullptr), const_shared_ptr_with_info_callback_(nullptr),
unique_ptr_callback_(nullptr), unique_ptr_with_info_callback_(nullptr)
@ -206,7 +206,7 @@ private:
MessageDeleter message_deleter_;
};
} /* namespace any_subscription_callback */
} /* namespace rclcpp */
} // namespace any_subscription_callback
} // namespace rclcpp
#endif /* RCLCPP_RCLCPP_ANY_SUBSCRIPTION_CALLBACK_HPP_ */
#endif // RCLCPP__ANY_SUBSCRIPTION_CALLBACK_HPP_

35
rclcpp/include/rclcpp/executor.hpp
View file

@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_EXECUTOR_HPP_
#define RCLCPP_RCLCPP_EXECUTOR_HPP_
#ifndef RCLCPP__EXECUTOR_HPP_
#define RCLCPP__EXECUTOR_HPP_
#include <algorithm>
#include <cassert>
@ -23,14 +23,14 @@
#include <memory>
#include <vector>
#include <rcl_interfaces/msg/intra_process_message.hpp>
#include "rcl_interfaces/msg/intra_process_message.hpp"
#include <rclcpp/any_executable.hpp>
#include <rclcpp/macros.hpp>
#include <rclcpp/memory_strategy.hpp>
#include <rclcpp/memory_strategies.hpp>
#include <rclcpp/node.hpp>
#include <rclcpp/utilities.hpp>
#include "rclcpp/any_executable.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/memory_strategies.hpp"
#include "rclcpp/memory_strategy.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/utilities.hpp"
namespace rclcpp
{
@ -49,7 +49,6 @@ namespace executor
*/
class Executor
{
public:
RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Executor);
@ -94,7 +93,7 @@ public:
for (auto & weak_node : weak_nodes_) {
auto node = weak_node.lock();
if (node == node_ptr) {
// TODO: Use a different error here?
// TODO(jacquelinekay): Use a different error here?
throw std::runtime_error("Cannot add node to executor, node already added.");
}
}
@ -358,7 +357,6 @@ protected:
}));
}
// Use the number of subscriptions to allocate memory in the handles
rmw_subscriptions_t subscriber_handles;
subscriber_handles.subscriber_count =
@ -431,9 +429,8 @@ protected:
memory_strategy_->clear_handles();
return;
}
// Add the new work to the class's list of things waiting to be executed
// Starting with the subscribers
memory_strategy_->clear_active_entities();
memory_strategy_->revalidate_handles();
}
/******************************/
@ -565,7 +562,7 @@ protected:
return any_exec;
}
// If there is no ready executable, return a null ptr
return std::shared_ptr<AnyExecutable>();
return nullptr;
}
template<typename T = std::milli>
@ -613,7 +610,7 @@ private:
std::array<void *, 2> guard_cond_handles_;
};
} /* executor */
} /* rclcpp */
} // namespace executor
} // namespace rclcpp
#endif /* RCLCPP_RCLCPP_EXECUTOR_HPP_ */
#endif // RCLCPP__EXECUTOR_HPP_

25
rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp
View file

@ -12,8 +12,10 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_EXECUTORS_MULTI_THREADED_EXECUTOR_HPP_
#define RCLCPP_RCLCPP_EXECUTORS_MULTI_THREADED_EXECUTOR_HPP_
#ifndef RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_
#define RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_
#include <rmw/rmw.h>
#include <cassert>
#include <cstdlib>
@ -21,12 +23,10 @@
#include <mutex>
#include <vector>
#include <rmw/rmw.h>
#include <rclcpp/executor.hpp>
#include <rclcpp/macros.hpp>
#include <rclcpp/node.hpp>
#include <rclcpp/utilities.hpp>
#include "rclcpp/executor.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/utilities.hpp"
namespace rclcpp
{
@ -97,11 +97,10 @@ private:
std::mutex wait_mutex_;
size_t number_of_threads_;
};
} /* namespace multi_threaded_executor */
} /* namespace executors */
} /* namespace rclcpp */
} // namespace multi_threaded_executor
} // namespace executors
} // namespace rclcpp
#endif /* RCLCPP_RCLCPP_EXECUTORS_MULTI_THREADED_EXECUTOR_HPP_ */
#endif // RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_

29
rclcpp/include/rclcpp/executors/single_threaded_executor.hpp
View file

@ -12,22 +12,22 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_EXECUTORS_SINGLE_THREADED_EXECUTOR_HPP_
#define RCLCPP_RCLCPP_EXECUTORS_SINGLE_THREADED_EXECUTOR_HPP_
#ifndef RCLCPP__EXECUTORS__SINGLE_THREADED_EXECUTOR_HPP_
#define RCLCPP__EXECUTORS__SINGLE_THREADED_EXECUTOR_HPP_
#include <rmw/rmw.h>
#include <cassert>
#include <cstdlib>
#include <memory>
#include <vector>
#include <rmw/rmw.h>
#include <rclcpp/executor.hpp>
#include <rclcpp/macros.hpp>
#include <rclcpp/memory_strategies.hpp>
#include <rclcpp/node.hpp>
#include <rclcpp/utilities.hpp>
#include <rclcpp/rate.hpp>
#include "rclcpp/executor.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/memory_strategies.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/utilities.hpp"
#include "rclcpp/rate.hpp"
namespace rclcpp
{
@ -64,11 +64,10 @@ public:
private:
RCLCPP_DISABLE_COPY(SingleThreadedExecutor);
};
} /* namespace single_threaded_executor */
} /* namespace executors */
} /* namespace rclcpp */
} // namespace single_threaded_executor
} // namespace executors
} // namespace rclcpp
#endif /* RCLCPP_RCLCPP_EXECUTORS_SINGLE_THREADED_EXECUTOR_HPP_ */
#endif // RCLCPP__EXECUTORS__SINGLE_THREADED_EXECUTOR_HPP_

61
rclcpp/include/rclcpp/intra_process_manager.hpp
View file

@ -12,24 +12,26 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_INTRA_PROCESS_MANAGER_HPP_
#define RCLCPP_RCLCPP_INTRA_PROCESS_MANAGER_HPP_
#ifndef RCLCPP__INTRA_PROCESS_MANAGER_HPP_
#define RCLCPP__INTRA_PROCESS_MANAGER_HPP_
#include <rclcpp/intra_process_manager_state.hpp>
#include <rclcpp/mapped_ring_buffer.hpp>
#include <rclcpp/macros.hpp>
#include <rclcpp/publisher.hpp>
#include <rclcpp/subscription.hpp>
#include <rmw/types.h>
#include <algorithm>
#include <atomic>
#include <cstdint>
#include <exception>
#include <map>
#include <memory>
#include <unordered_map>
#include <set>
#include <rmw/types.h>
#include "rclcpp/allocator/allocator_deleter.hpp"
#include "rclcpp/intra_process_manager_state.hpp"
#include "rclcpp/mapped_ring_buffer.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/publisher.hpp"
#include "rclcpp/subscription.hpp"
namespace rclcpp
{
@ -122,8 +124,9 @@ private:
public:
RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessManager);
//IntraProcessManager() = default;
IntraProcessManager(IntraProcessManagerStateBase::SharedPtr state = create_default_state())
explicit IntraProcessManager(
IntraProcessManagerStateBase::SharedPtr state = create_default_state()
)
: state_(state)
{
}
@ -236,19 +239,18 @@ public:
* \param message the message that is being stored.
* \return the message sequence number.
*/
template<typename MessageT, typename Alloc = std::allocator<void>>
template<typename MessageT, typename Alloc = std::allocator<void>,
typename Deleter = std::default_delete<MessageT>>
uint64_t
store_intra_process_message(
uint64_t intra_process_publisher_id,
std::unique_ptr<MessageT,
typename allocator::Deleter<typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>,
MessageT>> & message)
std::unique_ptr<MessageT, Deleter> & message)
{
using MRBMessageAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>;
typedef typename mapped_ring_buffer::MappedRingBuffer<MessageT, MRBMessageAlloc> TypedMRB;
mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer;
auto message_seq = state_->get_publisher_info_for_id(intra_process_publisher_id, buffer);
using TypedMRB = typename mapped_ring_buffer::MappedRingBuffer<MessageT, MRBMessageAlloc>;
uint64_t message_seq = 0;
mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer = state_->get_publisher_info_for_id(
intra_process_publisher_id, message_seq);
typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast<TypedMRB>(buffer);
if (!typed_buffer) {
throw std::runtime_error("Typecast failed due to incorrect message type");
@ -299,25 +301,25 @@ public:
* \param requesting_subscriptions_intra_process_id the subscription's id.
* \param message the message typed unique_ptr used to return the message.
*/
template<typename MessageT, typename Alloc = std::allocator<void>>
template<typename MessageT, typename Alloc = std::allocator<void>,
typename Deleter = std::default_delete<MessageT>>
void
take_intra_process_message(
uint64_t intra_process_publisher_id,
uint64_t message_sequence_number,
uint64_t requesting_subscriptions_intra_process_id,
std::unique_ptr<MessageT,
typename allocator::Deleter<typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>,
MessageT>> & message)
std::unique_ptr<MessageT, Deleter> & message)
{
using MRBMessageAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>;
typedef typename mapped_ring_buffer::MappedRingBuffer<MessageT, MRBMessageAlloc> TypedMRB;
using TypedMRB = mapped_ring_buffer::MappedRingBuffer<MessageT, MRBMessageAlloc>;
message = nullptr;
mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer;
size_t target_subs_size = state_->take_intra_process_message(
size_t target_subs_size = 0;
mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer = state_->take_intra_process_message(
intra_process_publisher_id,
message_sequence_number,
requesting_subscriptions_intra_process_id,
buffer
target_subs_size
);
typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast<TypedMRB>(buffer);
if (!typed_buffer) {
@ -365,12 +367,11 @@ private:
static std::atomic<uint64_t> next_unique_id_;
IntraProcessManagerStateBase::SharedPtr state_;
};
std::atomic<uint64_t> IntraProcessManager::next_unique_id_ {1};
} /* namespace intra_process_manager */
} /* namespace rclcpp */
} // namespace intra_process_manager
} // namespace rclcpp
#endif /* RCLCPP_RCLCPP_INTRA_PROCESS_MANAGER_HPP_ */
#endif // RCLCPP__INTRA_PROCESS_MANAGER_HPP_

53
rclcpp/include/rclcpp/intra_process_manager_state.hpp
View file

@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP__RCLCPP__INTRA_PROCESS_MANAGER_STATE_HPP_
#define RCLCPP__RCLCPP__INTRA_PROCESS_MANAGER_STATE_HPP_
#ifndef RCLCPP__INTRA_PROCESS_MANAGER_STATE_HPP_
#define RCLCPP__INTRA_PROCESS_MANAGER_STATE_HPP_
#include <rclcpp/mapped_ring_buffer.hpp>
#include <rclcpp/publisher.hpp>
@ -21,11 +21,14 @@
#include <algorithm>
#include <atomic>
#include <functional>
#include <limits>
#include <memory>
#include <map>
#include <unordered_map>
#include <set>
#include <string>
#include <utility>
namespace rclcpp
{
@ -35,8 +38,7 @@ namespace intra_process_manager
class IntraProcessManagerStateBase
{
public:
//RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessManagerStateBase);
using SharedPtr = std::shared_ptr<IntraProcessManagerStateBase>;
RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(IntraProcessManagerStateBase);
virtual void
add_subscription(uint64_t id, subscription::SubscriptionBase::SharedPtr subscription) = 0;
@ -52,19 +54,19 @@ public:
virtual void
remove_publisher(uint64_t intra_process_publisher_id) = 0;
virtual uint64_t
virtual mapped_ring_buffer::MappedRingBufferBase::SharedPtr
get_publisher_info_for_id(
uint64_t intra_process_publisher_id,
mapped_ring_buffer::MappedRingBufferBase::SharedPtr & mrb) = 0;
uint64_t & message_seq) = 0;
virtual void
store_intra_process_message(uint64_t intra_process_publisher_id, uint64_t message_seq) = 0;
virtual size_t
virtual mapped_ring_buffer::MappedRingBufferBase::SharedPtr
take_intra_process_message(uint64_t intra_process_publisher_id,
uint64_t message_sequence_number,
uint64_t requesting_subscriptions_intra_process_id,
mapped_ring_buffer::MappedRingBufferBase::SharedPtr & mrb) = 0;
size_t & size) = 0;
virtual bool
matches_any_publishers(const rmw_gid_t * id) const = 0;
@ -95,7 +97,6 @@ public:
sub_pair.second.erase(intra_process_subscription_id);
}
}
}
void add_publisher(uint64_t id,
@ -103,7 +104,6 @@ public:
mapped_ring_buffer::MappedRingBufferBase::SharedPtr mrb,
size_t size)
{
publishers_[id].publisher = publisher;
// As long as the size of the ring buffer is less than the max sequence number, we're safe.
if (size > std::numeric_limits<uint64_t>::max()) {
@ -121,23 +121,21 @@ public:
publishers_.erase(intra_process_publisher_id);
}
// TODO
// return message_seq and mrb
uint64_t
mapped_ring_buffer::MappedRingBufferBase::SharedPtr
get_publisher_info_for_id(
uint64_t intra_process_publisher_id,
mapped_ring_buffer::MappedRingBufferBase::SharedPtr & mrb)
uint64_t & message_seq)
{
auto it = publishers_.find(intra_process_publisher_id);
if (it == publishers_.end()) {
throw std::runtime_error("store_intra_process_message called with invalid publisher id");
}
PublisherInfo & info = it->second;
mrb = info.buffer;
// Calculate the next message sequence number.
uint64_t message_seq = info.sequence_number.fetch_add(1, std::memory_order_relaxed);
message_seq = info.sequence_number.fetch_add(1, std::memory_order_relaxed);
return message_seq;
return info.buffer;
}
void
@ -166,17 +164,15 @@ public:
info.target_subscriptions_by_message_sequence[message_seq].end()
)
);
}
size_t
mapped_ring_buffer::MappedRingBufferBase::SharedPtr
take_intra_process_message(uint64_t intra_process_publisher_id,
uint64_t message_sequence_number,
uint64_t requesting_subscriptions_intra_process_id,
mapped_ring_buffer::MappedRingBufferBase::SharedPtr & mrb
size_t & size
)
{
mrb = nullptr;
PublisherInfo * info;
{
auto it = publishers_.find(intra_process_publisher_id);
@ -206,8 +202,8 @@ public:
}
target_subs->erase(it);
}
mrb = info->buffer;
return target_subs->size();
size = target_subs->size();
return info->buffer;
}
bool
@ -229,14 +225,12 @@ private:
template<typename T>
using RebindAlloc = typename std::allocator_traits<Allocator>::template rebind_alloc<T>;
//using AllocString = std::basic_string<char, std::char_traits<char>, RebindAlloc<char>>;
using AllocString = std::string;
using AllocSet = std::set<uint64_t, std::less<uint64_t>, RebindAlloc<uint64_t>>;
using SubscriptionMap = std::unordered_map<uint64_t, subscription::SubscriptionBase::WeakPtr,
std::hash<uint64_t>, std::equal_to<uint64_t>,
RebindAlloc<std::pair<const uint64_t, subscription::SubscriptionBase::WeakPtr>>>;
using IDTopicMap = std::map<AllocString, AllocSet,
std::less<AllocString>, RebindAlloc<std::pair<AllocString, AllocSet>>>;
using IDTopicMap = std::map<std::string, AllocSet,
std::less<std::string>, RebindAlloc<std::pair<std::string, AllocSet>>>;
SubscriptionMap subscriptions_;
@ -263,7 +257,6 @@ private:
RebindAlloc<std::pair<const uint64_t, PublisherInfo>>>;
PublisherMap publishers_;
};
static IntraProcessManagerStateBase::SharedPtr create_default_state()
@ -271,7 +264,7 @@ static IntraProcessManagerStateBase::SharedPtr create_default_state()
return std::make_shared<IntraProcessManagerState<>>();
}
}
}
} // namespace intra_process_manager
} // namespace rclcpp
#endif
#endif // RCLCPP__INTRA_PROCESS_MANAGER_STATE_HPP_

19
rclcpp/include/rclcpp/mapped_ring_buffer.hpp
View file

@ -12,11 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_RING_BUFFER_HPP_
#define RCLCPP_RCLCPP_RING_BUFFER_HPP_
#include <rclcpp/allocator/allocator_common.hpp>
#include <rclcpp/macros.hpp>
#ifndef RCLCPP__MAPPED_RING_BUFFER_HPP_
#define RCLCPP__MAPPED_RING_BUFFER_HPP_
#include <algorithm>
#include <cstddef>
@ -24,6 +21,9 @@
#include <memory>
#include <vector>
#include "rclcpp/allocator/allocator_common.hpp"
#include "rclcpp/macros.hpp"
namespace rclcpp
{
namespace mapped_ring_buffer
@ -67,7 +67,7 @@ public:
*
* \param size size of the ring buffer; must be positive and non-zero.
*/
MappedRingBuffer(size_t size, std::shared_ptr<Alloc> allocator = nullptr)
explicit MappedRingBuffer(size_t size, std::shared_ptr<Alloc> allocator = nullptr)
: elements_(size), head_(0)
{
if (size == 0) {
@ -224,10 +224,9 @@ private:
std::vector<element, VectorAlloc> elements_;
size_t head_;
std::shared_ptr<ElemAlloc> allocator_;
};
} /* namespace ring_buffer */
} /* namespace rclcpp */
} // namespace mapped_ring_buffer
} // namespace rclcpp
#endif /* RCLCPP_RCLCPP_RING_BUFFER_HPP_ */
#endif // RCLCPP__MAPPED_RING_BUFFER_HPP_

16
rclcpp/include/rclcpp/memory_strategies.hpp
View file

@ -12,11 +12,9 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_MEMORY_STRATEGIES_HPP_
#define RCLCPP_RCLCPP_MEMORY_STRATEGIES_HPP_
#ifndef RCLCPP__MEMORY_STRATEGIES_HPP_
#define RCLCPP__MEMORY_STRATEGIES_HPP_
//#include <rclcpp/strategies/heap_pool_memory_strategy.hpp>
//#include <rclcpp/strategies/stack_pool_memory_strategy.hpp>
#include <rclcpp/memory_strategy.hpp>
#include <rclcpp/strategies/allocator_memory_strategy.hpp>
@ -26,15 +24,13 @@ namespace memory_strategies
{
using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy;
//using rclcpp::memory_strategies::heap_pool_memory_strategy::HeapPoolMemoryStrategy;
//using rclcpp::memory_strategies::stack_pool_memory_strategy::StackPoolMemoryStrategy;
static memory_strategy::MemoryStrategy::SharedPtr create_default_strategy()
{
return std::make_shared<memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy<>>();
return std::make_shared<AllocatorMemoryStrategy<>>();
}
} /* memory_strategies */
} /* rclcpp */
} // namespace memory_strategies
} // namespace rclcpp
#endif
#endif // RCLCPP__MEMORY_STRATEGIES_HPP_

83
rclcpp/include/rclcpp/memory_strategy.hpp
View file

@ -12,22 +12,21 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_MEMORY_STRATEGY_HPP_
#define RCLCPP_RCLCPP_MEMORY_STRATEGY_HPP_
#ifndef RCLCPP__MEMORY_STRATEGY_HPP_
#define RCLCPP__MEMORY_STRATEGY_HPP_
#include <memory>
#include <vector>
#include <rclcpp/any_executable.hpp>
#include <rclcpp/macros.hpp>
#include <rclcpp/node.hpp>
#include "rclcpp/any_executable.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/node.hpp"
namespace rclcpp
{
namespace memory_strategy
{
/// Delegate for handling memory allocations while the Executor is executing.
/**
* By default, the memory strategy dynamically allocates memory for structures that come in from
@ -52,6 +51,7 @@ public:
virtual void clear_active_entities() = 0;
virtual void clear_handles() = 0;
virtual void revalidate_handles() = 0;
virtual bool collect_entities(const WeakNodeVector & weak_nodes) = 0;
/// Provide a newly initialized AnyExecutable object.
@ -103,8 +103,7 @@ public:
}
static rclcpp::service::ServiceBase::SharedPtr
get_service_by_handle(void * service_handle,
const WeakNodeVector & weak_nodes)
get_service_by_handle(void * service_handle, const WeakNodeVector & weak_nodes)
{
for (auto & weak_node : weak_nodes) {
auto node = weak_node.lock();
@ -123,12 +122,11 @@ public:
}
}
}
return rclcpp::service::ServiceBase::SharedPtr();
return nullptr;
}
static rclcpp::client::ClientBase::SharedPtr
get_client_by_handle(void * client_handle,
const WeakNodeVector & weak_nodes)
get_client_by_handle(void * client_handle, const WeakNodeVector & weak_nodes)
{
for (auto & weak_node : weak_nodes) {
auto node = weak_node.lock();
@ -147,7 +145,7 @@ public:
}
}
}
return rclcpp::client::ClientBase::SharedPtr();
return nullptr;
}
static rclcpp::node::Node::SharedPtr
@ -155,7 +153,7 @@ public:
const WeakNodeVector & weak_nodes)
{
if (!group) {
return rclcpp::node::Node::SharedPtr();
return nullptr;
}
for (auto & weak_node : weak_nodes) {
auto node = weak_node.lock();
@ -169,7 +167,7 @@ public:
}
}
}
return rclcpp::node::Node::SharedPtr();
return nullptr;
}
static rclcpp::callback_group::CallbackGroup::SharedPtr
@ -195,14 +193,63 @@ public:
}
}
}
return rclcpp::callback_group::CallbackGroup::SharedPtr();
return nullptr;
}
static rclcpp::callback_group::CallbackGroup::SharedPtr
get_group_by_service(
rclcpp::service::ServiceBase::SharedPtr service,
const WeakNodeVector & weak_nodes)
{
for (auto & weak_node : weak_nodes) {
auto node = weak_node.lock();
if (!node) {
continue;
}
for (auto & weak_group : node->get_callback_groups()) {
auto group = weak_group.lock();
if (!group) {
continue;
}
for (auto & serv : group->get_service_ptrs()) {
if (serv == service) {
return group;
}
}
}
}
return nullptr;
}
static rclcpp::callback_group::CallbackGroup::SharedPtr
get_group_by_client(rclcpp::client::ClientBase::SharedPtr client,
const WeakNodeVector & weak_nodes)
{
for (auto & weak_node : weak_nodes) {
auto node = weak_node.lock();
if (!node) {
continue;
}
for (auto & weak_group : node->get_callback_groups()) {
auto group = weak_group.lock();
if (!group) {
continue;
}
for (auto & cli : group->get_client_ptrs()) {
if (cli == client) {
return group;
}
}
}
}
return nullptr;
}
};
} /* memory_strategy */
} // namespace memory_strategy
} /* rclcpp */
} // namespace rclcpp
#endif
#endif // RCLCPP__MEMORY_STRATEGY_HPP_

17
rclcpp/include/rclcpp/message_memory_strategy.hpp
View file

@ -12,13 +12,13 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_MSG_MEMORY_STRATEGY_HPP_
#define RCLCPP_RCLCPP_MSG_MEMORY_STRATEGY_HPP_
#ifndef RCLCPP__MESSAGE_MEMORY_STRATEGY_HPP_
#define RCLCPP__MESSAGE_MEMORY_STRATEGY_HPP_
#include <memory>
#include <rclcpp/allocator/allocator_common.hpp>
#include <rclcpp/macros.hpp>
#include "rclcpp/allocator/allocator_common.hpp"
#include "rclcpp/macros.hpp"
namespace rclcpp
{
@ -30,7 +30,6 @@ namespace message_memory_strategy
template<typename MessageT, typename Alloc = std::allocator<void>>
class MessageMemoryStrategy
{
public:
RCLCPP_SMART_PTR_DEFINITIONS(MessageMemoryStrategy);
@ -43,7 +42,7 @@ public:
message_allocator_ = std::make_shared<MessageAlloc>();
}
MessageMemoryStrategy(std::shared_ptr<Alloc> allocator)
explicit MessageMemoryStrategy(std::shared_ptr<Alloc> allocator)
{
message_allocator_ = std::make_shared<MessageAlloc>(*allocator.get());
}
@ -72,7 +71,7 @@ public:
MessageDeleter message_deleter_;
};
} /* message_memory_strategy */
} /* rclcpp */
} // namespace message_memory_strategy
} // namespace rclcpp
#endif /* RCLCPP_RCLCPP_MSG_MEMORY_STRATEGY_HPP_ */
#endif // RCLCPP__MESSAGE_MEMORY_STRATEGY_HPP_

46
rclcpp/include/rclcpp/node.hpp
View file

@ -12,48 +12,48 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_NODE_HPP_
#define RCLCPP_RCLCPP_NODE_HPP_
#ifndef RCLCPP__NODE_HPP_
#define RCLCPP__NODE_HPP_
#include <list>
#include <map>
#include <memory>
#include <string>
#include <tuple>
#include <vector>
#include <rcl_interfaces/msg/list_parameters_result.hpp>
#include <rcl_interfaces/msg/parameter_descriptor.hpp>
#include <rcl_interfaces/msg/parameter_event.hpp>
#include <rcl_interfaces/msg/set_parameters_result.hpp>
#include <rosidl_generator_cpp/message_type_support.hpp>
#include "rcl_interfaces/msg/list_parameters_result.hpp"
#include "rcl_interfaces/msg/parameter_descriptor.hpp"
#include "rcl_interfaces/msg/parameter_event.hpp"
#include "rcl_interfaces/msg/set_parameters_result.hpp"
#include "rosidl_generator_cpp/message_type_support.hpp"
#include <rclcpp/callback_group.hpp>
#include <rclcpp/client.hpp>
#include <rclcpp/context.hpp>
#include <rclcpp/macros.hpp>
#include <rclcpp/message_memory_strategy.hpp>
#include <rclcpp/parameter.hpp>
#include <rclcpp/publisher.hpp>
#include <rclcpp/service.hpp>
#include <rclcpp/subscription.hpp>
#include <rclcpp/timer.hpp>
#include "rclcpp/callback_group.hpp"
#include "rclcpp/client.hpp"
#include "rclcpp/context.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/message_memory_strategy.hpp"
#include "rclcpp/parameter.hpp"
#include "rclcpp/publisher.hpp"
#include "rclcpp/service.hpp"
#include "rclcpp/subscription.hpp"
#include "rclcpp/timer.hpp"
// Forward declaration of ROS middleware class
namespace rmw
{
struct rmw_node_t;
} // namespace rmw
} // namespace rmw
namespace rclcpp
{
namespace node
{
/// Node is the single point of entry for creating publishers and subscribers.
class Node
{
public:
RCLCPP_SMART_PTR_DEFINITIONS(Node);
@ -63,7 +63,7 @@ public:
* \param[in] use_intra_process_comms True to use the optimized intra-process communication
* pipeline to pass messages between nodes in the same process using shared memory.
*/
Node(const std::string & node_name, bool use_intra_process_comms = false);
explicit Node(const std::string & node_name, bool use_intra_process_comms = false);
/// Create a node based on the node name and a rclcpp::context::Context.
/**
@ -278,9 +278,9 @@ const rosidl_message_type_support_t * Node::ipm_ts_ =
make_shared())); \
}
#ifndef RCLCPP_RCLCPP_NODE_IMPL_HPP_
#ifndef RCLCPP__NODE_IMPL_HPP_
// Template implementations
#include "node_impl.hpp"
#endif
#endif /* RCLCPP_RCLCPP_NODE_HPP_ */
#endif // RCLCPP__NODE_HPP_

44
rclcpp/include/rclcpp/node_impl.hpp
View file

@ -12,34 +12,38 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_NODE_IMPL_HPP_
#define RCLCPP_RCLCPP_NODE_IMPL_HPP_
#ifndef RCLCPP__NODE_IMPL_HPP_
#define RCLCPP__NODE_IMPL_HPP_
#include <rmw/error_handling.h>
#include <rmw/rmw.h>
#include <algorithm>
#include <cstdlib>
#include <iostream>
#include <limits>
#include <map>
#include <memory>
#include <sstream>
#include <stdexcept>
#include <string>
#include <utility>
#include <vector>
#include <rcl_interfaces/msg/intra_process_message.hpp>
#include <rmw/error_handling.h>
#include <rmw/rmw.h>
#include <rosidl_generator_cpp/message_type_support.hpp>
#include <rosidl_generator_cpp/service_type_support.hpp>
#include "rcl_interfaces/msg/intra_process_message.hpp"
#include "rosidl_generator_cpp/message_type_support.hpp"
#include "rosidl_generator_cpp/service_type_support.hpp"
#include <rclcpp/contexts/default_context.hpp>
#include <rclcpp/intra_process_manager.hpp>
#include <rclcpp/parameter.hpp>
#include "rclcpp/contexts/default_context.hpp"
#include "rclcpp/intra_process_manager.hpp"
#include "rclcpp/parameter.hpp"
#ifndef RCLCPP_RCLCPP_NODE_HPP_
#ifndef RCLCPP__NODE_HPP_
#include "node.hpp"
#endif
using namespace rclcpp;
using namespace rclcpp::node;
using namespace node;
Node::Node(const std::string & node_name, bool use_intra_process_comms)
: Node(
@ -189,7 +193,8 @@ Node::create_publisher(
MessageT * typed_message_ptr = static_cast<MessageT *>(msg);
using MessageDeleter = typename publisher::Publisher<MessageT, Alloc>::MessageDeleter;
std::unique_ptr<MessageT, MessageDeleter> unique_msg(typed_message_ptr);
uint64_t message_seq = ipm->store_intra_process_message<MessageT, Alloc>(publisher_id, unique_msg);
uint64_t message_seq =
ipm->store_intra_process_message<MessageT, Alloc>(publisher_id, unique_msg);
return message_seq;
};
// *INDENT-ON*
@ -294,7 +299,8 @@ Node::create_subscription(
throw std::runtime_error(
"intra process take called after destruction of intra process manager");
}
ipm->take_intra_process_message<MessageT, Alloc>(publisher_id, message_sequence, subscription_id, message);
ipm->take_intra_process_message<MessageT, Alloc>(
publisher_id, message_sequence, subscription_id, message);
},
[weak_ipm](const rmw_gid_t * sender_gid) -> bool {
auto ipm = weak_ipm.lock();
@ -310,7 +316,7 @@ Node::create_subscription(
// Assign to a group.
if (group) {
if (!group_in_node(group)) {
// TODO: use custom exception
// TODO(jacquelinekay): use custom exception
throw std::runtime_error("Cannot create subscription, group not in node.");
}
group->add_subscription(sub_base_ptr);
@ -354,7 +360,7 @@ Node::create_wall_timer(
auto timer = rclcpp::timer::WallTimer::make_shared(period, callback);
if (group) {
if (!group_in_node(group)) {
// TODO: use custom exception
// TODO(jacquelinekay): use custom exception
throw std::runtime_error("Cannot create timer, group not in node.");
}
group->add_timer(timer);
@ -449,7 +455,7 @@ Node::create_service(
auto serv_base_ptr = std::dynamic_pointer_cast<service::ServiceBase>(serv);
if (group) {
if (!group_in_node(group)) {
// TODO: use custom exception
// TODO(jacquelinekay): use custom exception
throw std::runtime_error("Cannot create service, group not in node.");
}
group->add_service(serv_base_ptr);
@ -497,7 +503,7 @@ Node::set_parameters_atomically(
tmp_map.insert(parameters_.begin(), parameters_.end());
std::swap(tmp_map, parameters_);
// TODO: handle parameter constraints
// TODO(jacquelinekay): handle parameter constraints
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
@ -667,4 +673,4 @@ Node::get_callback_groups() const
return callback_groups_;
}
#endif /* RCLCPP_RCLCPP_NODE_IMPL_HPP_ */
#endif // RCLCPP__NODE_IMPL_HPP_

25
rclcpp/include/rclcpp/publisher.hpp
View file

@ -12,10 +12,11 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_PUBLISHER_HPP_
#define RCLCPP_RCLCPP_PUBLISHER_HPP_
#ifndef RCLCPP__PUBLISHER_HPP_
#define RCLCPP__PUBLISHER_HPP_
#include <rclcpp/macros.hpp>
#include <rmw/error_handling.h>
#include <rmw/rmw.h>
#include <iostream>
#include <memory>
@ -23,12 +24,11 @@
#include <sstream>
#include <string>
#include <rcl_interfaces/msg/intra_process_message.hpp>
#include <rmw/impl/cpp/demangle.hpp>
#include <rmw/error_handling.h>
#include <rmw/rmw.h>
#include "rcl_interfaces/msg/intra_process_message.hpp"
#include "rmw/impl/cpp/demangle.hpp"
#include <rclcpp/allocator/allocator_deleter.hpp>
#include "rclcpp/allocator/allocator_deleter.hpp"
#include "rclcpp/macros.hpp"
namespace rclcpp
{
@ -37,7 +37,7 @@ namespace rclcpp
namespace node
{
class Node;
} // namespace node
} // namespace node
namespace publisher
{
@ -346,10 +346,9 @@ protected:
std::shared_ptr<MessageAlloc> message_allocator_;
MessageDeleter message_deleter_;
};
} /* namespace publisher */
} /* namespace rclcpp */
} // namespace publisher
} // namespace rclcpp
#endif /* RCLCPP_RCLCPP_PUBLISHER_HPP_ */
#endif // RCLCPP__PUBLISHER_HPP_

179
rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
View file

@ -12,15 +12,15 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_ALLOCATOR_MEMORY_STRATEGY_HPP_
#define RCLCPP_RCLCPP_ALLOCATOR_MEMORY_STRATEGY_HPP_
#ifndef RCLCPP__STRATEGIES__ALLOCATOR_MEMORY_STRATEGY_HPP_
#define RCLCPP__STRATEGIES__ALLOCATOR_MEMORY_STRATEGY_HPP_
#include <memory>
#include <vector>
#include <rclcpp/allocator/allocator_common.hpp>
#include <rclcpp/memory_strategy.hpp>
#include <rclcpp/node.hpp>
#include "rclcpp/allocator/allocator_common.hpp"
#include "rclcpp/memory_strategy.hpp"
#include "rclcpp/node.hpp"
namespace rclcpp
{
@ -40,7 +40,6 @@ namespace allocator_memory_strategy
template<typename Alloc = std::allocator<void>>
class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
{
public:
RCLCPP_SMART_PTR_DEFINITIONS(AllocatorMemoryStrategy<Alloc>);
@ -52,7 +51,7 @@ public:
using WeakNodeVector = std::vector<std::weak_ptr<node::Node>>;
AllocatorMemoryStrategy(std::shared_ptr<Alloc> allocator)
explicit AllocatorMemoryStrategy(std::shared_ptr<Alloc> allocator)
{
executable_allocator_ = std::make_shared<ExecAlloc>(*allocator.get());
allocator_ = std::make_shared<VoidAlloc>(*allocator.get());
@ -66,48 +65,34 @@ public:
size_t fill_subscriber_handles(void ** & ptr)
{
size_t max_size = subscriptions_.size();
subscriber_handles_.resize(max_size);
size_t count = 0;
for (auto & subscription : subscriptions_) {
subscriber_handles_[count++] = subscription->get_subscription_handle()->data;
subscriber_handles_.push_back(subscription->get_subscription_handle()->data);
if (subscription->get_intra_process_subscription_handle()) {
subscriber_handles_[count++] =
subscription->get_intra_process_subscription_handle()->data;
subscriber_handles_.push_back(subscription->get_intra_process_subscription_handle()->data);
}
}
ptr = subscriber_handles_.data();
return count;
return subscriber_handles_.size();
}
// return the new number of services
size_t fill_service_handles(void ** & ptr)
{
size_t max_size = services_.size();
if (service_handles_.size() < max_size) {
service_handles_.resize(max_size);
}
size_t count = 0;
for (auto & service : services_) {
service_handles_[count++] = service->get_service_handle()->data;
service_handles_.push_back(service->get_service_handle()->data);
}
ptr = service_handles_.data();
return count;
return service_handles_.size();
}
// return the new number of clients
size_t fill_client_handles(void ** & ptr)
{
size_t max_size = clients_.size();
if (client_handles_.size() < max_size) {
client_handles_.resize(max_size);
}
size_t count = 0;
for (auto & client : clients_) {
client_handles_[count++] = client->get_client_handle()->data;
client_handles_.push_back(client->get_client_handle()->data);
}
ptr = client_handles_.data();
return count;
return client_handles_.size();
}
void clear_active_entities()
@ -124,6 +109,37 @@ public:
client_handles_.clear();
}
void revalidate_handles()
{
{
VectorRebind<void *> temp;
for (size_t i = 0; i < subscriptions_.size() * 2; i++) {
if (subscriber_handles_[i]) {
temp.push_back(subscriber_handles_[i]);
}
}
subscriber_handles_.swap(temp);
}
{
VectorRebind<void *> temp;
for (size_t i = 0; i < services_.size(); i++) {
if (service_handles_[i]) {
temp.push_back(service_handles_[i]);
}
}
service_handles_.swap(temp);
}
{
VectorRebind<void *> temp;
for (size_t i = 0; i < clients_.size(); i++) {
if (client_handles_[i]) {
temp.push_back(client_handles_[i]);
}
}
client_handles_.swap(temp);
}
}
bool collect_entities(const WeakNodeVector & weak_nodes)
{
bool has_invalid_weak_nodes = false;
@ -145,10 +161,14 @@ public:
}
}
for (auto & service : group->get_service_ptrs()) {
services_.push_back(service);
if (service) {
services_.push_back(service);
}
}
for (auto & client : group->get_client_ptrs()) {
clients_.push_back(client);
if (client) {
clients_.push_back(client);
}
}
}
}
@ -168,10 +188,8 @@ public:
const WeakNodeVector & weak_nodes)
{
auto it = subscriber_handles_.begin();
VectorRebind<typename VectorRebind<void *>::iterator> to_erase;
while (it != subscriber_handles_.end()) {
auto subscription = memory_strategy::MemoryStrategy::get_subscription_by_handle(*it,
weak_nodes);
auto subscription = get_subscription_by_handle(*it, weak_nodes);
if (subscription) {
// Figure out if this is for intra-process or not.
bool is_intra_process = false;
@ -183,8 +201,7 @@ public:
if (!group) {
// Group was not found, meaning the subscription is not valid...
// Remove it from the ready list and continue looking
//subscriber_handles_.erase(it++);
to_erase.push_back(it++);
subscriber_handles_.erase(it);
continue;
}
if (!group->can_be_taken_from().load()) {
@ -201,44 +218,12 @@ public:
}
any_exec->callback_group = group;
any_exec->node = get_node_by_group(group, weak_nodes);
to_erase.push_back(it++);
subscriber_handles_.erase(it);
return;
}
// Else, the subscription is no longer valid, remove it and continue
// TODO
to_erase.push_back(it++);
subscriber_handles_.erase(it);
}
for (auto & entry : to_erase) {
if (entry != subscriber_handles_.end()) {
subscriber_handles_.erase(entry);
}
}
}
static rclcpp::callback_group::CallbackGroup::SharedPtr
get_group_by_service(
rclcpp::service::ServiceBase::SharedPtr service,
const WeakNodeVector & weak_nodes)
{
for (auto & weak_node : weak_nodes) {
auto node = weak_node.lock();
if (!node) {
continue;
}
for (auto & weak_group : node->get_callback_groups()) {
auto group = weak_group.lock();
if (!group) {
continue;
}
for (auto & serv : group->get_service_ptrs()) {
if (serv == service) {
return group;
}
}
}
}
return rclcpp::callback_group::CallbackGroup::SharedPtr();
}
virtual void
@ -247,14 +232,14 @@ public:
{
auto it = service_handles_.begin();
while (it != service_handles_.end()) {
auto service = memory_strategy::MemoryStrategy::get_service_by_handle(*it, weak_nodes);
auto service = get_service_by_handle(*it, weak_nodes);
if (service) {
// Find the group for this handle and see if it can be serviced
auto group = get_group_by_service(service, weak_nodes);
if (!group) {
// Group was not found, meaning the service is not valid...
// Remove it from the ready list and continue looking
service_handles_.erase(it++);
service_handles_.erase(it);
continue;
}
if (!group->can_be_taken_from().load()) {
@ -267,53 +252,27 @@ public:
any_exec->service = service;
any_exec->callback_group = group;
any_exec->node = get_node_by_group(group, weak_nodes);
service_handles_.erase(it++);
service_handles_.erase(it);
return;
}
// Else, the service is no longer valid, remove it and continue
service_handles_.erase(it++);
service_handles_.erase(it);
}
}
static rclcpp::callback_group::CallbackGroup::SharedPtr
get_group_by_client(
rclcpp::client::ClientBase::SharedPtr client,
const WeakNodeVector & weak_nodes)
{
for (auto & weak_node : weak_nodes) {
auto node = weak_node.lock();
if (!node) {
continue;
}
for (auto & weak_group : node->get_callback_groups()) {
auto group = weak_group.lock();
if (!group) {
continue;
}
for (auto & cli : group->get_client_ptrs()) {
if (cli == client) {
return group;
}
}
}
}
return rclcpp::callback_group::CallbackGroup::SharedPtr();
}
virtual void
get_next_client(executor::AnyExecutable::SharedPtr any_exec,
const WeakNodeVector & weak_nodes)
get_next_client(executor::AnyExecutable::SharedPtr any_exec, const WeakNodeVector & weak_nodes)
{
auto it = client_handles_.begin();
while (it != client_handles_.end()) {
auto client = memory_strategy::MemoryStrategy::get_client_by_handle(*it, weak_nodes);
auto client = get_client_by_handle(*it, weak_nodes);
if (client) {
// Find the group for this handle and see if it can be serviced
auto group = get_group_by_client(client, weak_nodes);
if (!group) {
// Group was not found, meaning the service is not valid...
// Remove it from the ready list and continue looking
client_handles_.erase(it++);
client_handles_.erase(it);
continue;
}
if (!group->can_be_taken_from().load()) {
@ -326,18 +285,18 @@ public:
any_exec->client = client;
any_exec->callback_group = group;
any_exec->node = get_node_by_group(group, weak_nodes);
client_handles_.erase(it++);
client_handles_.erase(it);
return;
}
// Else, the service is no longer valid, remove it and continue
client_handles_.erase(it++);
client_handles_.erase(it);
}
}
private:
template<typename U>
template<typename T>
using VectorRebind =
std::vector<U, typename std::allocator_traits<Alloc>::template rebind_alloc<U>>;
std::vector<T, typename std::allocator_traits<Alloc>::template rebind_alloc<T>>;
VectorRebind<rclcpp::subscription::SubscriptionBase::SharedPtr> subscriptions_;
VectorRebind<rclcpp::service::ServiceBase::SharedPtr> services_;
@ -351,9 +310,9 @@ private:
std::shared_ptr<VoidAlloc> allocator_;
};
} /* allocator_memory_strategy */
} /* memory_strategies */
} // namespace allocator_memory_strategy
} // namespace memory_strategies
} /* rclcpp */
} // namespace rclcpp
#endif
#endif // RCLCPP__STRATEGIES__ALLOCATOR_MEMORY_STRATEGY_HPP_

13
rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp
View file

@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_MSG_POOL_MEMORY_STRATEGY_HPP_
#define RCLCPP_RCLCPP_MSG_POOL_MEMORY_STRATEGY_HPP_
#ifndef RCLCPP__STRATEGIES__MESSAGE_POOL_MEMORY_STRATEGY_HPP_
#define RCLCPP__STRATEGIES__MESSAGE_POOL_MEMORY_STRATEGY_HPP_
#include <rclcpp/macros.hpp>
#include <rclcpp/message_memory_strategy.hpp>
@ -96,10 +96,9 @@ protected:
std::array<PoolMember, Size> pool_;
size_t next_array_index_;
};
} /* message_pool_memory_strategy */
} /* strategies */
} /* rclcpp */
#endif /* RCLCPP_RCLCPP_MSG_POOL_MEMORY_STRATEGY_HPP_ */
} // namespace message_pool_memory_strategy
} // namespace strategies
} // namespace rclcpp
#endif // RCLCPP__STRATEGIES__MESSAGE_POOL_MEMORY_STRATEGY_HPP_

27
rclcpp/include/rclcpp/subscription.hpp
View file

@ -12,8 +12,11 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_RCLCPP_SUBSCRIPTION_HPP_
#define RCLCPP_RCLCPP_SUBSCRIPTION_HPP_
#ifndef RCLCPP__SUBSCRIPTION_HPP_
#define RCLCPP__SUBSCRIPTION_HPP_
#include <rmw/error_handling.h>
#include <rmw/rmw.h>
#include <functional>
#include <iostream>
@ -21,13 +24,11 @@
#include <sstream>
#include <string>
#include <rcl_interfaces/msg/intra_process_message.hpp>
#include <rmw/error_handling.h>
#include <rmw/rmw.h>
#include "rcl_interfaces/msg/intra_process_message.hpp"
#include <rclcpp/macros.hpp>
#include <rclcpp/message_memory_strategy.hpp>
#include <rclcpp/any_subscription_callback.hpp>
#include "rclcpp/macros.hpp"
#include "rclcpp/message_memory_strategy.hpp"
#include "rclcpp/any_subscription_callback.hpp"
namespace rclcpp
{
@ -35,7 +36,7 @@ namespace rclcpp
namespace node
{
class Node;
} // namespace node
} // namespace node
namespace subscription
{
@ -44,7 +45,6 @@ namespace subscription
/// specializations of Subscription, among other things.
class SubscriptionBase
{
public:
RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(SubscriptionBase);
@ -138,7 +138,6 @@ private:
std::string topic_name_;
bool ignore_local_publications_;
};
using namespace any_subscription_callback;
@ -279,7 +278,7 @@ private:
uint64_t intra_process_subscription_id_;
};
} /* namespace subscription */
} /* namespace rclcpp */
} // namespace subscription
} // namespace rclcpp
#endif /* RCLCPP_RCLCPP_SUBSCRIPTION_HPP_ */
#endif // RCLCPP__SUBSCRIPTION_HPP_

5
rclcpp/test/test_intra_process_manager.cpp
View file

@ -15,6 +15,7 @@
#include <memory>
#include <gtest/gtest.h>
#include <rclcpp/allocator/allocator_common.hpp>
#include <rclcpp/macros.hpp>
#include <rmw/types.h>
@ -115,8 +116,8 @@ public:
}
// Prevent rclcpp/publisher.hpp and rclcpp/subscription.hpp from being imported.
#define RCLCPP_RCLCPP_PUBLISHER_HPP_
#define RCLCPP_RCLCPP_SUBSCRIPTION_HPP_
#define RCLCPP__PUBLISHER_HPP_
#define RCLCPP__SUBSCRIPTION_HPP_
// Force ipm to use our mock publisher class.
#define Publisher mock::Publisher
#define PublisherBase mock::PublisherBase