Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[rclcpp] add WaitSet class and modify entities to work without executor #1047

Merged
merged 21 commits into from
Apr 13, 2020
Merged
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
add rclcpp::MessageInfo to replace use of rmw_message_info_t
Signed-off-by: William Woodall <[email protected]>
wjwwood committed Apr 13, 2020
commit 2866c720c87ca4cfad15fce8e8b12580043ce757
1 change: 1 addition & 0 deletions rclcpp/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -55,6 +55,7 @@ set(${PROJECT_NAME}_SRCS
src/rclcpp/logger.cpp
src/rclcpp/memory_strategies.cpp
src/rclcpp/memory_strategy.cpp
src/rclcpp/message_info.cpp
src/rclcpp/node.cpp
src/rclcpp/node_options.cpp
src/rclcpp/node_interfaces/node_base.cpp
13 changes: 7 additions & 6 deletions rclcpp/include/rclcpp/any_subscription_callback.hpp
Original file line number Diff line number Diff line change
@@ -25,6 +25,7 @@

#include "rclcpp/allocator/allocator_common.hpp"
#include "rclcpp/function_traits.hpp"
#include "rclcpp/message_info.hpp"
#include "rclcpp/visibility_control.hpp"
#include "tracetools/tracetools.h"
#include "tracetools/utils.hpp"
@@ -43,13 +44,13 @@ class AnySubscriptionCallback

using SharedPtrCallback = std::function<void (const std::shared_ptr<MessageT>)>;
using SharedPtrWithInfoCallback =
std::function<void (const std::shared_ptr<MessageT>, const rmw_message_info_t &)>;
std::function<void (const std::shared_ptr<MessageT>, const rclcpp::MessageInfo &)>;
using ConstSharedPtrCallback = std::function<void (const std::shared_ptr<const MessageT>)>;
using ConstSharedPtrWithInfoCallback =
std::function<void (const std::shared_ptr<const MessageT>, const rmw_message_info_t &)>;
std::function<void (const std::shared_ptr<const MessageT>, const rclcpp::MessageInfo &)>;
using UniquePtrCallback = std::function<void (MessageUniquePtr)>;
using UniquePtrWithInfoCallback =
std::function<void (MessageUniquePtr, const rmw_message_info_t &)>;
std::function<void (MessageUniquePtr, const rclcpp::MessageInfo &)>;

SharedPtrCallback shared_ptr_callback_;
SharedPtrWithInfoCallback shared_ptr_with_info_callback_;
@@ -155,7 +156,7 @@ class AnySubscriptionCallback
}

