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

Use namespaces (#328)

Browse source 
* add expand_topic_or_service_name()

* use namespace in intra process

(actual change to topic names in next commit)

* catch and report name issues

for node names, namespaces, and topic/service names

* address comment
This commit is contained in:
William Woodall 2017-05-30 18:25:11 -07:00 • committed by GitHub
parent 708903e5df
commit b90676871d
19 changed files with 986 additions and 79 deletions
Download patch file Download diff file Expand all files Collapse all files

61
rclcpp/CMakeLists.txt
View file

@ -30,6 +30,7 @@ set(${PROJECT_NAME}_SRCS
src/rclcpp/exceptions.cpp
src/rclcpp/executor.cpp
src/rclcpp/executors.cpp
src/rclcpp/expand_topic_or_service_name.cpp
src/rclcpp/executors/multi_threaded_executor.cpp
src/rclcpp/executors/single_threaded_executor.cpp
src/rclcpp/graph_listener.cpp
@ -92,6 +93,26 @@ if(BUILD_TESTING)
find_package(rmw_implementation_cmake REQUIRED)
ament_add_gtest(test_client test/test_client.cpp)
if(TARGET test_client)
target_include_directories(test_client PUBLIC
${rcl_interfaces_INCLUDE_DIRS}
${rmw_INCLUDE_DIRS}
${rosidl_generator_cpp_INCLUDE_DIRS}
${rosidl_typesupport_cpp_INCLUDE_DIRS}
)
target_link_libraries(test_client ${PROJECT_NAME})
endif()
ament_add_gtest(test_expand_topic_or_service_name test/test_expand_topic_or_service_name.cpp)
if(TARGET test_expand_topic_or_service_name)
target_include_directories(test_expand_topic_or_service_name PUBLIC
${rcl_interfaces_INCLUDE_DIRS}
${rmw_INCLUDE_DIRS}
${rosidl_generator_cpp_INCLUDE_DIRS}
${rosidl_typesupport_cpp_INCLUDE_DIRS}
)
target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME})
endif()
ament_add_gtest(test_function_traits test/test_function_traits.cpp)
if(TARGET test_function_traits)
target_include_directories(test_function_traits PUBLIC
@ -121,6 +142,26 @@ if(BUILD_TESTING)
${rosidl_typesupport_cpp_INCLUDE_DIRS}
)
endif()
ament_add_gtest(test_node test/test_node.cpp)
if(TARGET test_node)
target_include_directories(test_node PUBLIC
${rcl_interfaces_INCLUDE_DIRS}
${rmw_INCLUDE_DIRS}
${rosidl_generator_cpp_INCLUDE_DIRS}
${rosidl_typesupport_cpp_INCLUDE_DIRS}
)
target_link_libraries(test_node ${PROJECT_NAME})
endif()
ament_add_gtest(test_publisher test/test_publisher.cpp)
if(TARGET test_publisher)
target_include_directories(test_publisher PUBLIC
${rcl_interfaces_INCLUDE_DIRS}
${rmw_INCLUDE_DIRS}
${rosidl_generator_cpp_INCLUDE_DIRS}
${rosidl_typesupport_cpp_INCLUDE_DIRS}
)
target_link_libraries(test_publisher ${PROJECT_NAME})
endif()
ament_add_gtest(test_rate test/test_rate.cpp
ENV RCL_ASSERT_RMW_ID_MATCHES=${rmw_implementation})
if(TARGET test_rate)
@ -134,6 +175,26 @@ if(BUILD_TESTING)
${PROJECT_NAME}
)
endif()
ament_add_gtest(test_service test/test_service.cpp)
if(TARGET test_service)
target_include_directories(test_service PUBLIC
${rcl_interfaces_INCLUDE_DIRS}
${rmw_INCLUDE_DIRS}
${rosidl_generator_cpp_INCLUDE_DIRS}
${rosidl_typesupport_cpp_INCLUDE_DIRS}
)
target_link_libraries(test_service ${PROJECT_NAME})
endif()
ament_add_gtest(test_subscription test/test_subscription.cpp)
if(TARGET test_subscription)
target_include_directories(test_subscription PUBLIC
${rcl_interfaces_INCLUDE_DIRS}
${rmw_INCLUDE_DIRS}
${rosidl_generator_cpp_INCLUDE_DIRS}
${rosidl_typesupport_cpp_INCLUDE_DIRS}
)
target_link_libraries(test_subscription ${PROJECT_NAME})
endif()
ament_add_gtest(test_find_weak_nodes test/test_find_weak_nodes.cpp)
if(TARGET test_find_weak_nodes)
target_include_directories(test_find_weak_nodes PUBLIC

39
rclcpp/include/rclcpp/client.hpp
View file

@ -27,11 +27,13 @@
#include "rcl/error_handling.h"
#include "rcl/wait.h"
#include "rclcpp/exceptions.hpp"
#include "rclcpp/function_traits.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/node_interfaces/node_graph_interface.hpp"
#include "rclcpp/type_support_decl.hpp"
#include "rclcpp/utilities.hpp"
#include "rclcpp/expand_topic_or_service_name.hpp"
#include "rclcpp/visibility_control.hpp"
#include "rmw/error_handling.h"
@ -138,14 +140,24 @@ public:
using rosidl_typesupport_cpp::get_service_type_support_handle;
auto service_type_support_handle =
get_service_type_support_handle<ServiceT>();
if (rcl_client_init(&client_handle_, this->get_rcl_node_handle(),
service_type_support_handle, service_name.c_str(), &client_options) != RCL_RET_OK)
{
// *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
throw std::runtime_error(
std::string("could not create client: ") +
rcl_get_error_string_safe());
// *INDENT-ON*
rcl_ret_t ret = rcl_client_init(
&client_handle_,
this->get_rcl_node_handle(),
service_type_support_handle,
service_name.c_str(),
&client_options);
if (ret != RCL_RET_OK) {
if (ret == RCL_RET_SERVICE_NAME_INVALID) {
auto rcl_node_handle = this->get_rcl_node_handle();
// this will throw on any validation problem
rcl_reset_error();
expand_topic_or_service_name(
service_name,
rcl_node_get_name(rcl_node_handle),
rcl_node_get_namespace(rcl_node_handle),
true);
}
rclcpp::exceptions::throw_from_rcl_error(ret, "could not create client");
}
}
@ -153,7 +165,8 @@ public:
{
if (rcl_client_fini(&client_handle_, this->get_rcl_node_handle()) != RCL_RET_OK) {
fprintf(stderr,
"Error in destruction of rmw client handle: %s\n", rmw_get_error_string_safe());
"Error in destruction of rcl client handle: %s\n", rcl_get_error_string_safe());
rcl_reset_error();
}
}
@ -212,11 +225,9 @@ public:
{
std::lock_guard<std::mutex> lock(pending_requests_mutex_);
int64_t sequence_number;
if (RCL_RET_OK != rcl_send_request(get_client_handle(), request.get(), &sequence_number)) {
// *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
throw std::runtime_error(
std::string("failed to send request: ") + rcl_get_error_string_safe());
// *INDENT-ON*
rcl_ret_t ret = rcl_send_request(get_client_handle(), request.get(), &sequence_number);
if (RCL_RET_OK != ret) {
rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request");
}
SharedPromise call_promise = std::make_shared<Promise>();

74
rclcpp/include/rclcpp/exceptions.hpp
View file

@ -35,18 +35,88 @@ public:
: std::runtime_error("node is invalid") {}
};
/// Thrown when a any kind of name (node, namespace, topic, etc.) is invalid.
class NameValidationError : public std::invalid_argument
{
public:
NameValidationError(
const char * name_type_,
const char * name_,
const char * error_msg_,
size_t invalid_index_)
: std::invalid_argument(format_error(name_type_, name_, error_msg_, invalid_index_)),
name_type(name_type_), name(name_), error_msg(error_msg_), invalid_index(invalid_index_)
{}
static std::string
format_error(
const char * name_type,
const char * name,
const char * error_msg,
size_t invalid_index);
const std::string name_type;
const std::string name;
const std::string error_msg;
const size_t invalid_index;
};
/// Thrown when a node name is invalid.
class InvalidNodeNameError : public NameValidationError
{
public:
InvalidNodeNameError(const char * node_name, const char * error_msg, size_t invalid_index)
: NameValidationError("node name", node_name, error_msg, invalid_index)
{}
};
/// Thrown when a node namespace is invalid.
class InvalidNamespaceError : public NameValidationError
{
public:
InvalidNamespaceError(const char * namespace_, const char * error_msg, size_t invalid_index)
: NameValidationError("namespace", namespace_, error_msg, invalid_index)
{}
};
/// Thrown when a topic name is invalid.
class InvalidTopicNameError : public NameValidationError
{
public:
InvalidTopicNameError(const char * namespace_, const char * error_msg, size_t invalid_index)
: NameValidationError("topic name", namespace_, error_msg, invalid_index)
{}
};
/// Thrown when a service name is invalid.
class InvalidServiceNameError : public NameValidationError
{
public:
InvalidServiceNameError(const char * namespace_, const char * error_msg, size_t invalid_index)
: NameValidationError("service name", namespace_, error_msg, invalid_index)
{}
};
/// Throw a C++ std::exception which was created based on an rcl error.
/**
* Passing nullptr for reset_error is safe and will avoid calling any function
* to reset the error.
*
* \param ret the return code for the current error state
* \param prefix string to prefix to the error if applicable (not all errors have custom messages)
* \param reset_error if true rcl_reset_error() is called before returning
* \param error_state error state to create exception from, if nullptr rcl_get_error_state is used
* \param reset_error function to be called before throwing which whill clear the error state
* \throws std::invalid_argument if ret is RCL_RET_OK
* \throws std::runtime_error if the rcl_get_error_state returns 0
* \throws RCLErrorBase some child class exception based on ret
*/
RCLCPP_PUBLIC
void
throw_from_rcl_error(rcl_ret_t ret, const std::string & prefix = "", bool reset_error = true);
throw_from_rcl_error(
rcl_ret_t ret,
const std::string & prefix = "",
const rcl_error_state_t * error_state = nullptr,
void (* reset_error)() = rcl_reset_error);
class RCLErrorBase
{

63
rclcpp/include/rclcpp/expand_topic_or_service_name.hpp Normal file
View file

@ -0,0 +1,63 @@
// Copyright 2017 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__EXPAND_TOPIC_OR_SERVICE_NAME_HPP_
#define RCLCPP__EXPAND_TOPIC_OR_SERVICE_NAME_HPP_
#include <string>
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
/// Expand a topic or service name and throw if it is not valid.
/**
* This function can be used to "just" validate a topic or service name too,
* since expanding the topic name is required to fully validate a name.
*
* If the name is invalid, then InvalidTopicNameError is thrown or
* InvalidServiceNameError if is_service is true.
*
* This function can take any form of a topic or service name, i.e. it does not
* have to be a fully qualified name.
* The node name and namespace are used to expand it if necessary while
* validating it.
*
* Expansion is done with rcl_expand_topic_name.
* The validation is doen with rcl_validate_topic_name and
* rmw_validate_full_topic_name, so details about failures can be found in the
* documentation for those functions.
*
* \param name the topic or service name to be validated
* \param node_name the name of the node associated with the name
* \param namespace_ the namespace of the node associated with the name
* \param is_service if true InvalidServiceNameError is thrown instead
* \returns expanded (and validated) topic name
* \throws InvalidTopicNameError if name is invalid and is_service is false
* \throws InvalidServiceNameError if name is invalid and is_service is true
* \throws std::bad_alloc if memory cannot be allocated
* \throws RCLError if an unexpect error occurs
*/
RCLCPP_PUBLIC
std::string
expand_topic_or_service_name(
const std::string & name,
const std::string & node_name,
const std::string & namespace_,
bool is_service = false);
} // namespace rclcpp
#endif // RCLCPP__EXPAND_TOPIC_OR_SERVICE_NAME_HPP_

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

@ -55,7 +55,7 @@ namespace intra_process_manager
* When a publisher is created, it advertises on the topic the user provided,
* as well as a "shadowing" topic of type rcl_interfaces/IntraProcessMessage.
* For instance, if the user specified the topic '/namespace/chatter', then the
* corresponding intra process topic might be '/namespace/chatter__intra'.
* corresponding intra process topic might be '/namespace/chatter/_intra'.
* The publisher is also allocated an id which is unique among all publishers
* and subscriptions in this process.
* Additionally, when registered with this class a ring buffer is created and

37
rclcpp/include/rclcpp/service.hpp
View file

@ -25,8 +25,10 @@
#include "rcl/service.h"
#include "rclcpp/any_service_callback.hpp"
#include "rclcpp/exceptions.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/type_support_decl.hpp"
#include "rclcpp/expand_topic_or_service_name.hpp"
#include "rclcpp/visibility_control.hpp"
#include "rmw/error_handling.h"
#include "rmw/rmw.h"
@ -70,6 +72,10 @@ public:
protected:
RCLCPP_DISABLE_COPY(ServiceBase)
RCLCPP_PUBLIC
rcl_node_t *
get_rcl_node_handle() const;
std::shared_ptr<rcl_node_t> node_handle_;
rcl_service_t * service_handle_ = nullptr;
@ -109,12 +115,25 @@ public:
service_handle_ = new rcl_service_t;
*service_handle_ = rcl_get_zero_initialized_service();
if (rcl_service_init(
service_handle_, node_handle.get(), service_type_support_handle, service_name.c_str(),
&service_options) != RCL_RET_OK)
{
throw std::runtime_error(std::string("could not create service: ") +
rcl_get_error_string_safe());
rcl_ret_t ret = rcl_service_init(
service_handle_,
node_handle.get(),
service_type_support_handle,
service_name.c_str(),
&service_options);
if (ret != RCL_RET_OK) {
if (ret == RCL_RET_SERVICE_NAME_INVALID) {
auto rcl_node_handle = get_rcl_node_handle();
// this will throw on any validation problem
rcl_reset_error();
expand_topic_or_service_name(
service_name,
rcl_node_get_name(rcl_node_handle),
rcl_node_get_namespace(rcl_node_handle),
true);
}
rclcpp::exceptions::throw_from_rcl_error(ret, "could not create service");
}
}
@ -151,6 +170,7 @@ public:
ss << "Error in destruction of rcl service_handle_ handle: " <<
rcl_get_error_string_safe() << '\n';
(std::cerr << ss.str()).flush();
rcl_reset_error();
}
}
}
@ -183,10 +203,7 @@ public:
rcl_ret_t status = rcl_send_response(get_service_handle(), req_id.get(), response.get());
if (status != RCL_RET_OK) {
// *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
throw std::runtime_error(
std::string("failed to send response: ") + rcl_get_error_string_safe());
// *INDENT-ON*
rclcpp::exceptions::throw_from_rcl_error(status, "failed to send response");
}
}

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

