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);
    }
}

#include <urdf/model.h>
#include <rclcpp/rclcpp.hpp>

// Load URDF from the robot_description parameter
auto node = std::make_shared<rclcpp::Node>("urdf_reader");
std::string urdf_str;
node->declare_parameter("robot_description", "");
node->get_parameter("robot_description", urdf_str);

urdf::Model model;
if (!model.initString(urdf_str)) {
    RCLCPP_ERROR(node->get_logger(), "Failed to parse URDF");
    return 1;
}

RCLCPP_INFO(node->get_logger(), "Robot name: %s", model.getName().c_str());

// Iterate joints
for (auto& [name, joint] : model.joints_) {
    RCLCPP_INFO(node->get_logger(),
        "Joint %s: parent=%s child=%s type=%d",
        name.c_str(),
        joint->parent_link_name.c_str(),
        joint->child_link_name.c_str(),
        joint->type);
}

// Access link geometry
auto base_link = model.getLink("base_link");
if (base_link && base_link->visual) {
    // visual->geometry is a sphere, box, cylinder, or mesh
    auto& origin = base_link->visual->origin;
    RCLCPP_INFO(node->get_logger(),
        "base_link origin: xyz=[%.2f %.2f %.2f]",
        origin.position.x, origin.position.y, origin.position.z);
}

# Launch robot_state_publisher with URDF
ros2 run robot_state_publisher robot_state_publisher \
    --ros-args -p robot_description:="$(cat my_robot.urdf)"

#include <kdl_parser/kdl_parser.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainiksolverpos_lma.hpp>
#include <kdl/chainiksolvervel_pinv.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <urdf/model.h>

urdf::Model urdf_model;
urdf_model.initFile("my_robot.urdf");

KDL::Tree kdl_tree;
kdl_parser::treeFromUrdfModel(urdf_model, kdl_tree);

// Extract chain from base to end-effector
KDL::Chain chain;
kdl_tree.getChain("base_link", "tool_link", chain);

int n = chain.getNrOfJoints();  // e.g. 6 for a 6-DOF arm

// Forward kinematics
KDL::ChainFkSolverPos_Recursive fk_solver(chain);
KDL::JntArray q(n);
q(0) = 0.0; q(1) = -M_PI/4; q(2) = M_PI/2;
// ... fill remaining joints

KDL::Frame ee_frame;
fk_solver.JntToCart(q, ee_frame);
std::cout << "EE position: "
    << ee_frame.p.x() << " "
    << ee_frame.p.y() << " "
    << ee_frame.p.z() << "\n";

// Jacobian
KDL::ChainJntToJacSolver jac_solver(chain);
KDL::Jacobian J(n);
jac_solver.JntToJac(q, J);

// Inverse kinematics (Levenberg-Marquardt)
KDL::ChainIkSolverPos_LMA ik_solver(chain);
KDL::JntArray q_init(n), q_sol(n);
KDL::Frame target = ee_frame;
target.p.x(target.p.x() + 0.05);  // move 5 cm in x

int rc = ik_solver.CartToJnt(q_init, target, q_sol);
if (rc >= 0) {
    for (int i = 0; i < n; i++)
        std::cout << "q[" << i << "] = " << q_sol(i) << "\n";
}

#include <Eigen/Dense>

// State: [x, y, yaw, vx, vy, omega]  (6-DOF 2-D)
struct EKF {
    Eigen::VectorXd x;   // state estimate
    Eigen::MatrixXd P;   // covariance
    Eigen::MatrixXd Q;   // process noise
    Eigen::MatrixXd R_odom;  // odometry observation noise
    Eigen::MatrixXd R_imu;   // IMU observation noise

    static constexpr int N = 6;

    EKF() {
        x = Eigen::VectorXd::Zero(N);
        P = Eigen::MatrixXd::Identity(N, N) * 0.1;

        Q = Eigen::MatrixXd::Zero(N, N);
        Q.diagonal() << 0.01, 0.01, 0.001, 0.1, 0.1, 0.01;

        R_odom = Eigen::MatrixXd::Identity(3, 3) * 0.05;
        R_imu  = Eigen::MatrixXd::Identity(1, 1) * 0.001;
    }