void dispatch(
std::shared_ptr<MessageT> message, const rmw_message_info_t & message_info)
std::shared_ptr<MessageT> message, const rclcpp::MessageInfo & message_info)
{
TRACEPOINT(callback_start, (const void *)this, false);
if (shared_ptr_callback_) {
@@ -181,7 +182,7 @@ class AnySubscriptionCallback
}

void dispatch_intra_process(
ConstMessageSharedPtr message, const rmw_message_info_t & message_info)
ConstMessageSharedPtr message, const rclcpp::MessageInfo & message_info)
{
TRACEPOINT(callback_start, (const void *)this, true);
if (const_shared_ptr_callback_) {
@@ -204,7 +205,7 @@ class AnySubscriptionCallback
}

void dispatch_intra_process(
MessageUniquePtr message, const rmw_message_info_t & message_info)
MessageUniquePtr message, const rclcpp::MessageInfo & message_info)
{
TRACEPOINT(callback_start, (const void *)this, true);
if (shared_ptr_callback_) {
52 changes: 52 additions & 0 deletions rclcpp/include/rclcpp/message_info.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,52 @@
// Copyright 2020 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef RCLCPP__MESSAGE_INFO_HPP_
#define RCLCPP__MESSAGE_INFO_HPP_

#include "rmw/types.h"

#include "rclcpp/visibility_control.hpp"

namespace rclcpp
{

/// Additional meta data about messages taken from subscriptions.
class RCLCPP_PUBLIC MessageInfo
{
public:
/// Default empty constructor.
MessageInfo() = default;

/// Conversion constructor, which is intentionally not marked as explicit.
// cppcheck-suppress noExplicitConstructor
MessageInfo(const rmw_message_info_t & rmw_message_info); // NOLINT(runtime/explicit)

virtual ~MessageInfo();

/// Return the message info as the underlying rmw message info type.
const rmw_message_info_t &
get_rmw_message_info() const;

/// Return the message info as the underlying rmw message info type.
rmw_message_info_t &
get_rmw_message_info();

private:
rmw_message_info_t rmw_message_info_;
};

} // namespace rclcpp

#endif // RCLCPP__MESSAGE_INFO_HPP_
15 changes: 9 additions & 6 deletions rclcpp/include/rclcpp/subscription.hpp
Original file line number Diff line number Diff line change
@@ -38,6 +38,7 @@
#include "rclcpp/experimental/subscription_intra_process.hpp"
#include "rclcpp/logging.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/message_info.hpp"
#include "rclcpp/message_memory_strategy.hpp"
#include "rclcpp/node_interfaces/node_base_interface.hpp"
#include "rclcpp/subscription_base.hpp"
@@ -212,20 +213,22 @@ class Subscription : public SubscriptionBase
* \throws any rcl errors from rcl_take, \sa rclcpp::exceptions::throw_from_rcl_error()
*/
bool
take(CallbackMessageT & message_out, rmw_message_info_t & message_info_out)
take(CallbackMessageT & message_out, rclcpp::MessageInfo & message_info_out)
{
rcl_ret_t ret = rcl_take(
this->get_subscription_handle().get(),
&message_out,
&message_info_out,
&message_info_out.get_rmw_message_info(),
nullptr // rmw_subscription_allocation_t is unused here
);
if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) {
return false;
} else if (RCL_RET_OK != ret) {
rclcpp::exceptions::throw_from_rcl_error(ret);
}
if (matches_any_intra_process_publishers(&message_info_out.publisher_gid)) {
if (
matches_any_intra_process_publishers(&message_info_out.get_rmw_message_info().publisher_gid))
{
// In this case, the message will be delivered via intra-process and
// we should ignore this copy of the message.
return false;
@@ -248,9 +251,9 @@ class Subscription : public SubscriptionBase
}

void handle_message(
std::shared_ptr<void> & message, const rmw_message_info_t & message_info) override
std::shared_ptr<void> & message, const rclcpp::MessageInfo & message_info) override
{
if (matches_any_intra_process_publishers(&message_info.publisher_gid)) {
if (matches_any_intra_process_publishers(&message_info.get_rmw_message_info().publisher_gid)) {
// In this case, the message will be delivered via intra process and
// we should ignore this copy of the message.
return;
@@ -261,7 +264,7 @@ class Subscription : public SubscriptionBase

void
handle_loaned_message(
void * loaned_message, const rmw_message_info_t & message_info) override
void * loaned_message, const rclcpp::MessageInfo & message_info) override
{
auto typed_message = static_cast<CallbackMessageT *>(loaned_message);
// message is loaned, so we have to make sure that the deleter does not deallocate the message
5 changes: 3 additions & 2 deletions rclcpp/include/rclcpp/subscription_base.hpp
Original file line number Diff line number Diff line change
@@ -27,6 +27,7 @@
#include "rclcpp/experimental/intra_process_manager.hpp"
#include "rclcpp/experimental/subscription_intra_process_base.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/message_info.hpp"
#include "rclcpp/qos.hpp"
#include "rclcpp/qos_event.hpp"
#include "rclcpp/type_support_decl.hpp"
@@ -132,12 +133,12 @@ class SubscriptionBase : public std::enable_shared_from_this<SubscriptionBase>
RCLCPP_PUBLIC
virtual
void
handle_message(std::shared_ptr<void> & message, const rmw_message_info_t & message_info) = 0;
handle_message(std::shared_ptr<void> & message, const rclcpp::MessageInfo & message_info) = 0;

RCLCPP_PUBLIC
virtual
void
handle_loaned_message(void * loaned_message, const rmw_message_info_t & message_info) = 0;
handle_loaned_message(void * loaned_message, const rclcpp::MessageInfo & message_info) = 0;

/// Return the message borrowed in create_message.
/** \param[in] message Shared pointer to the returned message. */
39 changes: 39 additions & 0 deletions rclcpp/src/rclcpp/message_info.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,39 @@
// Copyright 2020 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "rclcpp/message_info.hpp"

namespace rclcpp
{

MessageInfo::MessageInfo(const rmw_message_info_t & rmw_message_info)
: rmw_message_info_(rmw_message_info)
{}

MessageInfo::~MessageInfo()
{}

const rmw_message_info_t &
MessageInfo::get_rmw_message_info() const
{
return rmw_message_info_;
}

rmw_message_info_t &
MessageInfo::get_rmw_message_info()
{
return rmw_message_info_;
}

} // namespace rclcpp
4 changes: 2 additions & 2 deletions rclcpp/test/test_subscription.cpp
Original file line number Diff line number Diff line change
@@ -266,7 +266,7 @@ TEST_F(TestSubscription, take) {
{
auto sub = node->create_subscription<test_msgs::msg::Empty>("~/test_take", 1, do_nothing);
test_msgs::msg::Empty msg;
rmw_message_info_t msg_info;
rclcpp::MessageInfo msg_info;
EXPECT_FALSE(sub->take(msg, msg_info));
}
{
@@ -281,7 +281,7 @@ TEST_F(TestSubscription, take) {
pub->publish(msg);
}
test_msgs::msg::Empty msg;
rmw_message_info_t msg_info;
rclcpp::MessageInfo msg_info;
bool message_recieved = false;
auto start = std::chrono::steady_clock::now();
do {