Yürüyen kutu

// include/pins.h — iki motor. I2C (21/22) IMU için boş bırakıldı.
#pragma once
// TB6612 A kanalı: sol motor
constexpr int PIN_PWMA = 25, PIN_AIN1 = 26, PIN_AIN2 = 27;
// TB6612 B kanalı: sağ motor (GPIO12 bir strapping pini, kullanma)
constexpr int PIN_PWMB = 32, PIN_BIN1 = 4, PIN_BIN2 = 16;
constexpr int PIN_STBY = 33;
// Encoder'lar
constexpr int PIN_ENC_L_A = 18, PIN_ENC_L_B = 19;
constexpr int PIN_ENC_R_A = 13, PIN_ENC_R_B = 14;
// I2C: IMU (4. hafta)
constexpr int PIN_SDA = 21, PIN_SCL = 22;

<?xml version="1.0"?>
<!-- urdf/rover.urdf.xacro — ölçüler mekanik sayfasındaki şasiyle aynı olsun -->
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="rover">
  <xacro:property name="wheel_r" value="0.0325"/>
  <xacro:property name="wheel_w" value="0.026"/>
  <xacro:property name="track" value="0.175"/>
  <xacro:property name="plate_l" value="0.17"/>
  <xacro:property name="plate_w" value="0.137"/>
  <xacro:property name="plate_z" value="0.045"/>

  <link name="base_footprint"/>
  <link name="base_link">
    <visual>
      <origin xyz="0 0 ${plate_z - wheel_r}"/>
      <geometry><box size="${plate_l} ${plate_w} 0.004"/></geometry>
      <material name="sari"><color rgba="0.96 0.77 0.26 1"/></material>
    </visual>
    <collision>
      <origin xyz="0 0 ${plate_z - wheel_r + 0.03}"/>
      <geometry><box size="${plate_l} ${plate_w} 0.07"/></geometry>
    </collision>
    <inertial>
      <mass value="0.75"/>
      <inertia ixx="0.002" iyy="0.003" izz="0.004" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>
  <!-- base_link teker aksının ortasında: diferansiyel sürüşte dönme merkezi orası -->
  <joint name="base_joint" type="fixed">
    <parent link="base_footprint"/><child link="base_link"/>
    <origin xyz="0 0 ${wheel_r}"/>
  </joint>

  <xacro:macro name="wheel" params="side y">
    <link name="${side}_wheel">
      <visual>
        <origin rpy="${pi/2} 0 0"/>
        <geometry><cylinder radius="${wheel_r}" length="${wheel_w}"/></geometry>
        <material name="koyu"><color rgba="0.15 0.15 0.15 1"/></material>
      </visual>
      <collision>
        <origin rpy="${pi/2} 0 0"/>
        <geometry><cylinder radius="${wheel_r}" length="${wheel_w}"/></geometry>
      </collision>
      <inertial>
        <mass value="0.03"/>
        <inertia ixx="1e-5" iyy="1.6e-5" izz="1e-5" ixy="0" ixz="0" iyz="0"/>
      </inertial>
    </link>
    <joint name="${side}_wheel_joint" type="continuous">
      <parent link="base_link"/><child link="${side}_wheel"/>
      <origin xyz="0 ${y} 0"/>
      <axis xyz="0 1 0"/>
    </joint>
  </xacro:macro>
  <xacro:wheel side="left" y="${track/2}"/>
  <xacro:wheel side="right" y="${-track/2}"/>

  <link name="caster">
    <visual><geometry><sphere radius="0.01"/></geometry></visual>
    <collision><geometry><sphere radius="0.01"/></geometry></collision>
    <inertial><mass value="0.01"/><inertia ixx="1e-7" iyy="1e-7" izz="1e-7" ixy="0" ixz="0" iyz="0"/></inertial>
  </link>
  <joint name="caster_joint" type="fixed">
    <parent link="base_link"/><child link="caster"/>
    <origin xyz="${-plate_l/2 + 0.018} 0 ${-wheel_r + 0.01}"/>
  </joint>

  <link name="imu_link"/>
  <joint name="imu_joint" type="fixed">
    <parent link="base_link"/><child link="imu_link"/>
    <origin xyz="0.035 0 ${plate_z - wheel_r + 0.01}"/>
  </joint>
</robot>

// src/wheel_odometry.cpp — tekerlek sayımlarından /odom ve odom→base_footprint TF
#include <cmath>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/int32_multi_array.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>

class WheelOdometry : public rclcpp::Node {
 public:
  WheelOdometry() : Node("wheel_odometry") {
    r_ = declare_parameter("wheel_radius", 0.0325);
    r_left_scale_ = declare_parameter("left_radius_scale", 1.0);    // UMBmark'tan
    r_right_scale_ = declare_parameter("right_radius_scale", 1.0);
    track_ = declare_parameter("wheel_separation", 0.175);
    cpr_ = declare_parameter("ticks_per_rev", 1400.0);
    publish_tf_ = declare_parameter("publish_tf", true);  // EKF kullanınca false
    odom_frame_ = declare_parameter("odom_frame", std::string("odom"));
    base_frame_ = declare_parameter("base_frame", std::string("base_footprint"));

    pub_ = create_publisher<nav_msgs::msg::Odometry>("odom", 10);
    tf_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
    sub_ = create_subscription<std_msgs::msg::Int32MultiArray>(
        "wheel_ticks", rclcpp::SensorDataQoS(),
        [this](const std_msgs::msg::Int32MultiArray& m) { onTicks(m); });
  }