    // Predict step: integrate motion model
    void predict(double dt) {
        double yaw = x(2), vx = x(3), vy = x(4), omega = x(5);
        // Nonlinear state transition f(x)
        Eigen::VectorXd x_pred = x;
        x_pred(0) += (vx * std::cos(yaw) - vy * std::sin(yaw)) * dt;
        x_pred(1) += (vx * std::sin(yaw) + vy * std::cos(yaw)) * dt;
        x_pred(2) += omega * dt;

        // Jacobian of f w.r.t. x (linearisation)
        Eigen::MatrixXd F = Eigen::MatrixXd::Identity(N, N);
        F(0, 2) = (-vx * std::sin(yaw) - vy * std::cos(yaw)) * dt;
        F(0, 3) =  std::cos(yaw) * dt;
        F(1, 2) = ( vx * std::cos(yaw) - vy * std::sin(yaw)) * dt;
        F(1, 3) =  std::sin(yaw) * dt;
        F(2, 5) = dt;

        x = x_pred;
        P = F * P * F.transpose() + Q;
    }

    // Update step: incorporate odometry [x, y, yaw]
    void update_odom(const Eigen::Vector3d& z) {
        Eigen::MatrixXd H = Eigen::MatrixXd::Zero(3, N);
        H(0, 0) = H(1, 1) = H(2, 2) = 1.0;

        Eigen::Vector3d y_innov = z - H * x;
        // Wrap yaw innovation to [-pi, pi]
        while (y_innov(2) >  M_PI) y_innov(2) -= 2*M_PI;
        while (y_innov(2) < -M_PI) y_innov(2) += 2*M_PI;

        Eigen::MatrixXd S = H * P * H.transpose() + R_odom;
        Eigen::MatrixXd K = P * H.transpose() * S.inverse();
        x += K * y_innov;
        P  = (Eigen::MatrixXd::Identity(N, N) - K * H) * P;
    }

    // Update step: incorporate IMU yaw rate
    void update_imu_yaw_rate(double omega_meas) {
        Eigen::MatrixXd H = Eigen::MatrixXd::Zero(1, N);
        H(0, 5) = 1.0;
        double innov = omega_meas - x(5);
        double S = (H * P * H.transpose())(0,0) + R_imu(0,0);
        Eigen::VectorXd K = P * H.transpose() / S;
        x += K * innov;
        P  = (Eigen::MatrixXd::Identity(N, N) - K * H) * P;
    }
};

# Install RT kernel (Ubuntu)
sudo apt-get install linux-image-rt-amd64   # Debian-based
uname -r   # should show -rt suffix

# Run cyclictest: 10 000 iterations, 1 ms interval, SCHED_FIFO priority 80
sudo cyclictest --mlockall --smp --priority=80 --interval=1000 \
    --distance=0 --loops=10000 --histfile=latency.hist

# Interpret output
# Min: ~5 µs  Avg: ~10 µs  Max: <100 µs is good for soft RT
# Max < 50 µs is achievable on RT kernels for hard RT targets

// Measure your own control-loop jitter
#include <time.h>
#include <cstdio>

struct LatencyStats {
    long min_ns = LONG_MAX, max_ns = 0, sum_ns = 0;
    int  count = 0;

    void record(long ns) {
        min_ns = std::min(min_ns, ns);
        max_ns = std::max(max_ns, ns);
        sum_ns += ns;
        count++;
    }
    void print() const {
        std::printf("Latency — min: %ld µs  avg: %ld µs  max: %ld µs\n",
            min_ns/1000, sum_ns/count/1000, max_ns/1000);
    }
};

void measure_loop_jitter(int iterations) {
    LatencyStats stats;
    struct timespec expected, actual;
    clock_gettime(CLOCK_MONOTONIC, &expected);

    for (int i = 0; i < iterations; i++) {
        expected.tv_nsec += 1'000'000;  // 1 ms
        if (expected.tv_nsec >= 1'000'000'000) {
            expected.tv_sec++;
            expected.tv_nsec -= 1'000'000'000;
        }
        clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &expected, nullptr);

        clock_gettime(CLOCK_MONOTONIC, &actual);
        long jitter = (actual.tv_sec  - expected.tv_sec ) * 1'000'000'000L
                    + (actual.tv_nsec - expected.tv_nsec);
        stats.record(std::abs(jitter));
    }
    stats.print();
}

