robot_driver.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__ROBOT_DRIVER_HPP_
#define STAUBLI_ROBOT_DRIVER__ROBOT_DRIVER_HPP_
 
// C++
#include <atomic>
#include <chrono>
#include <condition_variable>
#include <memory>
#include <mutex>
#include <string>
#include <vector>
 
// Staubli Robot Driver
#include "staubli_robot_driver/communication/messages.hpp"
#include "staubli_robot_driver/real_time_socket_interface.hpp"
 
namespace staubli_robot_driver {
 
class RobotDriver {
public:
    struct NetworkConfig {
        std::string robot_ip = "";
        std::string local_ip = "";
        uint16_t control_port = DEFAULT_CONTROL_PORT;
        uint16_t local_control_port = DEFAULT_CONTROL_PORT;
        uint16_t diagnostics_port = DEFAULT_DIAGNOSTICS_PORT;
        uint16_t local_diagnostics_port = DEFAULT_DIAGNOSTICS_PORT;
    };
 
public:
    RobotDriver();
 
    ~RobotDriver();
 
    bool connect(const std::string& robot_ip, int timeout_ms);
 
    bool connect(const NetworkConfig& config, int timeout_ms = 5000);
 
    bool disconnect();
 
    bool is_connected() const;
 
    bool send_stop_all_command();
 
    bool send_robot_command(const RobotCommandMessage& command);
 
    bool read_robot_state(RobotStateMessage& state);
 
    bool read_diagnostic_data(DiagnosticDataMessage& data);
 
    void set_state_staleness_timeout(
        rclcpp::Duration staleness_timeout = rclcpp::Duration(0, 1e8));
 
    void set_diagnostic_staleness_timeout(
        rclcpp::Duration staleness_timeout = rclcpp::Duration(0, 1e8));
 
    size_t get_current_message_sequence() const {
        return last_valid_sequence_id_;
    }
 
protected:
    // Helper methods
    void reset_data();
    bool is_state_msg_valid() const;
    bool is_diagnostic_msg_valid() const;
 
private:
    // Logger
    rclcpp::Logger logger_;
 
    // Sockets
    NetworkConfig network_config_;
    std::shared_ptr<Socket> control_socket_;
    std::shared_ptr<Socket> diagnostics_socket_;
 
    // Communication interfaces
    std::shared_ptr<
        RealTimeSocketInterface<RobotStateMessage, RobotCommandMessage>> control_interface_;
    std::shared_ptr<
        RealTimeSocketSubscriber<DiagnosticDataMessage>> diagnostics_subscriber_;
 
    // State tracking
    size_t last_valid_sequence_id_ = 0;
    RobotStateMessage current_robot_state_;
    DiagnosticDataMessage current_diagnostic_data_;
 
    // Staleness timeouts
    rclcpp::Duration state_staleness_timeout_;
    rclcpp::Duration diagnostic_staleness_timeout_;
};
 
}  // namespace staubli_robot_driver
 
#endif  // STAUBLI_ROBOT_DRIVER__ROBOT_DRIVER_HPP_