#!/usr/bin/env python #importing necessary classes import rospy from nav_msgs.msg import Odometry from geometry_msgs.msg import TransformStamped import serial import time import math from scipy.interpolate import interp1d #instantiate both dynamic and constant variables roll = pitch = yaw = 0.0 Pi = math.pi radToDegMult = 180/Pi xTransformCar = 0.0 yTransformCar = 0.0 xTransformBall = 0.0 yTransformBall = 0.0 angleRelativeDeg = 0.0 speed = 0 angleRangeConst = 3.2 mapConst = 1.2 dt = 0 lowBound = 15 highBound = 60 newLine = "/n" #initialize serial communication with microcontroller arduino = serial.Serial(port='/dev/ttyUSB0', baudrate=115200, timeout=.1) #instantiate publishers for both "car" (mobile robot) and ball carPub = rospy.Publisher('car_transform_data', TransformStamped, queue_size = 10) ballPub = rospy.Publisher('ball_transform_data', TransformStamped, queue_size = 10) #PID class definition class PID: def __init__(self, Kp, Ki, Kd): self.Kp = Kp self.Ki = Ki self.Kd = Kd self.last_error = 0 self.integral = 0 def update(self, error, dt): derivative = (error - self.last_error) / dt self.integral += error * dt output = self.Kp * error + self.Ki * self.integral + self.Kd * derivative self.last_error = error return output #Function definitions #Motion capture angle output is in quaternions, translated to euler angles for ease of use #Ensure understanding of gimbal lock if you desire to change this conversion def euler_from_quaternion(x, y, z, w): t0 = +2.0 * (w * x + y * z) t1 = +1.0 - 2.0 * (x * x + y * y) roll_x = math.atan2(t0, t1) t2 = +2.0 * (w * y - z * x) t2 = +1.0 if t2 > +1.0 else t2 t2 = -1.0 if t2 < -1.0 else t2 pitch_y = math.asin(t2) t3 = +2.0 * (w * z + x * y) t4 = +1.0 - 2.0 * (y * y + z * z) yaw_z = math.atan2(t3, t4) return roll_x, pitch_y, yaw_z # in radians #project a range of values onto another for ease of use in serial communication def mapValue(value, leftMin, leftMax, rightMin, rightMax): # Figure out how 'wide' each range is leftSpan = leftMax - leftMin rightSpan = rightMax - rightMin # Convert the left range into a 0-1 range (float) valueScaled = float(value - leftMin) / float(leftSpan) # Convert the 0-1 range into a value in the right range. return rightMin + (valueScaled * rightSpan) #serial communication function def write_read(x): input = str(x) arduino.write(bytes(input, 'utf-8')) #time.sleep(0.005) data = arduino.readline() return data #callback instantiation for publishers def carCb (msg): global roll, pitch, yaw new_msg = TransformStamped() orientation_q = msg.transform.rotation orientation_list = [orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w] (roll, pitch, yaw) = euler_from_quaternion(orientation_list[0], orientation_list[1],orientation_list[2],orientation_list[3]) global xTransformCar xTransformCar = msg.transform.translation.x * 1000 new_msg.transform.translation.x = xTransformCar global yTransformCar yTransformCar = msg.transform.translation.y * -1000 new_msg.transform.translation.y = yTransformCar new_msg.transform.translation.z = msg.transform.translation.z * 1000 global angleRelativeDeg roll = roll * radToDegMult pitch = pitch * radToDegMult yaw = yaw * radToDegMult new_msg.transform.rotation.x = roll new_msg.transform.rotation.y = pitch new_msg.transform.rotation.z = yaw new_msg.transform.rotation.w = angleRelativeDeg carPub.publish(new_msg) def ballCb (msg): new_msg = TransformStamped() global xTransformBall xTransformBall = msg.transform.translation.x * 1000 new_msg.transform.translation.x = xTransformBall global yTransformBall yTransformBall = msg.transform.translation.y * -1000 new_msg.transform.translation.y = yTransformBall new_msg.transform.translation.z = msg.transform.translation.z * 1000 ballPub.publish(new_msg) #begin script rospy.init_node('continuous_change') #instance of PID class pid = PID(Kp = 1.0, Ki = 0.05, Kd = 0.25) #main loop while not rospy.is_shutdown(): #time variable used to measure change in time between each loop instance for PID control startTime = time.time() #collect and store data from ROS subscription into variables carSub = rospy.Subscriber ('/car1_vrpn/estimated_transform', TransformStamped, carCb) ballSub = rospy.Subscriber ('/ball_vrpn/estimated_transform', TransformStamped, ballCb) xDelta = yTransformBall - yTransformCar yDelta = xTransformBall - xTransformCar distance = math.sqrt(pow(xDelta, 2) + pow(yDelta, 2)) #obtain and translate bearing angle from radians to degrees for ease of use angleRelativeRad = math.atan2(yDelta, xDelta) angleRelativeDeg = angleRelativeRad * radToDegMult #print statements for troubleshooting print("yaw: " + str(yaw)) print("angle: " + str(angleRelativeDeg)) print("distance: " + str(distance)) #obtain difference in current car angle to bearing angle yawPos = yaw >= 0 relAnglePos = angleRelativeDeg >= 0 angleErr = (angleRelativeDeg) - (yaw) overShoot = abs(angleErr) >180 print("angle diff: " + str(angleErr)) #PID speed control dt = time.time() - startTime control_signal = pid.update(angleErr, dt) speed += control_signal * dt #logic defining value mapping to ensure proper translation to serial as seen in previous tutorial #communicates speed value(s) or action value depending on if car angle matches bearing angle if(yaw > angleRelativeDeg + 1): print("Greater") if(not overShoot): speed = mapValue(speed, 0, 180, highBound, lowBound)/mapConst else: speed = -mapValue(speed, 180, 360, lowBound, highBound)/mapConst value = write_read(int(speed)) print(value) elif(yaw < angleRelativeDeg - 1): print("Lesser") if(not overShoot): speed = -mapValue(abs(speed), 0, 180, lowBound, highBound)/mapConst else: speed = mapValue(abs(speed), 180, 360, lowBound, highBound)/mapConst value = write_read(int(speed)) print(value) elif(angleRelativeDeg -angleRangeConst < yaw < angleRelativeDeg + angleRangeConst): print("In Range") if(distance > 150): value = write_read(200) time.sleep(0.4) print(value) else: value = write_read(300) print(value) time.sleep(3) print("Speed PID: " + str(speed)) r = rospy.Rate(100) r.sleep()