 private:
  void onTicks(const std_msgs::msg::Int32MultiArray& m) {
    if (m.data.size() < 2) return;
    const rclcpp::Time stamp= now();
    const int32_t l= m.data[0], r= m.data[1];
    if (!init_) {
      lastL_= l; lastR_= r; last_= stamp; init_= true;
      return;
    }
    // int32 taşmasına dayanıklı fark
    const double dl= static_cast<int32_t>(l - lastL_) / cpr_ * 2 * M_PI * r_ * r_left_scale_;
    const double dr = static_cast<int32_t>(r - lastR_) / cpr_ * 2 * M_PI * r_ * r_right_scale_;
    lastL_ = l;
    lastR_ = r;
    const double dt = (stamp - last_).seconds();
    last_ = stamp;
    if (dt <= 0) return;

    const double ds= (dr + dl) / 2;
    const double dth= (dr - dl) / track_;
    // orta nokta integrasyonu: Euler'den daha az sistematik hata
    x_ = ds * std::cos(th_ + dth / 2);
    y_ = ds * std::sin(th_ + dth / 2);
    th_= std::atan2(std::sin(th_ + dth), std::cos(th_ + dth));

    tf2::Quaternion q;
    q.setRPY(0, 0, th_);
    nav_msgs::msg::Odometry o;
    o.header.stamp= stamp;
    o.header.frame_id= odom_frame_;
    o.child_frame_id= base_frame_;
    o.pose.pose.position.x= x_;
    o.pose.pose.position.y= y_;
    o.pose.pose.orientation.x= q.x();
    o.pose.pose.orientation.y= q.y();
    o.pose.pose.orientation.z= q.z();
    o.pose.pose.orientation.w= q.w();
    o.twist.twist.linear.x= ds / dt;
    o.twist.twist.angular.z= dth / dt;
    // kovaryans: EKF bunlara güvenir. Düz robotta z, roll, pitch'i "bilmiyorum" diye büyük ver.
    const double big= 1e6;
    for (int i : {0, 7}) o.pose.covariance[i]= 0.01;
    for (int i : {14, 21, 28}) o.pose.covariance[i]= big;
    o.pose.covariance[35]= 0.05;
    for (int i : {0}) o.twist.covariance[i]= 0.005;
    for (int i : {7, 14, 21, 28}) o.twist.covariance[i]= big;
    o.twist.covariance[35]= 0.03;
    pub_->publish(o);

    if (publish_tf_) {
      geometry_msgs::msg::TransformStamped t;
      t.header = o.header;
      t.child_frame_id = base_frame_;
      t.transform.translation.x = x_;
      t.transform.translation.y = y_;
      t.transform.rotation = o.pose.pose.orientation;
      tf_->sendTransform(t);
    }
  }

  double r_, r_left_scale_, r_right_scale_, track_, cpr_;
  bool publish_tf_;
  std::string odom_frame_, base_frame_;
  bool init_ = false;
  int32_t lastL_ = 0, lastR_ = 0;
  rclcpp::Time last_;
  double x_ = 0, y_ = 0, th_ = 0;
  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub_;
  rclcpp::Subscription<std_msgs::msg::Int32MultiArray>::SharedPtr sub_;
  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_;
};

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

#!/usr/bin/env python3
"""umbmark.py — Borenstein & Feng (1996) UMBmark testi: düzeltme katsayıları.

Robotu 4 m'lik kare yerine kendi evine sığan L kenarla sür (örn. 1 m):
5 kez saat yönünde (CW), 5 kez tersine (CCW). Her turun sonunda başlangıç
noktasına göre (x, y) hatasını metreyle ölç ve aşağıya yaz. x = robotun ilk
gittiği yön, y = sol.

Formülleri makaleyle kontrol et: "Measurement and Correction of Systematic
Odometry Errors in Mobile Robots", IEEE T-RA 1996.
"""
import math

L = 1.0          # kare kenarı (m)
B = 0.175        # tekerlek merkezleri arası, kodda kullandığın değer (m)

cw = [(0.032, -0.041), (0.028, -0.036), (0.035, -0.044), (0.030, -0.039), (0.033, -0.040)]
ccw = [(-0.051, 0.012), (-0.048, 0.015), (-0.055, 0.010), (-0.049, 0.013), (-0.052, 0.011)]


def centroid(pts):
    return sum(p[0] for p in pts) / len(pts), sum(p[1] for p in pts) / len(pts)


xcw, ycw = centroid(cw)
xccw, yccw = centroid(ccw)

alpha = (xcw + xccw) / (-4 * L)          # rad — aks mesafesi hatasından
beta = (xcw - xccw) / (-4 * L)           # rad — tekerlek çapı farkından
R = (L / 2) / math.sin(beta / 2) if abs(beta) > 1e-9 else math.inf
Ed = (R + B / 2) / (R - B / 2) if math.isfinite(R) else 1.0
Eb = (math.pi / 2) / (math.pi / 2 - alpha)

left_scale = 2 / (Ed + 1)                # D_L = left_scale · D
right_scale = 2 / (1 / Ed + 1)           # D_R = right_scale · D
b_actual = Eb * B

print(f"alpha = {math.degrees(alpha):+.2f}°, beta = {math.degrees(beta):+.2f}°")
print(f"Ed (sağ/sol çap oranı) = {Ed:.4f}, Eb = {Eb:.4f}")
print("wheel_odometry parametreleri:")
print(f"  left_radius_scale:  {left_scale:.4f}")
print(f"  right_radius_scale: {right_scale:.4f}")
print(f"  wheel_separation:   {b_actual:.4f}")