Skip to content

Commit

Permalink
first try of node like something
Browse files Browse the repository at this point in the history
Signed-off-by: Karsten Knese <karsten@openrobotics.org>
  • Loading branch information
Karsten1987 committed May 9, 2019
1 parent ef41059 commit e6dd86d
Show file tree
Hide file tree
Showing 3 changed files with 160 additions and 3 deletions.
35 changes: 32 additions & 3 deletions rclcpp/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -85,19 +85,40 @@ configure_file(
COPYONLY
)
# generate header with logging macros
set(python_code
set(python_code_logging
"import em"
"em.invoke(['-o', 'include/rclcpp/logging.hpp', '${CMAKE_CURRENT_SOURCE_DIR}/resource/logging.hpp.em'])")
string(REPLACE ";" "$<SEMICOLON>" python_code "${python_code}")
string(REPLACE ";" "$<SEMICOLON>" python_code_logging "${python_code_logging}")
add_custom_command(OUTPUT include/rclcpp/logging.hpp
COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp"
COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_code}"
COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_code_logging}"
DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/logging.hpp.em.watch"
COMMENT "Expanding logging.hpp.em"
VERBATIM
)
list(APPEND ${PROJECT_NAME}_SRCS
include/rclcpp/logging.hpp)

# "watch" template for changes
configure_file(
"resource/interface_traits.hpp.em"
"interface_traits.hpp.em.watch"
COPYONLY
)
set(python_code_interface_traits
"import em"
"em.invoke(['-o', 'include/rclcpp/node_interfaces/interface_traits.hpp', '${CMAKE_CURRENT_SOURCE_DIR}/resource/interface_traits.hpp.em'])")
string(REPLACE ";" "$<SEMICOLON>" python_code_interface_traits "${python_code_interface_traits}")
add_custom_command(OUTPUT include/rclcpp/node_interfaces/interface_traits.hpp
COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp/node_interfaces"
COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_code_interface_traits}"
DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/interface_traits.hpp.em.watch"
COMMENT "Expanding interfae_traits.hpp.em"
VERBATIM
)
list(APPEND ${PROJECT_NAME}_SRCS
include/rclcpp/node_interfaces/interface_traits.hpp)

include_directories("${CMAKE_CURRENT_BINARY_DIR}/include")

add_library(${PROJECT_NAME}
Expand Down Expand Up @@ -448,6 +469,14 @@ if(BUILD_TESTING)
target_link_libraries(test_init ${PROJECT_NAME})
endif()

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

ament_add_gtest(test_multi_threaded_executor test/executors/test_multi_threaded_executor.cpp
APPEND_LIBRARY_DIRS "${append_library_dirs}")
if(TARGET test_multi_threaded_executor)
Expand Down
77 changes: 77 additions & 0 deletions rclcpp/resource/interface_traits.hpp.em
Original file line number Diff line number Diff line change
@@ -0,0 +1,77 @@
// Copyright 2019 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__NODE_INTERFACES__INTERFACE_TRAITS_HPP_
#define RCLCPP__NODE_INTERFACES__INTERFACE_TRAITS_HPP_

#include <functional>
#include <type_traits>

@{
node_interfaces = [
'node_base_interface',
'node_clock_interface',
'node_graph_interface',
'node_logging_interface',
'node_parameters_interface',
'node_services_interface',
'node_time_source_interface',
'node_timers_interface',
'node_topics_interface',
'node_waitables_interface',
]

node_interface_types = [
'NodeBaseInterface',
'NodeClockInterface',
'NodeGraphInterface',
'NodeLoggingInterface',
'NodeParametersInterface',
'NodeServicesInterface',
'NodeTimeSourceInterface',
'NodeTimersInterface',
'NodeTopicsInterface',
'NodeWaitablesInterface',
]

assert (len(node_interfaces) == len(node_interface_types))
}@

@[for interface_ in node_interfaces]@
#include "rclcpp/node_interfaces/@(interface_).hpp"
@[end for]@

namespace rclcpp
{
namespace node_interfaces
{

@[for (interface_, type_) in zip(node_interfaces, node_interface_types)]@
using @(interface_)_getter_t = std::shared_ptr<rclcpp::node_interfaces::@(type_)>;

template<class T, typename = void>
struct has_@(interface_) : std::false_type
{};

template<class T>
struct has_@(interface_)<
T, typename std::enable_if<
std::is_same<
@(interface_)_getter_t, decltype(std::declval<T>().get_@(interface_)())>::value>::type> : std::true_type
{};

@[end for]@
} // namespace node_interfaces
} // namespace rclcpp
#endif // RCLCPP__NODE_INTERFACES__INTERFACE_TRAITS_HPP_
51 changes: 51 additions & 0 deletions rclcpp/test/test_interface_traits.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,51 @@
// Copyright 2019 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 "rclcpp/node_interfaces/interface_traits.hpp"
#include "rclcpp/node_interfaces/node_base.hpp"
#include "rclcpp/node.hpp"

class MyNode
{
public:
std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> get_node_base_interface() const
{
return std::make_shared<rclcpp::node_interfaces::NodeBase>("my_node_name", "my_node_namespace", nullptr, rclcpp::NodeOptions());
}
};

class WrongNode
{
public:
std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> not_get_node_base_interface()
{
return nullptr;
}
};

template<class T, typename std::enable_if<rclcpp::node_interfaces::has_node_base_interface<T>::value>::type* = nullptr>
void get_node_name(const T & nodelike) {
ASSERT_STREQ("my_node_name", nodelike.get_node_base_interface()->get_name());
}

TEST(TestInterfaceTraits, has_node_base_interface) {
ASSERT_TRUE(rclcpp::node_interfaces::has_node_base_interface<MyNode>::value);
ASSERT_FALSE(rclcpp::node_interfaces::has_node_base_interface<WrongNode>::value);
ASSERT_TRUE(rclcpp::node_interfaces::has_node_base_interface<rclcpp::Node>::value);

get_node_name(MyNode());
}

0 comments on commit e6dd86d

Please sign in to comment.