@ -29,10 +29,12 @@
#include "rcl_interfaces/msg/intra_process_message.hpp"
#include "rclcpp/exceptions.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/message_memory_strategy.hpp"
#include "rclcpp/any_subscription_callback.hpp"
#include "rclcpp/type_support_decl.hpp"
#include "rclcpp/expand_topic_or_service_name.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
@ -238,16 +240,25 @@ public:
MatchesAnyPublishersCallbackType matches_any_publisher_callback,
const rcl_subscription_options_t & intra_process_options)
{
if (rcl_subscription_init(
&intra_process_subscription_handle_, node_handle_.get(),
rclcpp::type_support::get_intra_process_message_msg_type_support(),
(std::string(get_topic_name()) + "__intra").c_str(),
&intra_process_options) != RCL_RET_OK)
{
// *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
throw std::runtime_error(
std::string("could not create intra process subscription: ") + rcl_get_error_string_safe());
// *INDENT-ON*
std::string intra_process_topic_name = std::string(get_topic_name()) + "/_intra";
rcl_ret_t ret = rcl_subscription_init(
&intra_process_subscription_handle_,
node_handle_.get(),
rclcpp::type_support::get_intra_process_message_msg_type_support(),
intra_process_topic_name.c_str(),
&intra_process_options);
if (ret != RCL_RET_OK) {
if (ret == RCL_RET_TOPIC_NAME_INVALID) {
auto rcl_node_handle = node_handle_.get();
// this will throw on any validation problem
rcl_reset_error();
expand_topic_or_service_name(
intra_process_topic_name,
rcl_node_get_name(rcl_node_handle),
rcl_node_get_namespace(rcl_node_handle));
}
rclcpp::exceptions::throw_from_rcl_error(ret, "could not create intra process subscription");
}
intra_process_subscription_id_ = intra_process_subscription_id;

28
rclcpp/src/rclcpp/exceptions.cpp
View file

@ -18,18 +18,40 @@
#include <functional>
#include <string>
using namespace std::string_literals;
namespace rclcpp
{
namespace exceptions
{
std::string
NameValidationError::format_error(
const char * name_type,
const char * name,
const char * error_msg,
size_t invalid_index)
{
std::string msg = "";
msg += "Invalid "s + name_type + ": " + error_msg + ":\n";
msg += " '"s + name + "'\n";
msg += " "s + std::string(invalid_index, ' ') + "^\n";
return msg;
}
void
throw_from_rcl_error(rcl_ret_t ret, const std::string & prefix, bool reset_error)
throw_from_rcl_error(
rcl_ret_t ret,
const std::string & prefix,
const rcl_error_state_t * error_state,
void (* reset_error)())
{
if (RCL_RET_OK == ret) {
throw std::invalid_argument("ret is RCL_RET_OK");
}
const rcl_error_state_t * error_state = rcl_get_error_state();
if (!error_state) {
error_state = rcl_get_error_state();
}
if (!error_state) {
throw std::runtime_error("rcl error state is not set");
}
@ -39,7 +61,7 @@ throw_from_rcl_error(rcl_ret_t ret, const std::string & prefix, bool reset_error
}
RCLErrorBase base_exc(ret, error_state);
if (reset_error) {
rcl_reset_error();
reset_error();
}
switch (ret) {
case RCL_RET_BAD_ALLOC:

196
rclcpp/src/rclcpp/expand_topic_or_service_name.cpp Normal file
View file

@ -0,0 +1,196 @@
// Copyright 2017 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/expand_topic_or_service_name.hpp"
#include <string>
#include "rcl/expand_topic_name.h"
#include "rcl/validate_topic_name.h"
#include "rclcpp/exceptions.hpp"
#include "rclcpp/scope_exit.hpp"
#include "rcutils/types/string_map.h"
#include "rmw/error_handling.h"
#include "rmw/validate_namespace.h"
#include "rmw/validate_node_name.h"
#include "rmw/validate_full_topic_name.h"
using rclcpp::exceptions::throw_from_rcl_error;
std::string
rclcpp::expand_topic_or_service_name(
const std::string & name,
const std::string & node_name,
const std::string & namespace_,
bool is_service)
{
char * expanded_topic = nullptr;
rcl_allocator_t allocator = rcl_get_default_allocator();
rcutils_allocator_t rcutils_allocator = rcutils_get_default_allocator();
rcutils_string_map_t substitutions_map = rcutils_get_zero_initialized_string_map();
rcutils_ret_t rcutils_ret = rcutils_string_map_init(&substitutions_map, 0, rcutils_allocator);
if (rcutils_ret != RCUTILS_RET_OK) {
if (rcutils_ret == RCUTILS_RET_BAD_ALLOC) {
throw_from_rcl_error(RCL_RET_BAD_ALLOC, "", rcutils_get_error_state(), rcutils_reset_error);
} else {
throw_from_rcl_error(RCL_RET_ERROR, "", rcutils_get_error_state(), rcutils_reset_error);
}
}
rcl_ret_t ret = rcl_get_default_topic_name_substitutions(&substitutions_map);
if (ret != RCL_RET_OK) {
rcutils_error_state_t error_state;
if (rcutils_error_state_copy(rcl_get_error_state(), &error_state) != RCUTILS_RET_OK) {
throw std::bad_alloc();
}
auto error_state_scope_exit = rclcpp::make_scope_exit([&error_state]() {
rcutils_error_state_fini(&error_state);
});
// finalize the string map before throwing
rcutils_ret = rcutils_string_map_fini(&substitutions_map);
if (rcutils_ret != RCUTILS_RET_OK) {
fprintf(stderr,
"[rclcpp|" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: "
"failed to fini string_map (%d) during error handling: %s\n",
rcutils_ret,
rcutils_get_error_string_safe());
rcutils_reset_error();
}
throw_from_rcl_error(ret, "", &error_state);
}
ret = rcl_expand_topic_name(
name.c_str(),
node_name.c_str(),
namespace_.c_str(),
&substitutions_map,
allocator,
&expanded_topic);
std::string result;
if (ret == RCL_RET_OK) {
result = expanded_topic;
allocator.deallocate(expanded_topic, allocator.state);
}
rcutils_ret = rcutils_string_map_fini(&substitutions_map);
if (rcutils_ret != RCUTILS_RET_OK) {
throw_from_rcl_error(RCL_RET_ERROR, "", rcutils_get_error_state(), rcutils_reset_error);
}
// expansion failed
if (ret != RCL_RET_OK) {
// if invalid topic or unknown substitution
if (ret == RCL_RET_TOPIC_NAME_INVALID || ret == RCL_RET_UNKNOWN_SUBSTITUTION) {
rcl_reset_error(); // explicitly discard error from rcl_expand_topic_name()
int validation_result;
size_t invalid_index;
rcl_ret_t ret = rcl_validate_topic_name(name.c_str(), &validation_result, &invalid_index);
if (ret != RCL_RET_OK) {
throw_from_rcl_error(ret);
}
if (validation_result == RCL_TOPIC_NAME_VALID) {
throw std::runtime_error("topic name unexpectedly valid");
}
const char * validation_message = rcl_topic_name_validation_result_string(validation_result);
if (!validation_message) {
throw std::runtime_error("unable to get validation error message");
}
if (is_service) {
using rclcpp::exceptions::InvalidServiceNameError;
throw InvalidServiceNameError(name.c_str(), validation_message, invalid_index);
} else {
using rclcpp::exceptions::InvalidTopicNameError;
throw InvalidTopicNameError(name.c_str(), validation_message, invalid_index);
}
// if invalid node name
} else if (ret == RCL_RET_NODE_INVALID_NAME) {
rcl_reset_error(); // explicitly discard error from rcl_expand_topic_name()
int validation_result;
size_t invalid_index;
rmw_ret_t rmw_ret =
rmw_validate_node_name(node_name.c_str(), &validation_result, &invalid_index);
if (rmw_ret != RMW_RET_OK) {
if (rmw_ret == RMW_RET_INVALID_ARGUMENT) {
throw_from_rcl_error(
RCL_RET_INVALID_ARGUMENT, "failed to validate node name",
rmw_get_error_state(), rmw_reset_error);
}
throw_from_rcl_error(
RCL_RET_ERROR, "failed to validate node name",
rmw_get_error_state(), rmw_reset_error);
}
throw rclcpp::exceptions::InvalidNodeNameError(
node_name.c_str(),
rmw_node_name_validation_result_string(validation_result),
invalid_index);
// if invalid namespace
} else if (ret == RCL_RET_NODE_INVALID_NAMESPACE) {
rcl_reset_error(); // explicitly discard error from rcl_expand_topic_name()
int validation_result;
size_t invalid_index;
rmw_ret_t rmw_ret =
rmw_validate_namespace(namespace_.c_str(), &validation_result, &invalid_index);
if (rmw_ret != RMW_RET_OK) {
if (rmw_ret == RMW_RET_INVALID_ARGUMENT) {
throw_from_rcl_error(
RCL_RET_INVALID_ARGUMENT, "failed to validate namespace",
rmw_get_error_state(), rmw_reset_error);
}
throw_from_rcl_error(
RCL_RET_ERROR, "failed to validate namespace",
rmw_get_error_state(), rmw_reset_error);
}
throw rclcpp::exceptions::InvalidNamespaceError(
namespace_.c_str(),
rmw_namespace_validation_result_string(validation_result),
invalid_index);
// something else happened
} else {
throw_from_rcl_error(ret);
}
}
// expand succeeded, but full name validation may fail still
int validation_result;
size_t invalid_index;
rmw_ret_t rmw_ret =
rmw_validate_full_topic_name(result.c_str(), &validation_result, &invalid_index);
if (rmw_ret != RMW_RET_OK) {
if (rmw_ret == RMW_RET_INVALID_ARGUMENT) {
throw_from_rcl_error(
RCL_RET_INVALID_ARGUMENT, "failed to validate full topic name",
rmw_get_error_state(), rmw_reset_error);
}
throw_from_rcl_error(
RCL_RET_ERROR, "failed to validate full topic name",
rmw_get_error_state(), rmw_reset_error);
}
if (validation_result != RMW_TOPIC_VALID) {
if (is_service) {
throw rclcpp::exceptions::InvalidServiceNameError(
result.c_str(),
rmw_full_topic_name_validation_result_string(validation_result),
invalid_index);
} else {
throw rclcpp::exceptions::InvalidTopicNameError(
result.c_str(),
rmw_full_topic_name_validation_result_string(validation_result),
invalid_index);
}
}
return result;
}

38
rclcpp/src/rclcpp/node_interfaces/node_base.cpp
View file

@ -20,6 +20,8 @@
#include "rclcpp/node_interfaces/node_base.hpp"
#include "rclcpp/exceptions.hpp"
#include "rmw/validate_node_name.h"
#include "rmw/validate_namespace.h"
using rclcpp::exceptions::throw_from_rcl_error;
@ -90,6 +92,42 @@ NodeBase::NodeBase(
// Finalize the interrupt guard condition.
finalize_notify_guard_condition();
if (ret == RCL_RET_NODE_INVALID_NAME) {
rcl_reset_error(); // discard rcl_node_init error
int validation_result;
size_t invalid_index;
rmw_ret_t rmw_ret =
rmw_validate_node_name(node_name.c_str(), &validation_result, &invalid_index);
if (rmw_ret != RMW_RET_OK) {
if (rmw_ret == RMW_RET_INVALID_ARGUMENT) {
throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, "failed to validate node name");
}
throw_from_rcl_error(RCL_RET_ERROR, "failed to validate node name");
}
throw rclcpp::exceptions::InvalidNodeNameError(
node_name.c_str(),
rmw_node_name_validation_result_string(validation_result),
invalid_index);
}
if (ret == RCL_RET_NODE_INVALID_NAMESPACE) {
rcl_reset_error(); // discard rcl_node_init error
int validation_result;
size_t invalid_index;
rmw_ret_t rmw_ret =
rmw_validate_namespace(namespace_.c_str(), &validation_result, &invalid_index);
if (rmw_ret != RMW_RET_OK) {
if (rmw_ret == RMW_RET_INVALID_ARGUMENT) {
throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, "failed to validate namespace");
}
throw_from_rcl_error(RCL_RET_ERROR, "failed to validate namespace");
}
throw rclcpp::exceptions::InvalidNamespaceError(
namespace_.c_str(),
rmw_namespace_validation_result_string(validation_result),
invalid_index);
}
throw_from_rcl_error(ret, "failed to initialize rcl node");
}

102
rclcpp/src/rclcpp/publisher.cpp
View file

@ -28,8 +28,10 @@
#include "rclcpp/allocator/allocator_common.hpp"
#include "rclcpp/allocator/allocator_deleter.hpp"
#include "rclcpp/exceptions.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/expand_topic_or_service_name.hpp"
using rclcpp::publisher::PublisherBase;
@ -41,25 +43,36 @@ PublisherBase::PublisherBase(
: rcl_node_handle_(node_base->get_shared_rcl_node_handle()),
intra_process_publisher_id_(0), store_intra_process_message_(nullptr)
{
if (rcl_publisher_init(
&publisher_handle_, rcl_node_handle_.get(), &type_support,
topic.c_str(), &publisher_options) != RCL_RET_OK)
{
throw std::runtime_error(
std::string("could not create publisher: ") +
rcl_get_error_string_safe());
rcl_ret_t ret = rcl_publisher_init(
&publisher_handle_,
rcl_node_handle_.get(),
&type_support,
topic.c_str(),
&publisher_options);
if (ret != RCL_RET_OK) {
if (ret == RCL_RET_TOPIC_NAME_INVALID) {
auto rcl_node_handle = rcl_node_handle_.get();
// this will throw on any validation problem
rcl_reset_error();
expand_topic_or_service_name(
topic,
rcl_node_get_name(rcl_node_handle),
rcl_node_get_namespace(rcl_node_handle));
}
rclcpp::exceptions::throw_from_rcl_error(ret, "could not create publisher");
}
// Life time of this object is tied to the publisher handle.
rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle(&publisher_handle_);
if (!publisher_rmw_handle) {
throw std::runtime_error(
std::string("failed to get rmw handle: ") + rcl_get_error_string_safe());
auto msg = std::string("failed to get rmw handle: ") + rcl_get_error_string_safe();
rcl_reset_error();
throw std::runtime_error(msg);
}
if (rmw_get_gid_for_publisher(publisher_rmw_handle, &rmw_gid_) != RMW_RET_OK) {
// *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
throw std::runtime_error(
std::string("failed to get publisher gid: ") + rmw_get_error_string_safe());
// *INDENT-ON*
auto msg = std::string("failed to get publisher gid: ") + rmw_get_error_string_safe();
rmw_reset_error();
throw std::runtime_error(msg);
}
}
@ -70,6 +83,7 @@ PublisherBase::~PublisherBase()
stderr,
"Error in destruction of intra process rcl publisher handle: %s\n",
rcl_get_error_string_safe());
rcl_reset_error();
}
if (rcl_publisher_fini(&publisher_handle_, rcl_node_handle_.get()) != RCL_RET_OK) {
@ -77,6 +91,7 @@ PublisherBase::~PublisherBase()
stderr,
"Error in destruction of rcl publisher handle: %s\n",
rcl_get_error_string_safe());
rcl_reset_error();
}
}
@ -91,8 +106,9 @@ PublisherBase::get_queue_size() const
{
const rcl_publisher_options_t * publisher_options = rcl_publisher_get_options(&publisher_handle_);
if (!publisher_options) {
throw std::runtime_error(
std::string("failed to get publisher options: ") + rcl_get_error_string_safe());
auto msg = std::string("failed to get publisher options: ") + rcl_get_error_string_safe();
rcl_reset_error();
throw std::runtime_error(msg);
}
return publisher_options->qos.depth;
}
@ -121,14 +137,16 @@ PublisherBase::operator==(const rmw_gid_t * gid) const
bool result = false;
auto ret = rmw_compare_gids_equal(gid, &this->get_gid(), &result);
if (ret != RMW_RET_OK) {
throw std::runtime_error(
std::string("failed to compare gids: ") + rmw_get_error_string_safe());
auto msg = std::string("failed to compare gids: ") + rmw_get_error_string_safe();
rmw_reset_error();
throw std::runtime_error(msg);
}
if (!result) {
ret = rmw_compare_gids_equal(gid, &this->get_intra_process_gid(), &result);
if (ret != RMW_RET_OK) {
throw std::runtime_error(
std::string("failed to compare gids: ") + rmw_get_error_string_safe());
auto msg = std::string("failed to compare gids: ") + rmw_get_error_string_safe();
rmw_reset_error();
throw std::runtime_error(msg);
}
}
return result;
@ -140,15 +158,25 @@ PublisherBase::setup_intra_process(
StoreMessageCallbackT callback,
const rcl_publisher_options_t & intra_process_options)
{
auto intra_process_topic_name = std::string(this->get_topic_name()) + "__intra";
if (rcl_publisher_init(
&intra_process_publisher_handle_, rcl_node_handle_.get(),
rclcpp::type_support::get_intra_process_message_msg_type_support(),
intra_process_topic_name.c_str(), &intra_process_options) != RCL_RET_OK)
{
throw std::runtime_error(
std::string("could not create intra process publisher: ") +
rcl_get_error_string_safe());
auto intra_process_topic_name = std::string(this->get_topic_name()) + "/_intra";
rcl_ret_t ret = rcl_publisher_init(
&intra_process_publisher_handle_,
rcl_node_handle_.get(),
rclcpp::type_support::get_intra_process_message_msg_type_support(),
intra_process_topic_name.c_str(),
&intra_process_options);
if (ret != RCL_RET_OK) {
if (ret == RCL_RET_TOPIC_NAME_INVALID) {
auto rcl_node_handle = rcl_node_handle_.get();
// this will throw on any validation problem
rcl_reset_error();
expand_topic_or_service_name(
intra_process_topic_name,
rcl_node_get_name(rcl_node_handle),
rcl_node_get_namespace(rcl_node_handle));
}
rclcpp::exceptions::throw_from_rcl_error(ret, "could not create intra process publisher");
}
intra_process_publisher_id_ = intra_process_publisher_id;
@ -157,16 +185,16 @@ PublisherBase::setup_intra_process(
rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle(
&intra_process_publisher_handle_);
if (publisher_rmw_handle == nullptr) {
throw std::runtime_error(std::string(
"Failed to get rmw publisher handle") + rcl_get_error_string_safe());
auto msg = std::string("Failed to get rmw publisher handle") + rcl_get_error_string_safe();
rcl_reset_error();
throw std::runtime_error(msg);
}
auto ret = rmw_get_gid_for_publisher(
auto rmw_ret = rmw_get_gid_for_publisher(
publisher_rmw_handle, &intra_process_rmw_gid_);
if (ret != RMW_RET_OK) {
// *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
throw std::runtime_error(
std::string("failed to create intra process publisher gid: ") +
rmw_get_error_string_safe());
// *INDENT-ON*
if (rmw_ret != RMW_RET_OK) {
auto msg =
std::string("failed to create intra process publisher gid: ") + rmw_get_error_string_safe();
rmw_reset_error();
throw std::runtime_error(msg);
}
}

6
rclcpp/src/rclcpp/service.cpp
View file

@ -51,3 +51,9 @@ ServiceBase::get_service_handle()
{
return service_handle_;
}
rcl_node_t *
ServiceBase::get_rcl_node_handle() const
{
return node_handle_.get();
}

16
rclcpp/src/rclcpp/subscription.cpp
View file

@ -18,6 +18,9 @@
#include <memory>
#include <string>
#include "rclcpp/exceptions.hpp"
#include "rclcpp/expand_topic_or_service_name.hpp"
#include "rmw/error_handling.h"
#include "rmw/rmw.h"
@ -38,8 +41,17 @@ SubscriptionBase::SubscriptionBase(
topic_name.c_str(),
&subscription_options);
if (ret != RCL_RET_OK) {
throw std::runtime_error(
std::string("could not create subscription: ") + rcl_get_error_string_safe());
if (ret == RCL_RET_TOPIC_NAME_INVALID) {
auto rcl_node_handle = node_handle_.get();
// this will throw on any validation problem
rcl_reset_error();
expand_topic_or_service_name(
topic_name,
rcl_node_get_name(rcl_node_handle),
rcl_node_get_namespace(rcl_node_handle));
}
rclcpp::exceptions::throw_from_rcl_error(ret, "could not create subscription");
}
}

60
rclcpp/test/test_client.cpp Normal file
View file

@ -0,0 +1,60 @@
// Copyright 2017 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 <string>
#include <memory>
#include "rclcpp/exceptions.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rcl_interfaces/srv/list_parameters.hpp"
class TestClient : public ::testing::Test
{
protected:
static void SetUpTestCase()
{
rclcpp::init(0, nullptr);
}
void SetUp()
{
node = std::make_shared<rclcpp::node::Node>("my_node", "/ns");
}
void TearDown()
{
node.reset();
}
rclcpp::node::Node::SharedPtr node;
};
/*
Testing client construction and destruction.
*/
TEST_F(TestClient, construction_and_destruction) {
using rcl_interfaces::srv::ListParameters;
{
auto client = node->create_client<ListParameters>("service");
}
{
ASSERT_THROW({
auto client = node->create_client<ListParameters>("invalid_service?");
}, rclcpp::exceptions::InvalidServiceNameError);
}
}

74
rclcpp/test/test_expand_topic_or_service_name.cpp Normal file
View file

@ -0,0 +1,74 @@
// Copyright 2017 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/exceptions.hpp"
#include "rclcpp/expand_topic_or_service_name.hpp"
/*
Testing expand_topic_or_service_name.
*/
TEST(TestExpandTopicOrServiceName, normal) {
using rclcpp::expand_topic_or_service_name;
{
ASSERT_EQ("/ns/chatter", expand_topic_or_service_name("chatter", "node", "/ns"));
}
}
/*
Testing exceptions of expand_topic_or_service_name.
*/
TEST(TestExpandTopicOrServiceName, exceptions) {
using rclcpp::expand_topic_or_service_name;
{
ASSERT_THROW({
expand_topic_or_service_name("chatter", "invalid_node?", "/ns");
}, rclcpp::exceptions::InvalidNodeNameError);
}
{
ASSERT_THROW({
expand_topic_or_service_name("chatter", "node", "/invalid_ns?");
}, rclcpp::exceptions::InvalidNamespaceError);
}
{
ASSERT_THROW({
expand_topic_or_service_name("chatter/42invalid", "node", "/ns");
}, rclcpp::exceptions::InvalidTopicNameError);
}
{
ASSERT_THROW({
// this one will only fail on the "full" topic name validation check
expand_topic_or_service_name("chatter/{ns}/invalid", "node", "/ns");
}, rclcpp::exceptions::InvalidTopicNameError);
}
{
ASSERT_THROW({
// is_service = true
expand_topic_or_service_name("chatter/42invalid", "node", "/ns", true);
}, rclcpp::exceptions::InvalidServiceNameError);
}
{
ASSERT_THROW({
// is_service = true
// this one will only fail on the "full" topic name validation check
expand_topic_or_service_name("chatter/{ns}/invalid", "node", "/ns", true);
}, rclcpp::exceptions::InvalidServiceNameError);
}
}

52
rclcpp/test/test_node.cpp Normal file
View file

@ -0,0 +1,52 @@
// Copyright 2017 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 <string>
#include <memory>
#include "rclcpp/exceptions.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/rclcpp.hpp"
class TestNode : public ::testing::Test
{
protected:
static void SetUpTestCase()
{
rclcpp::init(0, nullptr);
}
};
/*
Testing node construction and destruction.
*/
TEST_F(TestNode, construction_and_destruction) {
{
auto node = std::make_shared<rclcpp::node::Node>("my_node", "/ns");
}
{
ASSERT_THROW({
auto node = std::make_shared<rclcpp::node::Node>("invalid_node?", "/ns");
}, rclcpp::exceptions::InvalidNodeNameError);
}
{
ASSERT_THROW({
auto node = std::make_shared<rclcpp::node::Node>("my_node", "/invalid_ns?");
}, rclcpp::exceptions::InvalidNamespaceError);
}
}

60
rclcpp/test/test_publisher.cpp Normal file
View file

@ -0,0 +1,60 @@
// Copyright 2017 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 <string>
#include <memory>
#include "rclcpp/exceptions.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rcl_interfaces/msg/intra_process_message.hpp"
class TestPublisher : public ::testing::Test
{
protected:
static void SetUpTestCase()
{
rclcpp::init(0, nullptr);
}
void SetUp()
{
node = std::make_shared<rclcpp::node::Node>("my_node", "/ns");
}
void TearDown()
{
node.reset();
}
rclcpp::node::Node::SharedPtr node;
};
/*
Testing publisher construction and destruction.
*/
TEST_F(TestPublisher, construction_and_destruction) {
using rcl_interfaces::msg::IntraProcessMessage;
{
auto publisher = node->create_publisher<IntraProcessMessage>("topic");
}
{
ASSERT_THROW({
auto publisher = node->create_publisher<IntraProcessMessage>("invalid_topic?");
}, rclcpp::exceptions::InvalidTopicNameError);
}
}

63
rclcpp/test/test_service.cpp Normal file
View file

@ -0,0 +1,63 @@
// Copyright 2017 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 <string>
#include <memory>
#include "rclcpp/exceptions.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rcl_interfaces/srv/list_parameters.hpp"
class TestService : public ::testing::Test
{
protected:
static void SetUpTestCase()
{
rclcpp::init(0, nullptr);
}
void SetUp()
{
node = std::make_shared<rclcpp::node::Node>("my_node", "/ns");
}
void TearDown()
{
node.reset();
}
rclcpp::node::Node::SharedPtr node;
};
/*
Testing service construction and destruction.
*/
TEST_F(TestService, construction_and_destruction) {
using rcl_interfaces::srv::ListParameters;
auto callback =
[](const ListParameters::Request::SharedPtr, ListParameters::Response::SharedPtr) {
};
{
auto service = node->create_service<ListParameters>("service", callback);
}
{
ASSERT_THROW({
auto service = node->create_service<ListParameters>("invalid_service?", callback);
}, rclcpp::exceptions::InvalidServiceNameError);
}
}

63
rclcpp/test/test_subscription.cpp Normal file
View file

@ -0,0 +1,63 @@
// Copyright 2017 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 <string>
#include <memory>
#include "rclcpp/exceptions.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rcl_interfaces/msg/intra_process_message.hpp"
class TestSubscription : public ::testing::Test
{
protected:
static void SetUpTestCase()
{
rclcpp::init(0, nullptr);
}
void SetUp()
{
node = std::make_shared<rclcpp::node::Node>("my_node", "/ns");
}
void TearDown()
{
node.reset();
}
rclcpp::node::Node::SharedPtr node;
};
/*
Testing subscription construction and destruction.
*/
TEST_F(TestSubscription, construction_and_destruction) {
using rcl_interfaces::msg::IntraProcessMessage;
auto callback = [](const IntraProcessMessage::SharedPtr msg) {
(void)msg;
};
{
auto sub = node->create_subscription<IntraProcessMessage>("topic", callback);
}
{
ASSERT_THROW({
auto sub = node->create_subscription<IntraProcessMessage>("invalid_topic?", callback);
}, rclcpp::exceptions::InvalidTopicNameError);
}
}