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

Make the rcl publisher handle a shared pointer.

Browse source 
Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>
This commit is contained in:
Chris Lalancette 2020-05-20 19:54:18 +00:00 • committed by Chris Lalancette
parent 1a48a60a75
commit 4efcfdc16b
4 changed files with 45 additions and 36 deletions
Download patch file Download diff file Expand all files Collapse all files

4
rclcpp/include/rclcpp/loaned_message.hpp
View file

@ -66,7 +66,7 @@ public:
if (pub_.can_loan_messages()) {
void * message_ptr = nullptr;
auto ret = rcl_borrow_loaned_message(
pub_.get_publisher_handle(),
pub_.get_publisher_handle().get(),
rosidl_typesupport_cpp::get_message_type_support_handle<MessageT>(),
&message_ptr);
if (RCL_RET_OK != ret) {
@ -139,7 +139,7 @@ public:
if (pub_.can_loan_messages()) {
// return allocated memory to the middleware
auto ret =
rcl_return_loaned_message_from_publisher(pub_.get_publisher_handle(), message_);
rcl_return_loaned_message_from_publisher(pub_.get_publisher_handle().get(), message_);
if (ret != RCL_RET_OK) {
RCLCPP_ERROR(
error_logger, "rcl_deallocate_loaned_message failed: %s", rcl_get_error_string().str);

14
rclcpp/include/rclcpp/publisher.hpp
View file

@ -279,12 +279,12 @@ protected:
void
do_inter_process_publish(const MessageT & msg)
{
auto status = rcl_publish(&publisher_handle_, &msg, nullptr);
auto status = rcl_publish(publisher_handle_.get(), &msg, nullptr);
if (RCL_RET_PUBLISHER_INVALID == status) {
rcl_reset_error(); // next call will reset error message if not context
if (rcl_publisher_is_valid_except_context(&publisher_handle_)) {
rcl_context_t * context = rcl_publisher_get_context(&publisher_handle_);
if (rcl_publisher_is_valid_except_context(publisher_handle_.get())) {
rcl_context_t * context = rcl_publisher_get_context(publisher_handle_.get());
if (nullptr != context && !rcl_context_is_valid(context)) {
// publisher is invalid due to context being shutdown
return;
@ -303,7 +303,7 @@ protected:
// TODO(Karsten1987): support serialized message passed by intraprocess
throw std::runtime_error("storing serialized messages in intra process is not supported yet");
}
auto status = rcl_publish_serialized_message(&publisher_handle_, serialized_msg, nullptr);
auto status = rcl_publish_serialized_message(publisher_handle_.get(), serialized_msg, nullptr);
if (RCL_RET_OK != status) {
rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish serialized message");
}
@ -312,12 +312,12 @@ protected:
void
do_loaned_message_publish(MessageT * msg)
{
auto status = rcl_publish_loaned_message(&publisher_handle_, msg, nullptr);
auto status = rcl_publish_loaned_message(publisher_handle_.get(), msg, nullptr);
if (RCL_RET_PUBLISHER_INVALID == status) {
rcl_reset_error(); // next call will reset error message if not context
if (rcl_publisher_is_valid_except_context(&publisher_handle_)) {
rcl_context_t * context = rcl_publisher_get_context(&publisher_handle_);
if (rcl_publisher_is_valid_except_context(publisher_handle_.get())) {
rcl_context_t * context = rcl_publisher_get_context(publisher_handle_.get());
if (nullptr != context && !rcl_context_is_valid(context)) {
// publisher is invalid due to context being shutdown
return;

8
rclcpp/include/rclcpp/publisher_base.hpp
View file

@ -99,13 +99,13 @@ public:
/// Get the rcl publisher handle.
/** \return The rcl publisher handle. */
RCLCPP_PUBLIC
rcl_publisher_t *
std::shared_ptr<rcl_publisher_t>
get_publisher_handle();
/// Get the rcl publisher handle.
/** \return The rcl publisher handle. */
RCLCPP_PUBLIC
const rcl_publisher_t *
std::shared_ptr<const rcl_publisher_t>
get_publisher_handle() const;
/// Get all the QoS event handlers associated with this publisher.
@ -203,7 +203,7 @@ protected:
auto handler = std::make_shared<QOSEventHandler<EventCallbackT>>(
callback,
rcl_publisher_event_init,
&publisher_handle_,
publisher_handle_.get(),
event_type);
event_handlers_.emplace_back(handler);
}
@ -213,7 +213,7 @@ protected:
std::shared_ptr<rcl_node_t> rcl_node_handle_;
rcl_publisher_t publisher_handle_ = rcl_get_zero_initialized_publisher();
std::shared_ptr<rcl_publisher_t> publisher_handle_;
std::vector<std::shared_ptr<rclcpp::QOSEventHandlerBase>> event_handlers_;

55
rclcpp/src/rclcpp/publisher_base.cpp
View file

@ -47,8 +47,24 @@ PublisherBase::PublisherBase(
: rcl_node_handle_(node_base->get_shared_rcl_node_handle()),
intra_process_is_enabled_(false), intra_process_publisher_id_(0)
{
auto custom_deleter = [node_handle = this->rcl_node_handle_](rcl_publisher_t * rcl_pub)
{
if (rcl_publisher_fini(rcl_pub, node_handle.get()) != RCL_RET_OK) {
RCLCPP_ERROR(
rclcpp::get_node_logger(node_handle.get()).get_child("rclcpp"),
"Error in destruction of rcl publisher handle: %s",
rcl_get_error_string().str);
rcl_reset_error();
}
delete rcl_pub;
};
publisher_handle_ = std::shared_ptr<rcl_publisher_t>(
new rcl_publisher_t, custom_deleter);
*publisher_handle_.get() = rcl_get_zero_initialized_publisher();
rcl_ret_t ret = rcl_publisher_init(
&publisher_handle_,
publisher_handle_.get(),
rcl_node_handle_.get(),
&type_support,
topic.c_str(),
@ -67,7 +83,7 @@ PublisherBase::PublisherBase(
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_);
rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle(publisher_handle_.get());
if (!publisher_rmw_handle) {
auto msg = std::string("failed to get rmw handle: ") + rcl_get_error_string().str;
rcl_reset_error();
@ -85,14 +101,6 @@ PublisherBase::~PublisherBase()
// must fini the events before fini-ing the publisher
event_handlers_.clear();
if (rcl_publisher_fini(&publisher_handle_, rcl_node_handle_.get()) != RCL_RET_OK) {
RCUTILS_LOG_ERROR_NAMED(
"rclcpp",
"Error in destruction of rcl publisher handle: %s",
rcl_get_error_string().str);
rcl_reset_error();
}
auto ipm = weak_ipm_.lock();
if (!intra_process_is_enabled_) {
@ -102,7 +110,7 @@ PublisherBase::~PublisherBase()
// TODO(ivanpauno): should this raise an error?
RCLCPP_WARN(
rclcpp::get_logger("rclcpp"),
"Intra process manager died before than a publisher.");
"Intra process manager died before a publisher.");
return;
}
ipm->remove_publisher(intra_process_publisher_id_);
@ -111,13 +119,14 @@ PublisherBase::~PublisherBase()
const char *
PublisherBase::get_topic_name() const
{
return rcl_publisher_get_topic_name(&publisher_handle_);
return rcl_publisher_get_topic_name(publisher_handle_.get());
}
size_t
PublisherBase::get_queue_size() const
{
const rcl_publisher_options_t * publisher_options = rcl_publisher_get_options(&publisher_handle_);
const rcl_publisher_options_t * publisher_options = rcl_publisher_get_options(
publisher_handle_.get());
if (!publisher_options) {
auto msg = std::string("failed to get publisher options: ") + rcl_get_error_string().str;
rcl_reset_error();
@ -132,16 +141,16 @@ PublisherBase::get_gid() const
return rmw_gid_;
}
rcl_publisher_t *
std::shared_ptr<rcl_publisher_t>
PublisherBase::get_publisher_handle()
{
return &publisher_handle_;
return publisher_handle_;
}
const rcl_publisher_t *
std::shared_ptr<const rcl_publisher_t>
PublisherBase::get_publisher_handle() const
{
return &publisher_handle_;
return publisher_handle_;
}
const std::vector<std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
@ -156,13 +165,13 @@ PublisherBase::get_subscription_count() const
size_t inter_process_subscription_count = 0;
rcl_ret_t status = rcl_publisher_get_subscription_count(
&publisher_handle_,
publisher_handle_.get(),
&inter_process_subscription_count);
if (RCL_RET_PUBLISHER_INVALID == status) {
rcl_reset_error(); /* next call will reset error message if not context */
if (rcl_publisher_is_valid_except_context(&publisher_handle_)) {
rcl_context_t * context = rcl_publisher_get_context(&publisher_handle_);
if (rcl_publisher_is_valid_except_context(publisher_handle_.get())) {
rcl_context_t * context = rcl_publisher_get_context(publisher_handle_.get());
if (nullptr != context && !rcl_context_is_valid(context)) {
/* publisher is invalid due to context being shutdown */
return 0;
@ -195,7 +204,7 @@ PublisherBase::get_intra_process_subscription_count() const
rclcpp::QoS
PublisherBase::get_actual_qos() const
{
const rmw_qos_profile_t * qos = rcl_publisher_get_actual_qos(&publisher_handle_);
const rmw_qos_profile_t * qos = rcl_publisher_get_actual_qos(publisher_handle_.get());
if (!qos) {
auto msg = std::string("failed to get qos settings: ") + rcl_get_error_string().str;
rcl_reset_error();
@ -208,13 +217,13 @@ PublisherBase::get_actual_qos() const
bool
PublisherBase::assert_liveliness() const
{
return RCL_RET_OK == rcl_publisher_assert_liveliness(&publisher_handle_);
return RCL_RET_OK == rcl_publisher_assert_liveliness(publisher_handle_.get());
}
bool
PublisherBase::can_loan_messages() const
{
return rcl_publisher_can_loan_messages(&publisher_handle_);
return rcl_publisher_can_loan_messages(publisher_handle_.get());
}
bool