Control Interfaces

#include <chrono>
#include <functional>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "xterra_msgs/msg/joy_data.hpp"

using namespace std::chrono_literals;

class JoyDataPublisherNode : public rclcpp::Node
{
public:
    JoyDataPublisherNode() : Node("high_level_command_publisher")
    {
        // Publisher targeting the mission input topic (Queue size 10)
        pub_ = this->create_publisher<xterra_msgs::msg::JoyData>(
            "/mission/joystick_data", 10);

        // Record the exact time the node started
        start_time_ = this->now();

        // Setup a timer to continuously send the drive command every 5ms (200Hz)
        timer_ = this->create_wall_timer(
            5ms, std::bind(&JoyDataPublisherNode::timer_callback, this));
    }

private:
    void timer_callback()
    {
        auto msg = xterra_msgs::msg::JoyData();
        msg.priority = 100;  // Set priority to override default idle state

        // Calculate how much time has passed since the node started
        auto elapsed_time = this->now() - start_time_;

        if (elapsed_time.seconds() < 1.0) {
            // PHASE 1: For the first 1.0 second, transition from SLEEP to FIXED STAND
            msg.buttons[1] = 1;
            RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 500,
                "State: FIXED STAND (Waiting...)");
        } else {
            // PHASE 2: After 1.0 second, transition to MOVE and drive forward
            msg.buttons[2] = 1;
            msg.axes[1] = -0.5f;  // Command forward drive (inverted logic)
            RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
                "State: MOVE | Publishing forward drive command");
        }

        // Publish the command continuously at 200Hz to satisfy watchdogs
        pub_->publish(msg);
    }

    rclcpp::Publisher<xterra_msgs::msg::JoyData>::SharedPtr pub_;
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Time start_time_;
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<JoyDataPublisherNode>());
    rclcpp::shutdown();
    return 0;
}

#include <iostream>
#include <memory>
// Include generated joystick data header
#include "JoyData.hpp"
// Include custom raw CycloneDDS publisher abstraction
#include "dds_publisher.hpp"

const uint32_t DOMAIN_ID = 0;

void sendMissionCommand() {
    auto pub = std::make_shared<DDSPublisher<xterra_msgs::msg::dds_::JoyData_>>(
        "rt/mission/joystick_data", DOMAIN_ID);

    xterra_msgs::msg::dds_::JoyData_ msg;

    // Set priority for this channel
    msg.priority() = 100;

    // Request transition to MOVE state (Button 2)
    msg.buttons()[2] = 1;

    // Command gradual forward drive (Axis 1, inverted logic)
    msg.axes()[1] = -0.5f;

    // NOTE: In production, call this inside a 200 Hz loop
    pub->publish(msg);
    std::cout << "Mission command published.\n";
}

τ = τ_ff + k_p × (q_ref − q_act) + k_d × (dq_ref − dq_act)

#include <chrono>
#include <functional>
#include "rclcpp/rclcpp.hpp"
#include "xterra/msg/joint_data.hpp"

using namespace std::chrono_literals;

class JointCommandNode : public rclcpp::Node
{
public:
    JointCommandNode() : Node("low_level_joint_publisher")
    {
        pub_ = this->create_publisher<xterra::msg::JointData>(
            "/m2_metal/hw/joint_command", 10);

        // Timer callback to publish commands at 200Hz (5ms)
        // High frequency is mandatory to prevent motor watchdog timeouts
        timer_ = this->create_wall_timer(
            5ms, std::bind(&JointCommandNode::publish_command, this));
    }

private:
    void publish_command()
    {
        auto cmd = xterra::msg::JointData();

        float target_pos[12] = {
             0.0001f,  1.306f, -2.738f,
            -0.0001f,  1.306f, -2.738f,
             0.0001f,  1.306f, -2.738f,
            -0.0001f,  1.306f, -2.738f
        };

        // Command all joints to hold the sleep pose
        for (int i = 0; i < 12; i++) {
            cmd.q[i]   = target_pos[i];  // Position target (rad)
            cmd.dq[i]  = 0.0f;           // Velocity target (rad/s)
            cmd.kp[i]  = 50.0f;          // P gain (Stiffness)
            cmd.kd[i]  = 1.0f;           // D gain (Damping)
            cmd.tau[i] = 0.0f;           // Torque feed-forward (N·m)
        }

        pub_->publish(cmd);
    }

    rclcpp::Publisher<xterra::msg::JointData>::SharedPtr pub_;
    rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<JointCommandNode>());
    rclcpp::shutdown();
    return 0;
}

#include <iostream>
#include <thread>
#include <chrono>
// Include header for generated joint data
#include "JointData.hpp"
// Include custom header for Raw CycloneDDS publisher abstraction
#include "dds_publisher.hpp"

// Define the DDS Domain ID
const uint32_t DOMAIN_ID = 0;

int main() {
    // Create the DDS publisher targeting the hardware layer
    auto pub = std::make_shared<xterra::DDSPublisher<xterra::msg::dds_::JointData_>>(
        "rt/m2_metal/hw/joint_command", DOMAIN_ID);

    std::cout << "Starting native DDS joint command stream at 200Hz..."
              << std::endl;

    // Instantiate the command message
    xterra::msg::dds_::JointData_ cmd;

    float target_pos[12] = {
         0.0001f,  1.306f, -2.738f,
        -0.0001f,  1.306f, -2.738f,
         0.0001f,  1.306f, -2.738f,
        -0.0001f,  1.306f, -2.738f
    };

    // 200Hz Control Loop (5ms period)
    const auto loop_period = std::chrono::milliseconds(5);

    while (true) {
        auto start_time = std::chrono::steady_clock::now();

        // Populate parameters for all 12 joints
        for (int i = 0; i < 12; i++) {
            cmd.q()[i]   = target_pos[i];  // Target position  (rad)
            cmd.dq()[i]  = 0.0f;           // Target velocity  (rad/s)
            cmd.kp()[i]  = 50.0f;          // Stiffness gain   (N·m/rad)
            cmd.kd()[i]  = 1.0f;           // Damping gain     (N·m·s/rad)
            cmd.tau()[i] = 0.0f;           // Feed-forward torque (N·m)
        }

        // Transmit synchronized arrays to the motor drivers
        pub->publish(cmd);

        // Enforce loop timing to satisfy hardware watchdog
        std::this_thread::sleep_until(start_time + loop_period);
    }
    return 0;
}