C++ for Robotics

#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>

class AvoidanceNode : public rclcpp::Node {
public:
    AvoidanceNode() : Node("obstacle_avoidance") {
        // Subscribe to laser scan
        scan_sub_ = create_subscription<sensor_msgs::msg::LaserScan>(
            "/scan", 10,
            std::bind(&AvoidanceNode::on_scan, this, std::placeholders::_1));

        // Publish velocity commands
        cmd_vel_pub_ = create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);

        // Timer: send commands at 10 Hz
        timer_ = create_wall_timer(
            std::chrono::milliseconds(100),
            std::bind(&AvoidanceNode::control_loop, this));

        RCLCPP_INFO(get_logger(), "AvoidanceNode started");
    }

private:
    void on_scan(const sensor_msgs::msg::LaserScan::SharedPtr msg) {
        // Find minimum distance in front
        int front_start = msg->ranges.size() / 2 - 30;
        int front_end   = msg->ranges.size() / 2 + 30;
        min_front_dist_ = *std::min_element(
            msg->ranges.begin() + front_start,
            msg->ranges.begin() + front_end);
    }

    void control_loop() {
        geometry_msgs::msg::Twist cmd;
        if (min_front_dist_ > 0.5f) {
            cmd.linear.x = 0.3;   // move forward
        } else {
            cmd.angular.z = 0.5;  // turn
        }
        cmd_vel_pub_->publish(cmd);
    }

    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
    rclcpp::TimerBase::SharedPtr timer_;
    float min_front_dist_ = 1.0f;
};

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

#include <rclcpp_lifecycle/lifecycle_node.hpp>

class SensorDriver : public rclcpp_lifecycle::LifecycleNode {
public:
    SensorDriver() : LifecycleNode("sensor_driver") {}

    // Called on configure transition
    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
    on_configure(const rclcpp_lifecycle::State&) override {
        pub_ = create_publisher<sensor_msgs::msg::Imu>("/imu", 10);
        RCLCPP_INFO(get_logger(), "Configured");
        return CallbackReturn::SUCCESS;
    }

    CallbackReturn on_activate(const rclcpp_lifecycle::State&) override {
        pub_->on_activate();
        timer_ = create_wall_timer(10ms, [this]{ publish_imu(); });
        return CallbackReturn::SUCCESS;
    }

    CallbackReturn on_deactivate(const rclcpp_lifecycle::State&) override {
        timer_.reset();
        pub_->on_deactivate();
        return CallbackReturn::SUCCESS;
    }

    CallbackReturn on_cleanup(const rclcpp_lifecycle::State&) override {
        pub_.reset();
        return CallbackReturn::SUCCESS;
    }

private:
    void publish_imu() { /* read sensor, publish */ }
    rclcpp_lifecycle::LifecyclePublisher<sensor_msgs::msg::Imu>::SharedPtr pub_;
    rclcpp::TimerBase::SharedPtr timer_;
};

#include <Eigen/Dense>
#include <Eigen/Geometry>

// 3D rotation representations
Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/4, Eigen::Vector3d::UnitZ()).toRotationMatrix();
Eigen::Quaterniond q{R};               // quaternion — no gimbal lock
Eigen::Vector3d rpy = R.eulerAngles(0, 1, 2);  // roll-pitch-yaw

// Homogeneous transform (4×4)
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.translation() = Eigen::Vector3d{1.0, 2.0, 0.5};
T.linear() = R;

// Chain transforms
Eigen::Isometry3d T_base_to_ee = T_base_to_link1 * T_link1_to_link2 * T_link2_to_ee;

// Transform a point
Eigen::Vector3d p_base = T_base_to_ee * p_ee;

// Jacobian for velocity kinematics
// vel_ee = J * dq (joint velocities → end-effector velocity)
Eigen::MatrixXd J(6, num_joints);  // 6 (3 linear + 3 angular) × n joints
// ... fill J from DH parameters

// Pseudo-inverse for inverse kinematics
Eigen::VectorXd dq = J.completeOrthogonalDecomposition().pseudoInverse() * vel_desired;

