Skip to content

Commit

Permalink
Minorly simplified the constructor with this trick
Browse files Browse the repository at this point in the history
  • Loading branch information
shuhaowu committed Aug 28, 2024
1 parent a23ffef commit 0969f79
Show file tree
Hide file tree
Showing 2 changed files with 11 additions and 17 deletions.
17 changes: 7 additions & 10 deletions include/cactus_rt/ros2/publisher.h
Original file line number Diff line number Diff line change
Expand Up @@ -83,25 +83,22 @@ class Publisher : public IPublisher {
const rclcpp::QoS& qos,
const size_t rt_queue_size = 1000
) {
return std::make_shared<Publisher<RealtimeT, RosT, CheckForTrivialRealtimeT>>(
logger,
node.create_publisher<AdaptedRosType>(topic_name, qos),
moodycamel::ReaderWriterQueue<RealtimeT>(rt_queue_size)
return std::shared_ptr<Publisher<RealtimeT, RosT, CheckForTrivialRealtimeT>>(
new Publisher<RealtimeT, RosT, CheckForTrivialRealtimeT>(
logger,
node.create_publisher<AdaptedRosType>(topic_name, qos),
moodycamel::ReaderWriterQueue<RealtimeT>(rt_queue_size)
)
);
}

public:
/**
* Constructs a publisher. Shouldn't be called directly. Only public to enable make_shared.
*
* @private
*/
Publisher(
quill::Logger* logger,
typename rclcpp::Publisher<AdaptedRosType>::SharedPtr publisher,
moodycamel::ReaderWriterQueue<RealtimeT>&& queue
) : logger_(logger), publisher_(publisher), queue_(std::move(queue)) {}

public:
template <typename... Args>
bool Publish(Args&&... args) noexcept {
const bool success = queue_.try_emplace(std::forward<Args>(args)...);
Expand Down
11 changes: 4 additions & 7 deletions include/cactus_rt/ros2/subscription.h
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,9 @@ class SubscriptionLatest : public ISubscription {
const std::string& topic_name,
const rclcpp::QoS& qos
) {
auto subscription = std::make_shared<SubscriptionLatest<RealtimeT, RosT, CheckForTrivialRealtimeT>>(logger);
std::shared_ptr<SubscriptionLatest<RealtimeT, RosT, CheckForTrivialRealtimeT>> subscription(
new SubscriptionLatest<RealtimeT, RosT, CheckForTrivialRealtimeT>(logger)
);

subscription->ros_subscription_ = node.create_subscription<AdaptedRosType>(
topic_name,
Expand All @@ -76,14 +78,9 @@ class SubscriptionLatest : public ISubscription {
return subscription;
}

public:
/**
* @brief Do not use this method. Defined to allow make_shared to work.
*
* @private
*/
explicit SubscriptionLatest(quill::Logger* logger) : logger_(logger) {}

public:
StampedValue<RealtimeT> ReadLatest() noexcept {
return latest_value_.Read();
}
Expand Down

0 comments on commit 0969f79

Please sign in to comment.