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
Next Next commit
add rclcpp::GuardCondition wrapping rcl_guard_condition_t
Signed-off-by: William Woodall <[email protected]>
wjwwood committed Apr 13, 2020

Verified

This commit was signed with the committer’s verified signature. The key has expired.
wjwwood William Woodall
commit ee885e6e05552e92243dd13cb6693143a00d7412
7 changes: 7 additions & 0 deletions rclcpp/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -49,6 +49,7 @@ set(${PROJECT_NAME}_SRCS
src/rclcpp/executors/static_executor_entities_collector.cpp
src/rclcpp/executors/static_single_threaded_executor.cpp
src/rclcpp/graph_listener.cpp
src/rclcpp/guard_condition.cpp
src/rclcpp/init_options.cpp
src/rclcpp/intra_process_manager.cpp
src/rclcpp/logger.cpp
@@ -519,6 +520,12 @@ if(BUILD_TESTING)
target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME})
endif()

ament_add_gtest(test_guard_condition test/test_guard_condition.cpp
APPEND_LIBRARY_DIRS "${append_library_dirs}")
if(TARGET test_guard_condition)
target_link_libraries(test_guard_condition ${PROJECT_NAME})
endif()

# Install test resources
install(
DIRECTORY test/resources
83 changes: 83 additions & 0 deletions rclcpp/include/rclcpp/guard_condition.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,83 @@
// 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__GUARD_CONDITION_HPP_
#define RCLCPP__GUARD_CONDITION_HPP_

#include "rcl/guard_condition.h"

#include "rclcpp/context.hpp"
#include "rclcpp/contexts/default_context.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/visibility_control.hpp"

namespace rclcpp
{

/// A condition that can be waited on in a single wait set and asynchronously triggered.
class GuardCondition
{
public:
RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(GuardCondition)

// TODO(wjwwood): support custom allocator, maybe restrict to polymorphic allocator
/// Construct the guard condition, optionally specifying which Context to use.
/**
* \param[in] context Optional custom context to be used.
* Defaults to using the global default context singleton.
* Shared ownership of the context is held with the guard condition until
* destruction.
* \throws std::invalid_argument if the context is nullptr.
* \throws rclcpp::exceptions::RCLErrorBase based exceptions when underlying
* rcl functions fail.
*/
RCLCPP_PUBLIC
explicit GuardCondition(
rclcpp::Context::SharedPtr context =
rclcpp::contexts::default_context::get_global_default_context());

RCLCPP_PUBLIC
virtual
~GuardCondition();

/// Return the context used when creating this guard condition.
RCLCPP_PUBLIC
rclcpp::Context::SharedPtr
get_context() const;

/// Return the underlying rcl guard condition structure.
RCLCPP_PUBLIC
const rcl_guard_condition_t &
get_rcl_guard_condtion() const;

/// Notify the wait set waiting on this condition, if any, that the condition had been met.
/**
* This function is thread-safe, and may be called concurrently with waiting
* on this guard condition in a wait set.
*
* \throws rclcpp::exceptions::RCLErrorBase based exceptions when underlying
* rcl functions fail.
*/
RCLCPP_PUBLIC
void
trigger();

protected:
rclcpp::Context::SharedPtr context_;
rcl_guard_condition_t rcl_guard_condition_;
};

} // namespace rclcpp

#endif // RCLCPP__GUARD_CONDITION_HPP_
1 change: 1 addition & 0 deletions rclcpp/include/rclcpp/rclcpp.hpp
Original file line number Diff line number Diff line change
@@ -143,6 +143,7 @@
#include <memory>

#include "rclcpp/executors.hpp"
#include "rclcpp/guard_condition.hpp"
#include "rclcpp/logging.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/parameter.hpp"
74 changes: 74 additions & 0 deletions rclcpp/src/rclcpp/guard_condition.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,74 @@
// 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/guard_condition.hpp"

#include "rclcpp/exceptions.hpp"
#include "rclcpp/logging.hpp"

namespace rclcpp
{

GuardCondition::GuardCondition(rclcpp::Context::SharedPtr context)
: context_(context), rcl_guard_condition_{rcl_get_zero_initialized_guard_condition()}
{
if (!context_) {
throw std::invalid_argument("context argument unexpectedly nullptr");
}
rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options();
rcl_ret_t ret = rcl_guard_condition_init(
&this->rcl_guard_condition_,
context_->get_rcl_context().get(),
guard_condition_options);
if (RCL_RET_OK != ret) {
rclcpp::exceptions::throw_from_rcl_error(ret);
}
}

GuardCondition::~GuardCondition()
{
rcl_ret_t ret = rcl_guard_condition_fini(&this->rcl_guard_condition_);
if (RCL_RET_OK != ret) {
try {
rclcpp::exceptions::throw_from_rcl_error(ret);
} catch (const std::exception & exception) {
RCLCPP_ERROR(
rclcpp::get_logger("rclcpp"),
"Error in destruction of rcl guard condition: %s", exception.what());
}
}
}

rclcpp::Context::SharedPtr
GuardCondition::get_context() const
{
return context_;
}

const rcl_guard_condition_t &
GuardCondition::get_rcl_guard_condtion() const
{
return rcl_guard_condition_;
}

void
GuardCondition::trigger()
{
rcl_ret_t ret = rcl_trigger_guard_condition(&rcl_guard_condition_);
if (RCL_RET_OK != ret) {
rclcpp::exceptions::throw_from_rcl_error(ret);
}
}

} // namespace rclcpp
87 changes: 87 additions & 0 deletions rclcpp/test/test_guard_condition.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,87 @@
// 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 <gtest/gtest.h>

#include <memory>

#include "rclcpp/rclcpp.hpp"

class TestGuardCondition : public ::testing::Test
{
protected:
static void SetUpTestCase()
{
rclcpp::init(0, nullptr);
}
};

/*
* Testing normal construction and destruction.
*/
TEST_F(TestGuardCondition, construction_and_destruction) {
{
auto gc = std::make_shared<rclcpp::GuardCondition>();
(void)gc;
}

{
// invalid context (nullptr)
ASSERT_THROW(
{
auto gc = std::make_shared<rclcpp::GuardCondition>(nullptr);
(void)gc;
}, std::invalid_argument);
}

{
// invalid context (uninitialized)
auto context = std::make_shared<rclcpp::Context>();
ASSERT_THROW(
{
auto gc = std::make_shared<rclcpp::GuardCondition>(context);
(void)gc;
}, rclcpp::exceptions::RCLInvalidArgument);
}
}

/*
* Testing context accessor.
*/
TEST_F(TestGuardCondition, get_context) {
{
auto gc = std::make_shared<rclcpp::GuardCondition>();
gc->get_context();
}
}

/*
* Testing rcl guard condition accessor.
*/
TEST_F(TestGuardCondition, get_rcl_guard_condition) {
{
auto gc = std::make_shared<rclcpp::GuardCondition>();
gc->get_rcl_guard_condtion();
}
}

/*
* Testing tigger method.
*/
TEST_F(TestGuardCondition, trigger) {
{
auto gc = std::make_shared<rclcpp::GuardCondition>();
gc->trigger();
}
}