class PIDController {
    double kp_, ki_, kd_;
    double integral_ = 0.0;
    double prev_error_ = 0.0;
    double integral_limit_;

public:
    PIDController(double kp, double ki, double kd, double ilim = 1e6)
        : kp_(kp), ki_(ki), kd_(kd), integral_limit_(ilim) {}

    double compute(double setpoint, double measured, double dt) {
        double error = setpoint - measured;

        integral_ = std::clamp(integral_ + error * dt,
                               -integral_limit_, integral_limit_);

        double derivative = (error - prev_error_) / dt;
        prev_error_ = error;

        return kp_ * error + ki_ * integral_ + kd_ * derivative;
    }

    void reset() { integral_ = 0.0; prev_error_ = 0.0; }
};

// Usage in real-time loop
PIDController joint_pid{10.0, 0.1, 0.5};
double torque = joint_pid.compute(setpoint_rad, current_rad, dt_s);

// Simple complementary filter for roll/pitch from accelerometer + gyroscope
class ComplementaryFilter {
    double alpha_;  // weight for gyroscope (0.98 typical)
    double roll_ = 0.0, pitch_ = 0.0;

public:
    explicit ComplementaryFilter(double alpha = 0.98) : alpha_(alpha) {}

    std::pair<double,double> update(
        double gx, double gy, double gz,   // gyro (rad/s)
        double ax, double ay, double az,   // accel (m/s^2)
        double dt)
    {
        // Gyroscope integration (fast, drifts)
        double roll_gyro  = roll_  + gx * dt;
        double pitch_gyro = pitch_ + gy * dt;

        // Accelerometer angle (noisy, no drift)
        double roll_accel  = std::atan2(ay, az);
        double pitch_accel = std::atan2(-ax, std::sqrt(ay*ay + az*az));

        // Fuse
        roll_  = alpha_ * roll_gyro  + (1 - alpha_) * roll_accel;
        pitch_ = alpha_ * pitch_gyro + (1 - alpha_) * pitch_accel;

        return {roll_, pitch_};
    }
};

#include <linux/can.h>
#include <linux/can/raw.h>
#include <sys/socket.h>
#include <net/if.h>

// Open CAN socket
int sockfd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
struct ifreq ifr;
strcpy(ifr.ifr_name, "can0");
ioctl(sockfd, SIOCGIFINDEX, &ifr);

struct sockaddr_can addr{};
addr.can_family  = AF_CAN;
addr.can_ifindex = ifr.ifr_ifindex;
bind(sockfd, (sockaddr*)&addr, sizeof(addr));

// Send velocity command to motor (example encoding)
struct can_frame frame{};
frame.can_id  = 0x141;  // motor node ID
frame.can_dlc = 4;      // data length
float velocity = 1000.0f;  // RPM
std::memcpy(frame.data, &velocity, sizeof(float));
write(sockfd, &frame, sizeof(frame));

// Read encoder feedback
struct can_frame response{};
read(sockfd, &response, sizeof(response));

#include <sched.h>
#include <sys/mman.h>

void setup_realtime() {
    // Lock all memory — no page faults during control loop
    mlockall(MCL_CURRENT | MCL_FUTURE);

    // Set real-time FIFO scheduling (requires root or CAP_SYS_NICE)
    struct sched_param param{};
    param.sched_priority = 80;
    sched_setscheduler(0, SCHED_FIFO, &param);

    // Pre-fault stack pages
    volatile char stack[65536];
    for (int i = 0; i < sizeof(stack); i += 4096) stack[i] = 0;
}

void realtime_loop() {
    setup_realtime();

    // Fixed-rate loop using clock_nanosleep
    struct timespec next_time;
    clock_gettime(CLOCK_MONOTONIC, &next_time);

    while (running) {
        // Advance deadline
        next_time.tv_nsec += 1'000'000;  // 1ms period
        while (next_time.tv_nsec >= 1'000'000'000) {
            next_time.tv_sec++;
            next_time.tv_nsec -= 1'000'000'000;
        }

        control_step();

        clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_time, nullptr);
    }
}