Compare commits

...

12 Commits

Author SHA1 Message Date
Yadunund
45df3555d2 21.0.3 2023-09-08 13:44:12 +08:00
Yadunund
3fa5bc49d6 Update changelogs
Signed-off-by: Yadunund <yadunund@openrobotics.org>
2023-09-08 13:43:56 +08:00
mergify[bot]
5dcd5a39a4 Do not crash Executor when send_response fails due to client failure. (#2276) (#2279)
* Do not crash Executor when send_response fails due to client failure.

Related to https://github.com/ros2/ros2/issues/1253

It is not sane that a faulty client can crash our service Executor, as
discussed in the referred issue, if the client is not setup properly,
send_response may return RCL_RET_TIMEOUT, we should not throw an error
in this case.

Signed-off-by: Zang MingJie <zealot0630@gmail.com>

* Update rclcpp/include/rclcpp/service.hpp

Co-authored-by: Tomoya Fujita <Tomoya.Fujita@sony.com>
Signed-off-by: Zang MingJie <zealot0630@gmail.com>

* address review comments.

Signed-off-by: Tomoya Fujita <Tomoya.Fujita@sony.com>

---------

Signed-off-by: Zang MingJie <zealot0630@gmail.com>
Signed-off-by: Tomoya Fujita <Tomoya.Fujita@sony.com>
Co-authored-by: Zang MingJie <zealot0630@gmail.com>
(cherry picked from commit fbe8f28cd1)

Co-authored-by: Tomoya Fujita <Tomoya.Fujita@sony.com>
2023-09-01 08:23:43 -07:00
mergify[bot]
7b7531bfd6 Switch lifecycle to use the RCLCPP macros. (backport #2233) (#2244)
* Switch lifecycle to use the RCLCPP macros. (#2233)

This ensures that they'll go out to /rosout and the disk.

Signed-off-by: Chris Lalancette <clalancette@gmail.com>
(cherry picked from commit 945d254e32)
2023-07-24 11:54:32 -04:00
Emerson Knapp
6d4a99f815 [Iron] Add new node interface TypeDescriptionsInterface to provide GetTypeDescription service (backport #2224) (#2236)
* Add new node interface TypeDescriptionsInterface to provide GetTypeDescription service (#2224)

* TypeDescriptions interface with readonly param configuration

* Add parameter descriptor, to make read only

* example of spinning in thread for get_type_description service

* Add a basic test for the new interface

* Fix tests with new parameter

* Add comments about builtin parameters

* Hide new member for ABI stability

* Add typedescription interface smoke test

Signed-off-by: Emerson Knapp <emerson.b.knapp@gmail.com>
Signed-off-by: William Woodall <william@osrfoundation.org>
2023-07-18 08:24:54 -04:00
Yadunund
052c075052 21.0.2 2023-07-14 01:21:56 +08:00
Yadunund
193c252036 Update Changelogs
Signed-off-by: Yadunund <yadunund@openrobotics.org>
2023-07-14 01:21:40 +08:00
mergify[bot]
efbb9b6c89 warning: comparison of integer expressions of different signedness (#2219) (#2222)
https://github.com/ros2/rclcpp/pull/2167#issuecomment-1597197552

Signed-off-by: Tomoya Fujita <Tomoya.Fujita@sony.com>
(cherry picked from commit fe2e0e4c64)

Co-authored-by: Tomoya Fujita <Tomoya.Fujita@sony.com>
2023-06-23 13:00:42 -04:00
mergify[bot]
3506dd1227 Fix race condition in events-executor (#2177) (#2191)
The initial implementation of the events-executor contained a bug where the executor
would end up in an inconsistent state and stop processing interrupt/shutdown notifications.
Manually adding a node to the executor results in a) producing a notify waitable event
and b) refreshing the executor collections.
The inconsistent state would happen if the event was processed before the collections were
finished to be refreshed: the executor would pick up the event but be unable to process it.
This would leave the `notify_waitable_event_pushed_` flag to true, preventing additional
notify waitable events to be pushed.
The behavior is observable only under heavy load.

Signed-off-by: Alberto Soragna <alberto.soragna@gmail.com>
(cherry picked from commit 6e1fea14e1)

Co-authored-by: Alberto Soragna <alberto.soragna@gmail.com>
2023-05-31 08:15:45 -05:00
Chris Lalancette
7a837496bd 21.0.1 2023-05-11 14:00:42 +00:00
Chris Lalancette
cd784f6612 Changelog.
Signed-off-by: Chris Lalancette <clalancette@gmail.com>
2023-05-11 14:00:36 +00:00
mergify[bot]
7aa390d5b1 Fix delivered message kind (#2175) (#2178)
Signed-off-by: methylDragon <methylDragon@gmail.com>
(cherry picked from commit 6ca1023ef7)

Co-authored-by: methylDragon <methylDragon@gmail.com>
2023-05-02 11:21:27 -04:00
32 changed files with 703 additions and 70 deletions

View File

@@ -2,6 +2,23 @@
Changelog for package rclcpp
^^^^^^^^^^^^^^^^^^^^^^^^^^^^
21.0.3 (2023-09-08)
-------------------
* Do not crash Executor when send_response fails due to client failure. (`#2279 <https://github.com/ros2/rclcpp/issues/2279>`_)
* Add new node interface TypeDescriptionsInterface to provide GetTypeDescription service (`#2236 <https://github.com/ros2/rclcpp/issues/2236>`_)
* Contributors: Emerson Knapp, Tomoya Fujita, Zang MingJie
21.0.2 (2023-07-14)
-------------------
* Fix warnings related to comparison of integer expressions of different signedness (`#2222 <https://github.com/ros2/rclcpp/issues/2222>`_)
* Fix race condition in events-executor (`#2191 <https://github.com/ros2/rclcpp/issues/2191>`_)
* Contributors: Alberto Soragna, Tomoya Fujita
21.0.1 (2023-05-11)
-------------------
* Fix delivered message kind (`#2175 <https://github.com/ros2/rclcpp/issues/2175>`_) (`#2178 <https://github.com/ros2/rclcpp/issues/2178>`_)
* Contributors: mergify[bot]
21.0.0 (2023-04-18)
-------------------
* Add support for logging service. (`#2122 <https://github.com/ros2/rclcpp/issues/2122>`_)

View File

@@ -92,6 +92,7 @@ set(${PROJECT_NAME}_SRCS
src/rclcpp/node_interfaces/node_time_source.cpp
src/rclcpp/node_interfaces/node_timers.cpp
src/rclcpp/node_interfaces/node_topics.cpp
src/rclcpp/node_interfaces/node_type_descriptions.cpp
src/rclcpp/node_interfaces/node_waitables.cpp
src/rclcpp/node_options.cpp
src/rclcpp/parameter.cpp

View File

@@ -243,6 +243,11 @@ private:
std::function<void(size_t, int)>
create_waitable_callback(const rclcpp::Waitable * waitable_id);
/// Utility to add the notify waitable to an entities collection
void
add_notify_waitable_to_collection(
rclcpp::executors::ExecutorEntitiesCollection::WaitableCollection & collection);
/// Searches for the provided entity_id in the collection and returns the entity if valid
template<typename CollectionType>
typename CollectionType::EntitySharedPtr

View File

@@ -56,6 +56,7 @@
#include "rclcpp/node_interfaces/node_time_source_interface.hpp"
#include "rclcpp/node_interfaces/node_timers_interface.hpp"
#include "rclcpp/node_interfaces/node_topics_interface.hpp"
#include "rclcpp/node_interfaces/node_type_descriptions_interface.hpp"
#include "rclcpp/node_interfaces/node_waitables_interface.hpp"
#include "rclcpp/node_options.hpp"
#include "rclcpp/parameter.hpp"
@@ -1454,6 +1455,11 @@ public:
rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr
get_node_time_source_interface();
/// Return the Node's internal NodeTypeDescriptionsInterface implementation.
RCLCPP_PUBLIC
rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr
get_node_type_descriptions_interface();
/// Return the sub-namespace, if this is a sub-node, otherwise an empty string.
/**
* The returned sub-namespace is either the accumulated sub-namespaces which
@@ -1591,6 +1597,11 @@ private:
const rclcpp::NodeOptions node_options_;
const std::string sub_namespace_;
const std::string effective_namespace_;
/// Static map(s) containing extra member variables for Node without changing its ABI.
// See node.cpp for more details.
class BackportMembers;
static BackportMembers backport_members_;
};
} // namespace rclcpp

View File

@@ -30,6 +30,7 @@
rclcpp::node_interfaces::NodeTimeSourceInterface, \
rclcpp::node_interfaces::NodeTimersInterface, \
rclcpp::node_interfaces::NodeTopicsInterface, \
rclcpp::node_interfaces::NodeTypeDescriptionsInterface, \
rclcpp::node_interfaces::NodeWaitablesInterface
@@ -118,6 +119,7 @@ public:
* - rclcpp::node_interfaces::NodeTimeSourceInterface
* - rclcpp::node_interfaces::NodeTimersInterface
* - rclcpp::node_interfaces::NodeTopicsInterface
* - rclcpp::node_interfaces::NodeTypeDescriptionsInterface
* - rclcpp::node_interfaces::NodeWaitablesInterface
*
* Or you use custom interfaces as long as you make a template specialization

View File

@@ -0,0 +1,63 @@
// Copyright 2023 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__NODE_TYPE_DESCRIPTIONS_HPP_
#define RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_HPP_
#include <memory>
#include "rclcpp/macros.hpp"
#include "rclcpp/node_interfaces/node_base_interface.hpp"
#include "rclcpp/node_interfaces/node_logging_interface.hpp"
#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
#include "rclcpp/node_interfaces/node_services_interface.hpp"
#include "rclcpp/node_interfaces/node_topics_interface.hpp"
#include "rclcpp/node_interfaces/node_type_descriptions_interface.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
namespace node_interfaces
{
/// Implementation of the NodeTypeDescriptions part of the Node API.
class NodeTypeDescriptions : public NodeTypeDescriptionsInterface
{
public:
RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTypeDescriptions)
RCLCPP_PUBLIC
explicit NodeTypeDescriptions(
NodeBaseInterface::SharedPtr node_base,
NodeLoggingInterface::SharedPtr node_logging,
NodeParametersInterface::SharedPtr node_parameters,
NodeServicesInterface::SharedPtr node_services);
RCLCPP_PUBLIC
virtual
~NodeTypeDescriptions();
private:
RCLCPP_DISABLE_COPY(NodeTypeDescriptions)
// Pimpl hides helper types and functions used for wrapping a C service, which would be
// awkward to expose in this header.
class NodeTypeDescriptionsImpl;
std::unique_ptr<NodeTypeDescriptionsImpl> impl_;
};
} // namespace node_interfaces
} // namespace rclcpp
#endif // RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_HPP_

View File

@@ -0,0 +1,44 @@
// Copyright 2023 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__NODE_TYPE_DESCRIPTIONS_INTERFACE_HPP_
#define RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_INTERFACE_HPP_
#include "rclcpp/macros.hpp"
#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
namespace node_interfaces
{
/// Pure virtual interface class for the NodeTypeDescriptions part of the Node API.
class NodeTypeDescriptionsInterface
{
public:
RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTypeDescriptionsInterface)
RCLCPP_PUBLIC
virtual
~NodeTypeDescriptionsInterface() = default;
};
} // namespace node_interfaces
} // namespace rclcpp
RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(
rclcpp::node_interfaces::NodeTypeDescriptionsInterface, type_descriptions)
#endif // RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_INTERFACE_HPP_

View File

@@ -482,6 +482,14 @@ public:
{
rcl_ret_t ret = rcl_send_response(get_service_handle().get(), &req_id, &response);
if (ret == RCL_RET_TIMEOUT) {
RCLCPP_WARN(
node_logger_.get_child("rclcpp"),
"failed to send response to %s (timeout): %s",
this->get_service_name(), rcl_get_error_string().str);
rcl_reset_error();
return;
}
if (ret != RCL_RET_OK) {
rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send response");
}

View File

@@ -260,13 +260,13 @@ public:
bool
is_serialized() const;
/// Return the type of the subscription.
/// Return the delivered message kind.
/**
* \return `DeliveredMessageKind`, which adjusts how messages are received and delivered.
*/
RCLCPP_PUBLIC
DeliveredMessageKind
get_subscription_type() const;
get_delivered_message_kind() const;
/// Get matching publisher count.
/** \return The number of publishers on this topic. */
@@ -663,7 +663,7 @@ private:
RCLCPP_DISABLE_COPY(SubscriptionBase)
rosidl_message_type_support_t type_support_;
DeliveredMessageKind delivered_message_type_;
DeliveredMessageKind delivered_message_kind_;
std::atomic<bool> subscription_in_use_by_wait_set_{false};
std::atomic<bool> intra_process_subscription_waitable_in_use_by_wait_set_{false};

View File

@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="2">
<name>rclcpp</name>
<version>21.0.0</version>
<version>21.0.3</version>
<description>The ROS client library in C++.</description>
<maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>

View File

@@ -603,7 +603,7 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription)
rclcpp::MessageInfo message_info;
message_info.get_rmw_message_info().from_intra_process = false;
switch (subscription->get_subscription_type()) {
switch (subscription->get_delivered_message_kind()) {
// Deliver ROS message
case rclcpp::DeliveredMessageKind::ROS_MESSAGE:
{

View File

@@ -50,6 +50,9 @@ EventsExecutor::EventsExecutor(
timers_manager_ =
std::make_shared<rclcpp::experimental::TimersManager>(context_, timer_on_ready_cb);
this->current_entities_collection_ =
std::make_shared<rclcpp::executors::ExecutorEntitiesCollection>();
notify_waitable_ = std::make_shared<rclcpp::executors::ExecutorNotifyWaitable>(
[this]() {
// This callback is invoked when:
@@ -61,6 +64,10 @@ EventsExecutor::EventsExecutor(
this->refresh_current_collection_from_callback_groups();
});
// Make sure that the notify waitable is immediately added to the collection
// to avoid missing events
this->add_notify_waitable_to_collection(current_entities_collection_->waitables);
notify_waitable_->add_guard_condition(interrupt_guard_condition_);
notify_waitable_->add_guard_condition(shutdown_guard_condition_);
@@ -87,9 +94,6 @@ EventsExecutor::EventsExecutor(
this->entities_collector_ =
std::make_shared<rclcpp::executors::ExecutorEntitiesCollector>(notify_waitable_);
this->current_entities_collection_ =
std::make_shared<rclcpp::executors::ExecutorEntitiesCollection>();
}
EventsExecutor::~EventsExecutor()
@@ -395,18 +399,8 @@ EventsExecutor::refresh_current_collection_from_callback_groups()
// retrieved in the "standard" way.
// To do it, we need to add the notify waitable as an entry in both the new and
// current collections such that it's neither added or removed.
rclcpp::CallbackGroup::WeakPtr weak_group_ptr;
new_collection.waitables.insert(
{
this->notify_waitable_.get(),
{this->notify_waitable_, weak_group_ptr}
});
this->current_entities_collection_->waitables.insert(
{
this->notify_waitable_.get(),
{this->notify_waitable_, weak_group_ptr}
});
this->add_notify_waitable_to_collection(new_collection.waitables);
this->add_notify_waitable_to_collection(current_entities_collection_->waitables);
this->refresh_current_collection(new_collection);
}
@@ -486,3 +480,16 @@ EventsExecutor::create_waitable_callback(const rclcpp::Waitable * entity_key)
};
return callback;
}
void
EventsExecutor::add_notify_waitable_to_collection(
rclcpp::executors::ExecutorEntitiesCollection::WaitableCollection & collection)
{
// The notify waitable is not associated to any group, so use an invalid one
rclcpp::CallbackGroup::WeakPtr weak_group_ptr;
collection.insert(
{
this->notify_waitable_.get(),
{this->notify_waitable_, weak_group_ptr}
});
}

View File

@@ -17,7 +17,10 @@
#include <limits>
#include <map>
#include <memory>
#include <mutex>
#include <shared_mutex>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>
@@ -36,6 +39,7 @@
#include "rclcpp/node_interfaces/node_time_source.hpp"
#include "rclcpp/node_interfaces/node_timers.hpp"
#include "rclcpp/node_interfaces/node_topics.hpp"
#include "rclcpp/node_interfaces/node_type_descriptions.hpp"
#include "rclcpp/node_interfaces/node_waitables.hpp"
#include "rclcpp/qos_overriding_options.hpp"
@@ -109,6 +113,72 @@ create_effective_namespace(const std::string & node_namespace, const std::string
} // namespace
/// \brief Associate new extra member variables with instances of Node without changing ABI.
/**
* It is used only for bugfixes or backported features that require new members.
* Atomically constructs/destroys all extra members.
* Node instance will register and remove itself, and use its methods to retrieve members.
* Note for performance consideration that accessing these members uses a map lookup.
*/
class Node::BackportMembers
{
public:
BackportMembers() = default;
~BackportMembers() = default;
/// \brief Add all backported members for a new Node.
/**
* \param[in] key Raw pointer to the Node instance that will use new members.
*/
void add(Node * key)
{
// Adding a new instance to the maps requires exclusive access
std::unique_lock lock(map_access_mutex_);
type_descriptions_map_.emplace(
key,
std::make_shared<rclcpp::node_interfaces::NodeTypeDescriptions>(
key->get_node_base_interface(),
key->get_node_logging_interface(),
key->get_node_parameters_interface(),
key->get_node_services_interface()));
}
/// \brief Remove the members for an instance of Node
/**
* \param[in] key Raw pointer to the Node
*/
void remove(const Node * key)
{
// Removing an instance from the maps requires exclusive access
std::unique_lock lock(map_access_mutex_);
type_descriptions_map_.erase(key);
}
/// \brief Retrieve the NodeTypeDescriptionsInterface for a Node.
/**
* \param[in] key Raw pointer to an instance of Node.
* \return A shared ptr to this Node's NodeTypeDescriptionsInterface instance.
*/
rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr
get_node_type_descriptions_interface(const Node * key) const
{
// Multiple threads can retrieve from the maps at the same time
std::shared_lock lock(map_access_mutex_);
return type_descriptions_map_.at(key);
}
private:
/// \brief Map that stored TypeDescriptionsInterface members
std::unordered_map<
const Node *, rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr
> type_descriptions_map_;
/// \brief Controls access to all private maps
mutable std::shared_mutex map_access_mutex_;
};
// Definition of static member declaration
Node::BackportMembers Node::backport_members_;
Node::Node(
const std::string & node_name,
const NodeOptions & options)
@@ -211,6 +281,8 @@ Node::Node(
sub_namespace_(""),
effective_namespace_(create_effective_namespace(this->get_namespace(), sub_namespace_))
{
backport_members_.add(this);
// we have got what we wanted directly from the overrides,
// but declare the parameters anyway so they are visible.
rclcpp::detail::declare_qos_parameters(
@@ -272,6 +344,7 @@ Node::Node(
Node::~Node()
{
// release sub-interfaces in an order that allows them to consult with node_base during tear-down
backport_members_.remove(this);
node_waitables_.reset();
node_time_source_.reset();
node_parameters_.reset();
@@ -591,6 +664,12 @@ Node::get_node_topics_interface()
return node_topics_;
}
rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr
Node::get_node_type_descriptions_interface()
{
return backport_members_.get_node_type_descriptions_interface(this);
}
rclcpp::node_interfaces::NodeServicesInterface::SharedPtr
Node::get_node_services_interface()
{

View File

@@ -0,0 +1,157 @@
// Copyright 2023 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 <memory>
#include <string>
#include <thread>
#include "rclcpp/node_interfaces/node_type_descriptions.hpp"
#include "rclcpp/parameter_client.hpp"
#include "type_description_interfaces/srv/get_type_description.h"
namespace
{
// Helper wrapper for rclcpp::Service to access ::Request and ::Response types for allocation.
struct GetTypeDescription__C
{
using Request = type_description_interfaces__srv__GetTypeDescription_Request;
using Response = type_description_interfaces__srv__GetTypeDescription_Response;
using Event = type_description_interfaces__srv__GetTypeDescription_Event;
};
} // namespace
// Helper function for C typesupport.
namespace rosidl_typesupport_cpp
{
template<>
rosidl_service_type_support_t const *
get_service_type_support_handle<GetTypeDescription__C>()
{
return ROSIDL_GET_SRV_TYPE_SUPPORT(type_description_interfaces, srv, GetTypeDescription);
}
} // namespace rosidl_typesupport_cpp
namespace rclcpp
{
namespace node_interfaces
{
class NodeTypeDescriptions::NodeTypeDescriptionsImpl
{
public:
using ServiceT = GetTypeDescription__C;
rclcpp::Logger logger_;
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
rclcpp::Service<ServiceT>::SharedPtr type_description_srv_;
NodeTypeDescriptionsImpl(
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters,
rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services)
: logger_(node_logging->get_logger()),
node_base_(node_base)
{
const std::string enable_param_name = "start_type_description_service";
bool enabled = false;
try {
auto enable_param = node_parameters->declare_parameter(
enable_param_name,
rclcpp::ParameterValue(true),
rcl_interfaces::msg::ParameterDescriptor()
.set__name(enable_param_name)
.set__type(rclcpp::PARAMETER_BOOL)
.set__description("Start the ~/get_type_description service for this node.")
.set__read_only(true));
enabled = enable_param.get<bool>();
} catch (const rclcpp::exceptions::InvalidParameterTypeException & exc) {
RCLCPP_ERROR(logger_, "%s", exc.what());
throw;
}
if (enabled) {
auto rcl_node = node_base->get_rcl_node_handle();
rcl_ret_t rcl_ret = rcl_node_type_description_service_init(rcl_node);
if (rcl_ret != RCL_RET_OK) {
RCLCPP_ERROR(
logger_, "Failed to initialize ~/get_type_description_service: %s",
rcl_get_error_string().str);
throw std::runtime_error(
"Failed to initialize ~/get_type_description service.");
}
rcl_service_t * rcl_srv = nullptr;
rcl_ret = rcl_node_get_type_description_service(rcl_node, &rcl_srv);
if (rcl_ret != RCL_RET_OK) {
throw std::runtime_error(
"Failed to get initialized ~/get_type_description service from rcl.");
}
rclcpp::AnyServiceCallback<ServiceT> cb;
cb.set(
[this](
std::shared_ptr<rmw_request_id_t> header,
std::shared_ptr<ServiceT::Request> request,
std::shared_ptr<ServiceT::Response> response
) {
rcl_node_type_description_service_handle_request(
node_base_->get_rcl_node_handle(),
header.get(),
request.get(),
response.get());
});
type_description_srv_ = std::make_shared<Service<ServiceT>>(
node_base_->get_shared_rcl_node_handle(),
rcl_srv,
cb);
node_services->add_service(
std::dynamic_pointer_cast<ServiceBase>(type_description_srv_),
nullptr);
}
}
~NodeTypeDescriptionsImpl()
{
if (
type_description_srv_ &&
RCL_RET_OK != rcl_node_type_description_service_fini(node_base_->get_rcl_node_handle()))
{
RCLCPP_ERROR(
logger_,
"Error in shutdown of get_type_description service: %s", rcl_get_error_string().str);
}
}
};
NodeTypeDescriptions::NodeTypeDescriptions(
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters,
rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services)
: impl_(new NodeTypeDescriptionsImpl(
node_base,
node_logging,
node_parameters,
node_services))
{}
NodeTypeDescriptions::~NodeTypeDescriptions()
{}
} // namespace node_interfaces
} // namespace rclcpp

View File

@@ -52,7 +52,7 @@ SubscriptionBase::SubscriptionBase(
intra_process_subscription_id_(0),
event_callbacks_(event_callbacks),
type_support_(type_support_handle),
delivered_message_type_(delivered_message_kind)
delivered_message_kind_(delivered_message_kind)
{
auto custom_deletor = [node_handle = this->node_handle_](rcl_subscription_t * rcl_subs)
{
@@ -261,13 +261,13 @@ SubscriptionBase::get_message_type_support_handle() const
bool
SubscriptionBase::is_serialized() const
{
return delivered_message_type_ == rclcpp::DeliveredMessageKind::SERIALIZED_MESSAGE;
return delivered_message_kind_ == rclcpp::DeliveredMessageKind::SERIALIZED_MESSAGE;
}
rclcpp::DeliveredMessageKind
SubscriptionBase::get_subscription_type() const
SubscriptionBase::get_delivered_message_kind() const
{
return delivered_message_type_;
return delivered_message_kind_;
}
size_t

View File

@@ -262,6 +262,11 @@ if(TARGET test_node_interfaces__node_topics)
"test_msgs")
target_link_libraries(test_node_interfaces__node_topics ${PROJECT_NAME} mimick)
endif()
ament_add_gtest(test_node_interfaces__node_type_descriptions
node_interfaces/test_node_type_descriptions.cpp)
if(TARGET test_node_interfaces__node_type_descriptions)
target_link_libraries(test_node_interfaces__node_type_descriptions ${PROJECT_NAME} mimick)
endif()
ament_add_gtest(test_node_interfaces__node_waitables
node_interfaces/test_node_waitables.cpp)
if(TARGET test_node_interfaces__node_waitables)

View File

@@ -20,12 +20,14 @@
#include <gtest/gtest.h>
#include <algorithm>
#include <atomic>
#include <chrono>
#include <limits>
#include <memory>
#include <string>
#include <thread>
#include <utility>
#include <vector>
#include "rcl/error_handling.h"
#include "rcl/time.h"
@@ -43,18 +45,10 @@ template<typename T>
class TestExecutors : public ::testing::Test
{
public:
static void SetUpTestCase()
{
rclcpp::init(0, nullptr);
}
static void TearDownTestCase()
{
rclcpp::shutdown();
}
void SetUp()
{
rclcpp::init(0, nullptr);
const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info();
std::stringstream test_name;
test_name << test_info->test_case_name() << "_" << test_info->name();
@@ -75,6 +69,8 @@ public:
publisher.reset();
subscription.reset();
node.reset();
rclcpp::shutdown();
}
rclcpp::Node::SharedPtr node;
@@ -729,6 +725,77 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted)
spinner.join();
}
// This test verifies that the add_node operation is robust wrt race conditions.
// It's mostly meant to prevent regressions in the events-executor, but the operation should be
// thread-safe in all executor implementations.
// The initial implementation of the events-executor contained a bug where the executor
// would end up in an inconsistent state and stop processing interrupt/shutdown notifications.
// Manually adding a node to the executor results in a) producing a notify waitable event
// and b) refreshing the executor collections.
// The inconsistent state would happen if the event was processed before the collections were
// finished to be refreshed: the executor would pick up the event but be unable to process it.
// This would leave the `notify_waitable_event_pushed_` flag to true, preventing additional
// notify waitable events to be pushed.
// The behavior is observable only under heavy load, so this test spawns several worker
// threads. Due to the nature of the bug, this test may still succeed even if the
// bug is present. However repeated runs will show its flakiness nature and indicate
// an eventual regression.
TYPED_TEST(TestExecutors, testRaceConditionAddNode)
{
using ExecutorType = TypeParam;
// rmw_connextdds doesn't support events-executor
if (
std::is_same<ExecutorType, rclcpp::experimental::executors::EventsExecutor>() &&
std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0)
{
GTEST_SKIP();
}
// Spawn some threads to do some heavy work
std::atomic<bool> should_cancel = false;
std::vector<std::thread> stress_threads;
for (size_t i = 0; i < 5 * std::thread::hardware_concurrency(); i++) {
stress_threads.emplace_back(
[&should_cancel, i]() {
// This is just some arbitrary heavy work
volatile size_t total = 0;
for (size_t k = 0; k < 549528914167; k++) {
if (should_cancel) {
break;
}
total += k * (i + 42);
}
});
}
// Create an executor
auto executor = std::make_shared<ExecutorType>();
// Start spinning
auto executor_thread = std::thread(
[executor]() {
executor->spin();
});
// Add a node to the executor
executor->add_node(this->node);
// Cancel the executor (make sure that it's already spinning first)
while (!executor->is_spinning() && rclcpp::ok()) {
continue;
}
executor->cancel();
// Try to join the thread after cancelling the executor
// This is the "test". We want to make sure that we can still cancel the executor
// regardless of the presence of race conditions
executor_thread.join();
// The test is now completed: we can join the stress threads
should_cancel = true;
for (auto & t : stress_threads) {
t.join();
}
}
// Check spin_until_future_complete with node base pointer (instantiates its own executor)
TEST(TestExecutors, testSpinUntilFutureCompleteNodeBasePtr)
{
@@ -791,7 +858,7 @@ public:
test_name << test_info->test_case_name() << "_" << test_info->name();
node = std::make_shared<rclcpp::Node>("node", test_name.str());
callback_count = 0;
callback_count = 0u;
const std::string topic_name = std::string("topic_") + test_name.str();
@@ -800,7 +867,7 @@ public:
publisher = node->create_publisher<test_msgs::msg::Empty>(topic_name, rclcpp::QoS(1), po);
auto callback = [this](test_msgs::msg::Empty::ConstSharedPtr) {
this->callback_count.fetch_add(1);
this->callback_count.fetch_add(1u);
};
rclcpp::SubscriptionOptions so;
@@ -822,7 +889,7 @@ public:
rclcpp::Node::SharedPtr node;
rclcpp::Publisher<test_msgs::msg::Empty>::SharedPtr publisher;
rclcpp::Subscription<test_msgs::msg::Empty>::SharedPtr subscription;
std::atomic_int callback_count;
std::atomic_size_t callback_count;
};
TYPED_TEST_SUITE(TestIntraprocessExecutors, ExecutorTypes, ExecutorTypeNames);
@@ -838,7 +905,7 @@ TYPED_TEST(TestIntraprocessExecutors, testIntraprocessRetrigger) {
ExecutorType executor;
executor.add_node(this->node);
EXPECT_EQ(0, this->callback_count.load());
EXPECT_EQ(0u, this->callback_count.load());
this->publisher->publish(test_msgs::msg::Empty());
// Wait for up to 5 seconds for the first message to come available.
@@ -852,7 +919,7 @@ TYPED_TEST(TestIntraprocessExecutors, testIntraprocessRetrigger) {
EXPECT_EQ(1u, this->callback_count.load());
// reset counter
this->callback_count.store(0);
this->callback_count.store(0u);
for (size_t ii = 0; ii < kNumMessages; ++ii) {
this->publisher->publish(test_msgs::msg::Empty());

View File

@@ -77,9 +77,9 @@ TEST_F(TestNodeParameters, list_parameters)
std::vector<std::string> prefixes;
const auto list_result = node_parameters->list_parameters(prefixes, 1u);
// Currently the only default parameter is 'use_sim_time', but that may change.
// Currently the default parameters are 'use_sim_time' and 'start_type_description_service'
size_t number_of_parameters = list_result.names.size();
EXPECT_GE(1u, number_of_parameters);
EXPECT_GE(2u, number_of_parameters);
const std::string parameter_name = "new_parameter";
const rclcpp::ParameterValue value(true);

View File

@@ -0,0 +1,63 @@
// Copyright 2023 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.hpp"
#include "rclcpp/node_interfaces/node_type_descriptions.hpp"
class TestNodeTypeDescriptions : public ::testing::Test
{
public:
void SetUp()
{
rclcpp::init(0, nullptr);
}
void TearDown()
{
rclcpp::shutdown();
}
};
TEST_F(TestNodeTypeDescriptions, interface_created)
{
rclcpp::Node node{"node", "ns"};
ASSERT_NE(nullptr, node.get_node_type_descriptions_interface());
}
TEST_F(TestNodeTypeDescriptions, disabled_no_service)
{
rclcpp::NodeOptions node_options;
node_options.append_parameter_override("start_type_description_service", false);
rclcpp::Node node{"node", "ns", node_options};
rcl_node_t * rcl_node = node.get_node_base_interface()->get_rcl_node_handle();
rcl_service_t * srv = nullptr;
rcl_ret_t ret = rcl_node_get_type_description_service(rcl_node, &srv);
ASSERT_EQ(RCL_RET_NOT_INIT, ret);
}
TEST_F(TestNodeTypeDescriptions, enabled_creates_service)
{
rclcpp::NodeOptions node_options;
node_options.append_parameter_override("start_type_description_service", true);
rclcpp::Node node{"node", "ns", node_options};
rcl_node_t * rcl_node = node.get_node_base_interface()->get_rcl_node_handle();
rcl_service_t * srv = nullptr;
rcl_ret_t ret = rcl_node_get_type_description_service(rcl_node, &srv);
ASSERT_EQ(RCL_RET_OK, ret);
ASSERT_NE(nullptr, srv);
}

View File

@@ -78,6 +78,7 @@ TEST_F(TestNode, construction_and_destruction) {
EXPECT_NE(nullptr, node->get_node_options().get_rcl_node_options());
EXPECT_NE(nullptr, node->get_graph_event());
EXPECT_NE(nullptr, node->get_clock());
EXPECT_NE(nullptr, node->get_node_type_descriptions_interface());
}
{

View File

@@ -59,6 +59,8 @@ protected:
node_with_option.reset();
}
// "start_type_description_service" and "use_sim_time"
const uint64_t builtin_param_count = 2;
rclcpp::Node::SharedPtr node;
rclcpp::Node::SharedPtr node_with_option;
};
@@ -925,6 +927,7 @@ TEST_F(TestParameterClient, sync_parameter_delete_parameters) {
Coverage for async load_parameters
*/
TEST_F(TestParameterClient, async_parameter_load_parameters) {
const uint64_t expected_param_count = 4 + builtin_param_count;
auto load_node = std::make_shared<rclcpp::Node>(
"load_node",
"namespace",
@@ -944,12 +947,13 @@ TEST_F(TestParameterClient, async_parameter_load_parameters) {
auto list_parameters = asynchronous_client->list_parameters({}, 3);
rclcpp::spin_until_future_complete(
load_node, list_parameters, std::chrono::milliseconds(100));
ASSERT_EQ(list_parameters.get().names.size(), static_cast<uint64_t>(5));
ASSERT_EQ(list_parameters.get().names.size(), expected_param_count);
}
/*
Coverage for sync load_parameters
*/
TEST_F(TestParameterClient, sync_parameter_load_parameters) {
const uint64_t expected_param_count = 4 + builtin_param_count;
auto load_node = std::make_shared<rclcpp::Node>(
"load_node",
"namespace",
@@ -964,13 +968,14 @@ TEST_F(TestParameterClient, sync_parameter_load_parameters) {
ASSERT_EQ(load_future[0].successful, true);
// list parameters
auto list_parameters = synchronous_client->list_parameters({}, 3);
ASSERT_EQ(list_parameters.names.size(), static_cast<uint64_t>(5));
ASSERT_EQ(list_parameters.names.size(), static_cast<uint64_t>(expected_param_count));
}
/*
Coverage for async load_parameters with complicated regex expression
*/
TEST_F(TestParameterClient, async_parameter_load_parameters_complicated_regex) {
const uint64_t expected_param_count = 5 + builtin_param_count;
auto load_node = std::make_shared<rclcpp::Node>(
"load_node",
"namespace",
@@ -990,7 +995,7 @@ TEST_F(TestParameterClient, async_parameter_load_parameters_complicated_regex) {
auto list_parameters = asynchronous_client->list_parameters({}, 3);
rclcpp::spin_until_future_complete(
load_node, list_parameters, std::chrono::milliseconds(100));
ASSERT_EQ(list_parameters.get().names.size(), static_cast<uint64_t>(6));
ASSERT_EQ(list_parameters.get().names.size(), expected_param_count);
// to check the parameter "a_value"
std::string param_name = "a_value";
auto param = load_node->get_parameter(param_name);
@@ -1020,6 +1025,7 @@ TEST_F(TestParameterClient, async_parameter_load_no_valid_parameter) {
Coverage for async load_parameters from maps with complicated regex expression
*/
TEST_F(TestParameterClient, async_parameter_load_parameters_from_map) {
const uint64_t expected_param_count = 5 + builtin_param_count;
auto load_node = std::make_shared<rclcpp::Node>(
"load_node",
"namespace",
@@ -1068,7 +1074,7 @@ TEST_F(TestParameterClient, async_parameter_load_parameters_from_map) {
auto list_parameters = asynchronous_client->list_parameters({}, 3);
rclcpp::spin_until_future_complete(
load_node, list_parameters, std::chrono::milliseconds(100));
ASSERT_EQ(list_parameters.get().names.size(), static_cast<uint64_t>(6));
ASSERT_EQ(list_parameters.get().names.size(), expected_param_count);
// to check the parameter "a_value"
std::string param_name = "a_value";
auto param = load_node->get_parameter(param_name);

View File

@@ -3,6 +3,15 @@ Changelog for package rclcpp_action
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
21.0.3 (2023-09-08)
-------------------
21.0.2 (2023-07-14)
-------------------
21.0.1 (2023-05-11)
-------------------
21.0.0 (2023-04-18)
-------------------

View File

@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="2">
<name>rclcpp_action</name>
<version>21.0.0</version>
<version>21.0.3</version>
<description>Adds action APIs for C++.</description>
<maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>

View File

@@ -2,6 +2,15 @@
Changelog for package rclcpp_components
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
21.0.3 (2023-09-08)
-------------------
21.0.2 (2023-07-14)
-------------------
21.0.1 (2023-05-11)
-------------------
21.0.0 (2023-04-18)
-------------------

View File

@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="2">
<name>rclcpp_components</name>
<version>21.0.0</version>
<version>21.0.3</version>
<description>Package containing tools for dynamically loadable components</description>
<maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>

View File

@@ -3,6 +3,18 @@ Changelog for package rclcpp_lifecycle
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
21.0.3 (2023-09-08)
-------------------
* Switch lifecycle to use the RCLCPP macros. (`#2244 <https://github.com/ros2/rclcpp/issues/2244>`_)
* Add new node interface TypeDescriptionsInterface to provide GetTypeDescription service (`#2236 <https://github.com/ros2/rclcpp/issues/2236>`_)
* Contributors: Emerson Knapp
21.0.2 (2023-07-14)
-------------------
21.0.1 (2023-05-11)
-------------------
21.0.0 (2023-04-18)
-------------------
* Add support for logging service. (`#2122 <https://github.com/ros2/rclcpp/issues/2122>`_)

View File

@@ -72,6 +72,7 @@
#include "rclcpp/node_interfaces/node_time_source_interface.hpp"
#include "rclcpp/node_interfaces/node_timers_interface.hpp"
#include "rclcpp/node_interfaces/node_topics_interface.hpp"
#include "rclcpp/node_interfaces/node_type_descriptions_interface.hpp"
#include "rclcpp/node_interfaces/node_waitables_interface.hpp"
#include "rclcpp/parameter.hpp"
#include "rclcpp/publisher.hpp"
@@ -823,6 +824,14 @@ public:
rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr
get_node_time_source_interface();
/// Return the Node's internal NodeTypeDescriptionsInterface implementation.
/**
* \sa rclcpp::Node::get_node_type_descriptions_interface
*/
RCLCPP_LIFECYCLE_PUBLIC
rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr
get_node_type_descriptions_interface();
/// Return the Node's internal NodeWaitablesInterface implementation.
/**
* \sa rclcpp::Node::get_node_waitables_interface

View File

@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="2">
<name>rclcpp_lifecycle</name>
<version>21.0.0</version>
<version>21.0.3</version>
<description>Package containing a prototype for lifecycle implementation</description>
<maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>

View File

@@ -43,6 +43,7 @@
#include "rclcpp/node_interfaces/node_time_source.hpp"
#include "rclcpp/node_interfaces/node_timers.hpp"
#include "rclcpp/node_interfaces/node_topics.hpp"
#include "rclcpp/node_interfaces/node_type_descriptions.hpp"
#include "rclcpp/node_interfaces/node_waitables.hpp"
#include "rclcpp/parameter_service.hpp"
#include "rclcpp/qos.hpp"
@@ -115,7 +116,11 @@ LifecycleNode::LifecycleNode(
)),
node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())),
node_options_(options),
impl_(new LifecycleNodeInterfaceImpl(node_base_, node_services_))
impl_(new LifecycleNodeInterfaceImpl(
node_base_,
node_logging_,
node_parameters_,
node_services_))
{
impl_->init(enable_communication_interface);
@@ -474,6 +479,12 @@ LifecycleNode::get_node_services_interface()
return node_services_;
}
rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr
LifecycleNode::get_node_type_descriptions_interface()
{
return impl_->get_node_type_descriptions_interface();
}
rclcpp::node_interfaces::NodeParametersInterface::SharedPtr
LifecycleNode::get_node_parameters_interface()
{

View File

@@ -29,7 +29,9 @@
#include "lifecycle_msgs/srv/get_available_transitions.hpp"
#include "rclcpp/node_interfaces/node_base_interface.hpp"
#include "rclcpp/node_interfaces/node_logging_interface.hpp"
#include "rclcpp/node_interfaces/node_services_interface.hpp"
#include "rclcpp/node_interfaces/node_type_descriptions.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
@@ -50,9 +52,17 @@ namespace rclcpp_lifecycle
LifecycleNode::LifecycleNodeInterfaceImpl::LifecycleNodeInterfaceImpl(
std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> node_base_interface,
std::shared_ptr<rclcpp::node_interfaces::NodeLoggingInterface> node_logging_interface,
std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> node_parameters_interface,
std::shared_ptr<rclcpp::node_interfaces::NodeServicesInterface> node_services_interface)
: node_base_interface_(node_base_interface),
node_services_interface_(node_services_interface)
node_services_interface_(node_services_interface),
node_logging_interface_(node_logging_interface),
node_type_descriptions_(new rclcpp::node_interfaces::NodeTypeDescriptions(
node_base_interface,
node_logging_interface,
node_parameters_interface,
node_services_interface))
{
}
@@ -65,8 +75,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::~LifecycleNodeInterfaceImpl()
ret = rcl_lifecycle_state_machine_fini(&state_machine_, node_handle);
}
if (ret != RCL_RET_OK) {
RCUTILS_LOG_FATAL_NAMED(
"rclcpp_lifecycle",
RCLCPP_FATAL(
node_logging_interface_->get_logger(),
"failed to destroy rcl_state_machine");
}
}
@@ -398,7 +408,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state(
{
std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
RCUTILS_LOG_ERROR(
RCLCPP_ERROR(
node_logging_interface_->get_logger(),
"Unable to change state for state machine for %s: %s",
node_base_interface_->get_name(), rcl_get_error_string().str);
return RCL_RET_ERROR;
@@ -411,7 +422,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state(
rcl_lifecycle_trigger_transition_by_id(
&state_machine_, transition_id, publish_update) != RCL_RET_OK)
{
RCUTILS_LOG_ERROR(
RCLCPP_ERROR(
node_logging_interface_->get_logger(),
"Unable to start transition %u from current state %s: %s",
transition_id, state_machine_.current_state->label, rcl_get_error_string().str);
rcutils_reset_error();
@@ -443,7 +455,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state(
rcl_lifecycle_trigger_transition_by_label(
&state_machine_, transition_label, publish_update) != RCL_RET_OK)
{
RCUTILS_LOG_ERROR(
RCLCPP_ERROR(
node_logging_interface_->get_logger(),
"Failed to finish transition %u. Current state is now: %s (%s)",
transition_id, state_machine_.current_state->label, rcl_get_error_string().str);
rcutils_reset_error();
@@ -458,7 +471,9 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state(
// error handling ?!
// TODO(karsten1987): iterate over possible ret value
if (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR) {
RCUTILS_LOG_WARN("Error occurred while doing error handling.");
RCLCPP_WARN(
node_logging_interface_->get_logger(),
"Error occurred while doing error handling.");
auto error_cb_code = execute_callback(current_state_id, initial_state);
auto error_cb_label = get_label_for_return_code(error_cb_code);
@@ -467,7 +482,9 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state(
rcl_lifecycle_trigger_transition_by_label(
&state_machine_, error_cb_label, publish_update) != RCL_RET_OK)
{
RCUTILS_LOG_ERROR("Failed to call cleanup on error state: %s", rcl_get_error_string().str);
RCLCPP_ERROR(
node_logging_interface_->get_logger(),
"Failed to call cleanup on error state: %s", rcl_get_error_string().str);
rcutils_reset_error();
return RCL_RET_ERROR;
}
@@ -495,8 +512,12 @@ LifecycleNode::LifecycleNodeInterfaceImpl::execute_callback(
try {
cb_success = callback(State(previous_state));
} catch (const std::exception & e) {
RCUTILS_LOG_ERROR("Caught exception in callback for transition %d", it->first);
RCUTILS_LOG_ERROR("Original error: %s", e.what());
RCLCPP_ERROR(
node_logging_interface_->get_logger(),
"Caught exception in callback for transition %d", it->first);
RCLCPP_ERROR(
node_logging_interface_->get_logger(),
"Original error: %s", e.what());
cb_success = node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
}
}
@@ -581,4 +602,10 @@ LifecycleNode::LifecycleNodeInterfaceImpl::on_deactivate() const
}
}
rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr
LifecycleNode::LifecycleNodeInterfaceImpl::get_node_type_descriptions_interface()
{
return node_type_descriptions_;
}
} // namespace rclcpp_lifecycle

View File

@@ -32,6 +32,7 @@
#include "rclcpp/macros.hpp"
#include "rclcpp/node_interfaces/node_base_interface.hpp"
#include "rclcpp/node_interfaces/node_logging_interface.hpp"
#include "rclcpp/node_interfaces/node_services_interface.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
@@ -52,6 +53,8 @@ class LifecycleNode::LifecycleNodeInterfaceImpl final
public:
LifecycleNodeInterfaceImpl(
std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> node_base_interface,
std::shared_ptr<rclcpp::node_interfaces::NodeLoggingInterface> node_logging_interface,
std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> node_parameters_interface,
std::shared_ptr<rclcpp::node_interfaces::NodeServicesInterface> node_services_interface);
~LifecycleNodeInterfaceImpl();
@@ -102,6 +105,9 @@ public:
void
add_timer_handle(std::shared_ptr<rclcpp::TimerBase> timer);
rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr
get_node_type_descriptions_interface();
private:
RCLCPP_DISABLE_COPY(LifecycleNodeInterfaceImpl)
@@ -152,6 +158,7 @@ private:
using NodeBasePtr = std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>;
using NodeServicesPtr = std::shared_ptr<rclcpp::node_interfaces::NodeServicesInterface>;
using NodeLoggingPtr = std::shared_ptr<rclcpp::node_interfaces::NodeLoggingInterface>;
using ChangeStateSrvPtr = std::shared_ptr<rclcpp::Service<ChangeStateSrv>>;
using GetStateSrvPtr = std::shared_ptr<rclcpp::Service<GetStateSrv>>;
using GetAvailableStatesSrvPtr =
@@ -163,6 +170,7 @@ private:
NodeBasePtr node_base_interface_;
NodeServicesPtr node_services_interface_;
NodeLoggingPtr node_logging_interface_;
ChangeStateSrvPtr srv_change_state_;
GetStateSrvPtr srv_get_state_;
GetAvailableStatesSrvPtr srv_get_available_states_;
@@ -172,6 +180,9 @@ private:
// to controllable things
std::vector<std::weak_ptr<rclcpp_lifecycle::ManagedEntityInterface>> weak_managed_entities_;
std::vector<std::weak_ptr<rclcpp::TimerBase>> weak_timers_;
// Backported members hidden in impl
rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr node_type_descriptions_;
};
} // namespace rclcpp_lifecycle

View File

@@ -427,11 +427,15 @@ TEST_F(TestDefaultStateMachine, lifecycle_subscriber) {
// Parameters are tested more thoroughly in rclcpp's test_node.cpp
// These are provided for coverage of lifecycle node's API
TEST_F(TestDefaultStateMachine, declare_parameters) {
// "start_type_description_service" and "use_sim_time"
const uint64_t builtin_param_count = 2;
const uint64_t expected_param_count = 6 + builtin_param_count;
auto test_node = std::make_shared<EmptyLifecycleNode>("testnode");
auto list_result = test_node->list_parameters({}, 0u);
EXPECT_EQ(list_result.names.size(), 1u);
EXPECT_STREQ(list_result.names[0].c_str(), "use_sim_time");
EXPECT_EQ(list_result.names.size(), builtin_param_count);
EXPECT_STREQ(list_result.names[0].c_str(), "start_type_description_service");
EXPECT_STREQ(list_result.names[1].c_str(), "use_sim_time");
const std::string bool_name = "test_boolean";
const std::string int_name = "test_int";
@@ -469,10 +473,11 @@ TEST_F(TestDefaultStateMachine, declare_parameters) {
test_node->declare_parameters("test_double", double_parameters);
list_result = test_node->list_parameters({}, 0u);
EXPECT_EQ(list_result.names.size(), 7u);
EXPECT_EQ(list_result.names.size(), expected_param_count);
// The order of these names is not controlled by lifecycle_node, doing set equality
std::set<std::string> expected_names = {
"start_type_description_service",
"test_boolean",
"test_double.double_one",
"test_double.double_two",
@@ -487,11 +492,13 @@ TEST_F(TestDefaultStateMachine, declare_parameters) {
}
TEST_F(TestDefaultStateMachine, check_parameters) {
const uint64_t builtin_param_count = 2;
auto test_node = std::make_shared<EmptyLifecycleNode>("testnode");
auto list_result = test_node->list_parameters({}, 0u);
EXPECT_EQ(list_result.names.size(), 1u);
EXPECT_STREQ(list_result.names[0].c_str(), "use_sim_time");
EXPECT_EQ(list_result.names.size(), builtin_param_count);
EXPECT_STREQ(list_result.names[0].c_str(), "start_type_description_service");
EXPECT_STREQ(list_result.names[1].c_str(), "use_sim_time");
const std::string bool_name = "test_boolean";
const std::string int_name = "test_int";
@@ -549,8 +556,7 @@ TEST_F(TestDefaultStateMachine, check_parameters) {
std::map<std::string, rclcpp::ParameterValue> parameter_map;
EXPECT_TRUE(test_node->get_parameters({}, parameter_map));
// int param, bool param, and use_sim_time
EXPECT_EQ(parameter_map.size(), 3u);
EXPECT_EQ(parameter_map.size(), parameter_names.size() + builtin_param_count);
// Check parameter types
auto parameter_types = test_node->get_parameter_types(parameter_names);
@@ -585,10 +591,12 @@ TEST_F(TestDefaultStateMachine, check_parameters) {
// List parameters
list_result = test_node->list_parameters({}, 0u);
EXPECT_EQ(list_result.names.size(), 3u);
EXPECT_STREQ(list_result.names[0].c_str(), parameter_names[0].c_str());
EXPECT_STREQ(list_result.names[1].c_str(), parameter_names[1].c_str());
EXPECT_STREQ(list_result.names[2].c_str(), "use_sim_time");
EXPECT_EQ(list_result.names.size(), parameter_names.size() + builtin_param_count);
size_t index = 0;
EXPECT_STREQ(list_result.names[index++].c_str(), "start_type_description_service");
EXPECT_STREQ(list_result.names[index++].c_str(), parameter_names[0].c_str());
EXPECT_STREQ(list_result.names[index++].c_str(), parameter_names[1].c_str());
EXPECT_STREQ(list_result.names[index++].c_str(), "use_sim_time");
// Undeclare parameter
test_node->undeclare_parameter(bool_name);
@@ -633,6 +641,7 @@ TEST_F(TestDefaultStateMachine, test_getters) {
EXPECT_LT(0u, test_node->now().nanoseconds());
EXPECT_STREQ("testnode", test_node->get_logger().get_name());
EXPECT_NE(nullptr, const_cast<const EmptyLifecycleNode *>(test_node.get())->get_clock());
EXPECT_NE(nullptr, test_node->get_node_type_descriptions_interface());
}
TEST_F(TestDefaultStateMachine, test_graph_topics) {