<!-- robot.urdf.xacro — attach ros2_control plugin to Gazebo -->
<ros2_control name="GazeboSystem" type="system">
  <hardware>
    <plugin>gz_ros2_control/GazeboSimSystem</plugin>
  </hardware>
  <joint name="joint1">
    <command_interface name="position"/>
    <state_interface name="position"/>
    <state_interface name="velocity"/>
  </joint>
  <joint name="joint2">
    <command_interface name="position"/>
    <state_interface name="position"/>
    <state_interface name="velocity"/>
  </joint>
</ros2_control>

<gazebo>
  <plugin filename="gz_ros2_control-system"
          name="gz_ros2_control::GazeboSimROS2ControlPlugin">
    <parameters>$(find my_robot_description)/config/controllers.yaml</parameters>
  </plugin>
</gazebo>

# controllers.yaml
controller_manager:
  ros__parameters:
    update_rate: 1000  # Hz

joint_trajectory_controller:
  ros__parameters:
    type: joint_trajectory_controller/JointTrajectoryController
    joints:
      - joint1
      - joint2
    command_interfaces: [position]
    state_interfaces:   [position, velocity]

#include <hardware_interface/system_interface.hpp>
#include <hardware_interface/types/hardware_interface_type_values.hpp>

class MyActuatorHW : public hardware_interface::SystemInterface {
public:
    hardware_interface::CallbackReturn on_init(
        const hardware_interface::HardwareInfo& info) override
    {
        if (SystemInterface::on_init(info) !=
            hardware_interface::CallbackReturn::SUCCESS)
            return hardware_interface::CallbackReturn::ERROR;

        n_joints_ = info_.joints.size();
        pos_state_.resize(n_joints_, 0.0);
        vel_state_.resize(n_joints_, 0.0);
        pos_cmd_.resize(n_joints_, 0.0);
        return hardware_interface::CallbackReturn::SUCCESS;
    }

    // Export state interfaces (what controllers can read)
    std::vector<hardware_interface::StateInterface>
    export_state_interfaces() override {
        std::vector<hardware_interface::StateInterface> si;
        for (std::size_t i = 0; i < n_joints_; i++) {
            si.emplace_back(info_.joints[i].name,
                hardware_interface::HW_IF_POSITION, &pos_state_[i]);
            si.emplace_back(info_.joints[i].name,
                hardware_interface::HW_IF_VELOCITY, &vel_state_[i]);
        }
        return si;
    }

    // Export command interfaces (what controllers can write)
    std::vector<hardware_interface::CommandInterface>
    export_command_interfaces() override {
        std::vector<hardware_interface::CommandInterface> ci;
        for (std::size_t i = 0; i < n_joints_; i++) {
            ci.emplace_back(info_.joints[i].name,
                hardware_interface::HW_IF_POSITION, &pos_cmd_[i]);
        }
        return ci;
    }

    hardware_interface::return_type read(
        const rclcpp::Time&, const rclcpp::Duration&) override
    {
        // Read from real hardware (CAN, serial, EtherCAT...)
        for (std::size_t i = 0; i < n_joints_; i++) {
            pos_state_[i] = hardware_read_position(i);
            vel_state_[i] = hardware_read_velocity(i);
        }
        return hardware_interface::return_type::OK;
    }

    hardware_interface::return_type write(
        const rclcpp::Time&, const rclcpp::Duration&) override
    {
        for (std::size_t i = 0; i < n_joints_; i++) {
            hardware_send_position_command(i, pos_cmd_[i]);
        }
        return hardware_interface::return_type::OK;
    }

private:
    std::size_t n_joints_ = 0;
    std::vector<double> pos_state_, vel_state_, pos_cmd_;

    double hardware_read_position(std::size_t /*joint*/) { return 0.0; }
    double hardware_read_velocity(std::size_t /*joint*/) { return 0.0; }
    void   hardware_send_position_command(std::size_t, double) {}
};

#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(MyActuatorHW, hardware_interface::SystemInterface)