Untitled

from jetbot import Robot
import rclpy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from rclpy.qos import QoSProfile

import time

rclpy.init()
node = rclpy.create_node("jet_node")
robot = Robot()

def cmd_vel(vel:Twist):
    R = 0.07
    w = vel.angular.z*R
    v = vel.linear.x
    l_v = v - w
    r_v = v + w
    robot.set_motors(l_v,r_v)

Kp = 3
Kd = 160
Ki = 0.1

prev_l = 0
arr_of_div = []
Ti = 20

AIM = 0.3

def scan_cb(msg):
    global prev_l
    vel = Twist()
    vel.linear.x = 0.5
    l_to_wall = min(msg.ranges)
    div = l_to_wall - AIM
    arr_of_div.append(div)
    if len(arr_of_div) > Ti:
        arr_of_div.pop(0)
    vel.angular.z = Kp*(div) + Kd*(l_to_wall - prev_l) + Ki*sum(arr_of_div)
    prev_l = l_to_wall
    cmd_vel(vel)


sub = node.create_subscription(LaserScan, "/scan", scan_cb, QoSProfile(depth=10))
pub = node.create_publisher(Twist, "/cmd_vel", 10)

rclpy.spin(node)