|
|
@@ -11,15 +11,79 @@
|
|
|
"""
|
|
|
import os
|
|
|
import rospy
|
|
|
+import time
|
|
|
import roslib; roslib.load_manifest('ros_arduino_python')
|
|
|
import dynamic_reconfigure.client
|
|
|
+import queue
|
|
|
|
|
|
from math import sin, cos, pi, sqrt
|
|
|
+import numpy as np
|
|
|
from geometry_msgs.msg import Quaternion, Twist
|
|
|
from nav_msgs.msg import Odometry
|
|
|
from tf.broadcaster import TransformBroadcaster
|
|
|
from std_msgs.msg import Int32
|
|
|
-
|
|
|
+from rasp_imu_hat_6dof.srv import GetYawData
|
|
|
+
|
|
|
+class SimplePID:
|
|
|
+ '''very simple discrete PID controller'''
|
|
|
+ def __init__(self, target, P, I, D):
|
|
|
+ '''Create a discrete PID controller
|
|
|
+ each of the parameters may be a vector if they have the same length
|
|
|
+
|
|
|
+ Args:
|
|
|
+ target (double) -- the target value(s)
|
|
|
+ P, I, D (double)-- the PID parameter
|
|
|
+
|
|
|
+ '''
|
|
|
+
|
|
|
+ # check if parameter shapes are compatabile.
|
|
|
+ self.Kp =np.array(P)
|
|
|
+ self.Ki =np.array(I)
|
|
|
+ self.Kd =np.array(D)
|
|
|
+ self.setPoint =np.array(target)
|
|
|
+
|
|
|
+ self.last_error=0
|
|
|
+ self.integrator = 0
|
|
|
+ self.integrator_max = float('inf')
|
|
|
+ self.timeOfLastCall = None
|
|
|
+
|
|
|
+
|
|
|
+ def update(self, current_value):
|
|
|
+ '''Updates the PID controller.
|
|
|
+
|
|
|
+ Args:
|
|
|
+ current_value (double): vector/number of same legth as the target given in the constructor
|
|
|
+
|
|
|
+ Returns:
|
|
|
+ controll signal (double): vector of same length as the target
|
|
|
+
|
|
|
+ '''
|
|
|
+ current_value=np.array(current_value)
|
|
|
+ if(self.timeOfLastCall is None):
|
|
|
+ # the PID was called for the first time. we don't know the deltaT yet
|
|
|
+ # no controll signal is applied
|
|
|
+ self.timeOfLastCall = time.clock()
|
|
|
+ return np.zeros(np.size(current_value))
|
|
|
+
|
|
|
+
|
|
|
+ error = self.setPoint - current_value
|
|
|
+ P = error
|
|
|
+
|
|
|
+ currentTime = time.clock()
|
|
|
+ deltaT = (currentTime-self.timeOfLastCall)
|
|
|
+
|
|
|
+ # integral of the error is current error * time since last update
|
|
|
+ self.integrator = self.integrator + (error*deltaT)
|
|
|
+ I = self.integrator
|
|
|
+
|
|
|
+ # derivative is difference in error / time since last update
|
|
|
+ D = (error-self.last_error)/deltaT
|
|
|
+
|
|
|
+ self.last_error = error
|
|
|
+ self.timeOfLastCall = currentTime
|
|
|
+
|
|
|
+ # return controll signal
|
|
|
+ return self.Kp*P + self.Ki*I + self.Kd*D
|
|
|
|
|
|
""" Class to receive Twist commands and publish wheel Odometry data """
|
|
|
class BaseController:
|
|
|
@@ -34,6 +98,19 @@ class BaseController:
|
|
|
self.rate = float(rospy.get_param("~base_controller_rate", 20))
|
|
|
self.timeout = rospy.get_param("~base_controller_timeout", 0.7)
|
|
|
|
|
|
+ self.yaw_data_service = rospy.get_param("~yaw_data_service","imu_node/GetYawData")
|
|
|
+ self.yaw_target = rospy.get_param("~yaw_target",0)
|
|
|
+ self.yaw_kp = rospy.get_param("~yaw_kp",1)
|
|
|
+ self.yaw_ki = rospy.get_param("~yaw_ki",1)
|
|
|
+ self.yaw_kd = rospy.get_param("~yaw_kd",1)
|
|
|
+ self.correction_forward_err_yaw_data = rospy.get_param("~correction_forward_err_yaw_data",0.017)
|
|
|
+ self.correction_forward_rate = rospy.get_param("~correction_forward_rate",1)
|
|
|
+ self.pid_controller = SimplePID(self.yaw_target, self.yaw_kp, self.yaw_ki, self.yaw_kd)
|
|
|
+ self.correction_yaw_flag = 0
|
|
|
+ self.last_yaw_data = 0
|
|
|
+ self.now_yaw_data = 0
|
|
|
+ self.initial_odom_yaw_data = self.get_yaw()
|
|
|
+
|
|
|
self.debugPID = rospy.get_param('~debugPID', False)
|
|
|
|
|
|
pid_params = dict()
|
|
|
@@ -243,6 +320,9 @@ class BaseController:
|
|
|
self.pose_y += delta_y
|
|
|
self.th += delta_th
|
|
|
|
|
|
+ self.now_odom_yaw_data = self.get_yaw()
|
|
|
+ self.th = self.now_odom_yaw_data - self.initial_odom_yaw_data
|
|
|
+
|
|
|
quaternion = Quaternion()
|
|
|
quaternion.x = 0.0
|
|
|
quaternion.y = 0.0
|
|
|
@@ -289,6 +369,7 @@ class BaseController:
|
|
|
# send motor cmd
|
|
|
if time_now > (self.last_cmd_time + rospy.Duration(self.timeout)):
|
|
|
self.stop()
|
|
|
+ self.correction_yaw_flag = 0
|
|
|
else:
|
|
|
self.send_motor_speed()
|
|
|
|
|
|
@@ -373,8 +454,17 @@ class BaseController:
|
|
|
self.v_des_BWheel = 0
|
|
|
self.v_des_CWheel = 0
|
|
|
|
|
|
+ def get_yaw(self):
|
|
|
+ rospy.wait_for_service(self.yaw_data_service)
|
|
|
+ try:
|
|
|
+ yaw_data = rospy.ServiceProxy(self.yaw_data_service, GetYawData)
|
|
|
+ resp = yaw_data()
|
|
|
+ return resp.yaw
|
|
|
+ except rospy.ServiceException, e:
|
|
|
+ print "Service call failed: %s"%e
|
|
|
|
|
|
def cmdVelCallback(self, req):
|
|
|
+
|
|
|
# Handle velocity-based movement requests
|
|
|
self.last_cmd_time = rospy.Time.now()
|
|
|
|
|
|
@@ -382,6 +472,21 @@ class BaseController:
|
|
|
y = req.linear.y # m/s
|
|
|
th = req.angular.z # rad/s
|
|
|
|
|
|
+ if x != 0 and y == 0 and th == 0:
|
|
|
+ if self.correction_yaw_flag < 2:
|
|
|
+ self.stop()
|
|
|
+ self.last_yaw_data = self.get_yaw()
|
|
|
+ self.correction_yaw_flag += 1
|
|
|
+ return
|
|
|
+ else:
|
|
|
+ self.now_yaw_data = self.get_yaw()
|
|
|
+ err_yaw_data = self.now_yaw_data - self.last_yaw_data
|
|
|
+ if abs(err_yaw_data) > self.correction_forward_err_yaw_data:
|
|
|
+ th = self.pid_controller.update(err_yaw_data)
|
|
|
+ print("err_yaw_data is ",err_yaw_data,"th is ",th)
|
|
|
+ else:
|
|
|
+ self.correction_yaw_flag = 0
|
|
|
+
|
|
|
#Inverse Kinematic
|
|
|
tmpX = sqrt(3)/2.0
|
|
|
tmpY = 0.5 # 1/2
|
