SMACC2
smacc_publisher_client.hpp
Go to the documentation of this file.
1// Copyright 2021 RobosoftAI Inc.
2//
3// Licensed under the Apache License, Version 2.0 (the "License");
4// you may not use this file except in compliance with the License.
5// You may obtain a copy of the License at
6//
7// http://www.apache.org/licenses/LICENSE-2.0
8//
9// Unless required by applicable law or agreed to in writing, software
10// distributed under the License is distributed on an "AS IS" BASIS,
11// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12// See the License for the specific language governing permissions and
13// limitations under the License.
14
15/*****************************************************************************************************************
16 *
17 * Authors: Pablo Inigo Blasco, Brett Aldrich
18 *
19 ******************************************************************************************************************/
20
21#pragma once
22
23#include <optional>
25
26namespace smacc2
27{
28namespace client_bases
29{
31{
32public:
33 std::optional<std::string> topicName;
34 std::optional<int> queueSize;
35 std::optional<rmw_qos_durability_policy_t> durability;
36 std::optional<rmw_qos_reliability_policy_t> reliability;
37
39 virtual ~SmaccPublisherClient();
40
41 template <typename MessageType>
42 void configure(std::string topicName)
43 {
44 this->topicName = topicName;
45 if (!initialized_)
46 {
47 if (!this->topicName)
48 {
49 RCLCPP_ERROR(getLogger(), "topic publisher with no topic name set. Skipping advertising.");
50 return;
51 }
52
53 if (!queueSize) queueSize = 1;
54 if (!durability) durability = RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT;
55 if (!reliability) reliability = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
56 rclcpp::SensorDataQoS qos;
57 qos.keep_last(*queueSize);
58 qos.durability(*durability);
59 qos.reliability(*reliability);
60
61 RCLCPP_INFO_STREAM(
62 getLogger(), "[" << this->getName() << "] Client Publisher to topic: " << topicName);
63 pub_ = getNode()->create_publisher<MessageType>(*(this->topicName), qos);
64
65 this->initialized_ = true;
66 }
67 }
68
69 template <typename MessageType>
70 void publish(const MessageType & msg)
71 {
72 //pub_->publish(msg);
73 std::dynamic_pointer_cast<rclcpp::Publisher<MessageType>>(pub_)->publish(msg);
74 }
75
76 rclcpp::PublisherBase::SharedPtr pub_;
77
78private:
80};
81} // namespace client_bases
82} // namespace smacc2
rclcpp::Node::SharedPtr getNode()
Definition: client.cpp:60
virtual std::string getName() const
Definition: client.cpp:42
rclcpp::Logger getLogger()
std::optional< rmw_qos_reliability_policy_t > reliability
std::optional< rmw_qos_durability_policy_t > durability