real_time_socket_subscriber.hpp

Go to the documentation of this file.

// Copyright 2025 ICUBE Laboratory, University of Strasbourg
//
// 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.
//
// Author: Thibault Poignonec (thibault.poignonec@gmail.fr)
 
#ifndef STAUBLI_ROBOT_DRIVER__REAL_TIME_SOCKET_SUBSCRIBER_HPP_
#define STAUBLI_ROBOT_DRIVER__REAL_TIME_SOCKET_SUBSCRIBER_HPP_
 
// C++
#include <atomic>
#include <cstdint>
#include <chrono>
#include <functional>
#include <memory>
#include <string>
#include <vector>
 
// ROS2
#include <rclcpp/duration.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/time.hpp>
 
// Realtime tools (ROS2 package)
#include <realtime_tools/realtime_thread_safe_box.hpp>
 
// Staubli Robot Driver
#include "staubli_robot_driver/communication/messages.hpp"
#include "staubli_robot_driver/communication/protocol.hpp"
#include "staubli_robot_driver/communication/socket.hpp"
 
namespace staubli_robot_driver {
 
struct MessageStatus {
    bool new_message;
    rclcpp::Duration time_since_received{1, 0};
    size_t lost_packages;
};
 
template <typename MessageType>
class RealTimeSocketSubscriber {
public:
    explicit RealTimeSocketSubscriber(std::shared_ptr<Socket> socket);
    virtual ~RealTimeSocketSubscriber();
 
    // Read the latest message and check for lost packages and staleness
 
    bool read_message(MessageType& msg, MessageStatus& status);
 
    virtual bool is_ready() const;
 
protected:
    // Communication initialization
    bool init_communication();
 
    // Update the message buffer with a new message
    void update_message(std::vector<uint8_t>& data, size_t bytes_transferred);
 
    // Error handling
    void handle_error(const std::string& error_msg);
 
protected:
    std::shared_ptr<Socket> socket_;
    size_t last_read_sequence_number_;
 
    rclcpp::Logger logger_;
 
private:
    realtime_tools::RealtimeThreadSafeBox<MessageType> message_buffer_;
    bool first_read_;  // Flag to indicate if it's the first read
};
 
// Declarations for message types
template class RealTimeSocketSubscriber<DiagnosticDataMessage>;
template class RealTimeSocketSubscriber<RobotCommandMessage>;
template class RealTimeSocketSubscriber<RobotStateMessage>;
 
}  // namespace staubli_robot_driver
 
#endif  // STAUBLI_ROBOT_DRIVER__REAL_TIME_SOCKET_SUBSCRIBER_HPP_